crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Some mixed performance improvements.

Browse Source
This commit is contained in:
Daniel
2024-09-13 16:56:53 +02:00
parent f7a7f69f37
commit 3350e2c4af
3 changed files with 745 additions and 642 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
spt/triangulator.cpp
View File

@ -170,6 +170,14 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
auto it = std::find(joint_names.begin(), joint_names.end(), joint);
core_joint_idx.push_back(std::distance(joint_names.begin(), it));
}
std::vector<std::array<size_t, 2>> core_limbs_idx;
for (const auto &limb : core_limbs)
{
auto it1 = std::find(core_joints.begin(), core_joints.end(), limb.first);
auto it2 = std::find(core_joints.begin(), core_joints.end(), limb.second);
core_limbs_idx.push_back({(size_t)std::distance(core_joints.begin(), it1),
(size_t)std::distance(core_joints.begin(), it2)});
}
// Undistort 2D poses
for (size_t i = 0; i < cameras.size(); ++i)
@ -236,7 +244,8 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
// Calculate score
double score = calc_pose_score(pose_core, last_pose, dists[j], internal_cameras[i]);
double score = calc_pose_score(
pose_core, last_pose, dists[j], internal_cameras[i]);
if (score > threshold)
{
scored_pasts[i][j].push_back(k);
@ -325,7 +334,8 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
// Triangulate and score
auto [pose3d, score] = triangulate_and_score(pose1_core, pose2_core, cam1, cam2, roomparams);
auto [pose3d, score] = triangulate_and_score(
pose1_core, pose2_core, cam1, cam2, roomparams, core_limbs_idx);
all_scored_poses.emplace_back(pose3d, score);
}
@ -362,7 +372,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
const auto &pose1 = poses_2d_mats[std::get<0>(pids)][std::get<2>(pids)];
const auto &pose2 = poses_2d_mats[std::get<1>(pids)][std::get<3>(pids)];
auto [pose3d, score] = triangulate_and_score(pose1, pose2, cam1, cam2, roomparams);
auto [pose3d, score] = triangulate_and_score(pose1, pose2, cam1, cam2, roomparams, {});
all_full_poses.push_back(pose3d);
}
@ -451,13 +461,7 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
int num_joints = poses[p].rows;
// Extract the (x, y) coordinates
std::vector<int> dims = {num_joints, 2};
cv::Mat points = cv::Mat(dims, CV_64F);
for (int j = 0; j < num_joints; ++j)
{
points.at<double>(j, 0) = poses[p].at<double>(j, 0);
points.at<double>(j, 1) = poses[p].at<double>(j, 1);
}
cv::Mat points = poses[p].colRange(0, 2).clone();
// Undistort the points
cv::Mat newK = undistort_points(points, icam);
@ -508,14 +512,7 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
size_t num_joints = body3D.size[0];
// Split up vector
std::vector<int> dimsA = {(int)num_joints, 3};
cv::Mat points3d = cv::Mat(dimsA, CV_64F);
for (size_t i = 0; i < num_joints; ++i)
{
points3d.at<double>(i, 0) = body3D.at<double>(i, 0);
points3d.at<double>(i, 1) = body3D.at<double>(i, 1);
points3d.at<double>(i, 2) = body3D.at<double>(i, 2);
}
cv::Mat points3d = body3D.colRange(0, 3).clone();
// Project from world to camera coordinate system
cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints);
@ -609,15 +606,36 @@ double TriangulatorInternal::calc_pose_score(
double iscale = (icam.cam.width + icam.cam.height) / 2;
cv::Mat scores = score_projection(pose1, pose2, dist1, mask, iscale);
double score = 0.0;
// Drop lowest scores
size_t drop_k = static_cast<size_t>(pose1.rows * 0.2);
size_t min_k = 3;
std::vector<double> scores_vec;
for (int i = 0; i < scores.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
{
score += scores.at<double>(i);
scores_vec.push_back(scores.at<double>(i));
}
}
score /= valid_count;
std::sort(scores_vec.begin(), scores_vec.end());
drop_k = (scores_vec.size() >= min_k) ? std::min(drop_k, scores_vec.size() - min_k) : 0;
if (drop_k > 0)
{
scores_vec.erase(scores_vec.begin(), scores_vec.begin() + drop_k);
}
// Calculate final score
double score = 0.0;
size_t items = 0;
for (size_t i = 0; i < scores_vec.size(); i++)
{
score += scores_vec[i];
items++;
}
if (items > 0)
{
score /= (double)items;
}
return score;
}
@ -635,8 +653,7 @@ cv::Mat TriangulatorInternal::score_projection(
// Calculate error
cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2);
cv::Mat error1;
error1 = diff.mul(diff);
cv::Mat error1 = diff.mul(diff);
cv::reduce(error1, error1, 1, cv::REDUCE_SUM, CV_64F);
cv::sqrt(error1, error1);
error1.setTo(0.0, ~mask);
@ -665,7 +682,8 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
const cv::Mat &pose2,
const CameraInternal &cam1,
const CameraInternal &cam2,
const std::array<std::array<double, 3>, 2> &roomparams)
const std::array<std::array<double, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx)
{
// Mask out invisible joints
double min_score = 0.1;
@ -720,10 +738,8 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
}
}
// Check if points are inside the room bounds
// Check if mean of the points is inside the room bounds
std::array<double, 3> mean = {0, 0, 0};
std::array<double, 3> mins = {std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), std::numeric_limits<double>::max()};
std::array<double, 3> maxs = {std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest()};
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
@ -731,8 +747,6 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
for (int j = 0; j < 3; ++j)
{
mean[j] += pose3d.at<double>(i, j);
mins[j] = std::min(mins[j], pose3d.at<double>(i, j));
maxs[j] = std::max(maxs[j], pose3d.at<double>(i, j));
}
}
}
@ -740,15 +754,12 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
{
mean[j] /= num_visible;
}
double wdist = 0.1;
std::array<double, 3> center = roomparams[0];
std::array<double, 3> size = roomparams[1];
for (int i = 0; i < 3; ++i)
{
if (mean[i] > center[i] + size[i] / 2 ||
mean[i] < center[i] - size[i] / 2 ||
maxs[i] > center[i] + size[i] / 2 + wdist ||
mins[i] < center[i] - size[i] / 2 - wdist)
mean[i] < center[i] - size[i] / 2)
{
// Very low score if outside room
for (int j = 0; j < pose1.rows; ++j)
@ -786,22 +797,77 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
}
}
// Drop lowest scores
int drop_k = static_cast<int>(pose1.rows * 0.2);
std::vector<double> scores_vec;
// Set scores outside the room to a low value
double wdist = 0.1;
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
{
scores_vec.push_back(scores.at<double>(i));
for (int j = 0; j < 3; ++j)
{
if (pose3d.at<double>(i, j) > center[j] + size[j] / 2 + wdist ||
pose3d.at<double>(i, j) < center[j] - size[j] / 2 - wdist)
{
pose3d.at<double>(i, 3) = 0.001;
}
}
}
}
// Filter clearly wrong limbs
if (core_limbs_idx.size() > 0)
{
double max_length = 0.9;
std::vector<size_t> invalid_joints;
for (size_t i = 0; i < core_limbs_idx.size(); ++i)
{
auto limb = core_limbs_idx[i];
if (pose3d.at<double>(limb[0], 3) > min_score &&
pose3d.at<double>(limb[1], 3) > min_score)
{
cv::Point3d p1 = cv::Point3d(
pose3d.at<double>(limb[0], 0),
pose3d.at<double>(limb[0], 1),
pose3d.at<double>(limb[0], 2));
cv::Point3d p2 = cv::Point3d(
pose3d.at<double>(limb[1], 0),
pose3d.at<double>(limb[1], 1),
pose3d.at<double>(limb[1], 2));
double length = cv::norm(p1 - p2);
if (length > max_length)
{
invalid_joints.push_back(limb[1]);
}
}
}
for (size_t i = 0; i < invalid_joints.size(); ++i)
{
pose3d.at<double>(invalid_joints[i], 3) = 0.001;
}
}
// Drop lowest scores
size_t drop_k = static_cast<size_t>(pose1.rows * 0.2);
size_t min_k = 3;
std::vector<double> scores_vec;
for (int i = 0; i < pose1.rows; ++i)
{
if (pose3d.at<double>(i, 3) > min_score)
{
scores_vec.push_back(pose3d.at<double>(i, 3));
}
}
std::sort(scores_vec.begin(), scores_vec.end());
drop_k = (scores_vec.size() >= min_k) ? std::min(drop_k, scores_vec.size() - min_k) : 0;
if (drop_k > 0)
{
scores_vec.erase(scores_vec.begin(), scores_vec.begin() + drop_k);
}
// Calculate final score
double score = 0.0;
size_t items = 0;
for (size_t i = drop_k; i < scores_vec.size(); i++)
for (size_t i = 0; i < scores_vec.size(); i++)
{
score += scores_vec[i];
items++;
@ -823,13 +889,13 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
{
double max_center_dist = 0.6;
double max_joint_avg_dist = 0.3;
size_t num_joints = all_scored_poses[0].first.rows;
// Calculate pose centers
std::vector<cv::Point3d> centers;
for (size_t i = 0; i < all_pairs.size(); ++i)
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows;
cv::Point3d center(0, 0, 0);
size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j)
@ -857,6 +923,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
for (size_t i = 0; i < all_pairs.size(); ++i)
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows;
const cv::Point3d &center = centers[i];
double best_dist = std::numeric_limits<double>::infinity();
int best_group = -1;
@ -874,10 +941,17 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
std::vector<double> dists;
for (size_t row = 0; row < num_joints; row++)
{
if (pose_3d.at<double>(row, 3) > min_score && group_pose.at<double>(row, 3) > min_score)
if (pose_3d.at<double>(row, 3) > min_score &&
group_pose.at<double>(row, 3) > min_score)
{
cv::Point3d p1(pose_3d.at<double>(row, 0), pose_3d.at<double>(row, 1), pose_3d.at<double>(row, 2));
cv::Point3d p2(group_pose.at<double>(row, 0), group_pose.at<double>(row, 1), group_pose.at<double>(row, 2));
cv::Point3d p1 = cv::Point3d(
pose_3d.at<double>(row, 0),
pose_3d.at<double>(row, 1),
pose_3d.at<double>(row, 2));
cv::Point3d p2 = cv::Point3d(
group_pose.at<double>(row, 0),
group_pose.at<double>(row, 1),
group_pose.at<double>(row, 2));
dists.push_back(cv::norm(p1 - p2));
}
}
@ -917,15 +991,24 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
for (size_t row = 0; row < num_joints; row++)
{
new_pose.at<double>(row, 0) = (group_pose.at<double>(row, 0) * n_elems + pose_3d.at<double>(row, 0)) / (n_elems + 1);
new_pose.at<double>(row, 1) = (group_pose.at<double>(row, 1) * n_elems + pose_3d.at<double>(row, 1)) / (n_elems + 1);
new_pose.at<double>(row, 2) = (group_pose.at<double>(row, 2) * n_elems + pose_3d.at<double>(row, 2)) / (n_elems + 1);
new_pose.at<double>(row, 3) = (group_pose.at<double>(row, 3) * n_elems + pose_3d.at<double>(row, 3)) / (n_elems + 1);
new_pose.at<double>(row, 0) =
(group_pose.at<double>(row, 0) * n_elems + pose_3d.at<double>(row, 0)) /
(n_elems + 1);
new_pose.at<double>(row, 1) =
(group_pose.at<double>(row, 1) * n_elems + pose_3d.at<double>(row, 1)) /
(n_elems + 1);
new_pose.at<double>(row, 2) =
(group_pose.at<double>(row, 2) * n_elems + pose_3d.at<double>(row, 2)) /
(n_elems + 1);
new_pose.at<double>(row, 3) =
(group_pose.at<double>(row, 3) * n_elems + pose_3d.at<double>(row, 3)) /
(n_elems + 1);
}
group_pose = new_pose;
group_indices.push_back(i);
}
}
return groups;
}
@ -970,7 +1053,10 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
if (initial_pose_3d.at<double>(j, 3) > 0.0)
{
jmask.at<uchar>(j) = 1;
center += cv::Point3d(initial_pose_3d.at<double>(j, 0), initial_pose_3d.at<double>(j, 1), initial_pose_3d.at<double>(j, 2));
center += cv::Point3d(
initial_pose_3d.at<double>(j, 0),
initial_pose_3d.at<double>(j, 1),
initial_pose_3d.at<double>(j, 2));
valid_joints++;
}
}
@ -997,8 +1083,14 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
{
for (int j = 0; j < num_joints; ++j)
{
cv::Point3d joint_i(poses_3d[i].at<double>(j, 0), poses_3d[i].at<double>(j, 1), poses_3d[i].at<double>(j, 2));
cv::Point3d joint_initial(initial_pose_3d.at<double>(j, 0), initial_pose_3d.at<double>(j, 1), initial_pose_3d.at<double>(j, 2));
cv::Point3d joint_i = cv::Point3d(
poses_3d[i].at<double>(j, 0),
poses_3d[i].at<double>(j, 1),
poses_3d[i].at<double>(j, 2));
cv::Point3d joint_initial = cv::Point3d(
initial_pose_3d.at<double>(j, 0),
initial_pose_3d.at<double>(j, 1),
initial_pose_3d.at<double>(j, 2));
double distance = cv::norm(joint_i - joint_initial);
distances.at<double>(i, j) = distance;
if (distance <= max_dist && poses_3d[i].at<double>(j, 3) > (min_score - offset))

13
spt/triangulator.hpp
View File

@ -57,6 +57,16 @@ private:
"ankle_left",
"ankle_right",
};
const std::vector<std::pair<std::string, std::string>> core_limbs = {
{"knee_left", "ankle_left"},
{"hip_left", "knee_left"},
{"hip_right", "knee_right"},
{"knee_right", "ankle_right"},
{"elbow_left", "wrist_left"},
{"elbow_right", "wrist_right"},
{"shoulder_left", "elbow_left"},
{"shoulder_right", "elbow_right"},
};
std::vector<cv::Mat> last_poses_3d;
@ -84,7 +94,8 @@ private:
const cv::Mat &pose2,
const CameraInternal &cam1,
const CameraInternal &cam2,
const std::array<std::array<double, 3>, 2> &roomparams);
const std::array<std::array<double, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> calc_grouping(
const std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,