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

Improving triangulation speed.

Browse Source
This commit is contained in:
Daniel
2024-09-16 17:57:46 +02:00
parent 3bf6e6e639
commit 57cf91ae95
3 changed files with 279 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
media/RESULTS.md
View File

@ -278,9 +278,9 @@ Results of the model in various experiments on different datasets.
(duration 00:00:56)
```json
{
"avg_time_2d": 0.10053871915102824,
"avg_time_3d": 0.0020945760392651115,
"avg_fps": 9.743426810431163
"avg_time_2d": 0.101639888540576,
"avg_time_3d": 0.0018720938168030833,
"avg_fps": 9.660717312392508
}
{
"person_nums": {

481
spt/triangulator.cpp
View File

@ -149,16 +149,19 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
std::vector<int> dims = {(int)num_joints, 3};
cv::Mat pose_mat = cv::Mat(dims, CV_64F);
// Use pointer to copy data efficiently
double *mat_ptr = pose_mat.ptr<double>(0);
for (size_t k = 0; k < num_joints; ++k)
{
for (size_t l = 0; l < 3; ++l)
{
pose_mat.at<double>(k, l) = poses_2d[i][j][k][l];
}
const auto &joint = poses_2d[i][j][k];
mat_ptr[k * 3 + 0] = joint[0];
mat_ptr[k * 3 + 1] = joint[1];
mat_ptr[k * 3 + 2] = joint[2];
}
camera_poses.push_back(pose_mat);
camera_poses.push_back(std::move(pose_mat));
}
poses_2d_mats.push_back(camera_poses);
poses_2d_mats.push_back(std::move(camera_poses));
}
std::vector<CameraInternal> internal_cameras;
for (size_t i = 0; i < cameras.size(); ++i)
@ -194,9 +197,11 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
// Select core joints
std::vector<cv::Mat> last_core_poses;
last_core_poses.resize(last_poses_3d.size());
#pragma omp parallel for
for (size_t i = 0; i < last_poses_3d.size(); ++i)
{
cv::Mat pose = last_poses_3d[i];
cv::Mat &pose = last_poses_3d[i];
std::vector<int> dims = {(int)core_joint_idx.size(), 4};
cv::Mat last_poses_core(dims, pose.type());
@ -204,7 +209,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
pose.row(core_joint_idx[j]).copyTo(last_poses_core.row(j));
}
last_core_poses.push_back(last_poses_core);
last_core_poses[i] = last_poses_core;
}
// Project
@ -223,50 +228,81 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
if (!last_poses_3d.empty())
{
// Calculate index pairs and initialize vectors
std::vector<std::array<size_t, 3>> indices;
std::vector<std::array<size_t, 3>> indices_ijk;
for (size_t i = 0; i < cameras.size(); ++i)
{
size_t num_persons = std::get<0>(last_poses_2d[i]).size();
size_t num_last_persons = std::get<0>(last_poses_2d[i]).size();
scored_pasts[i] = std::map<size_t, std::vector<size_t>>();
for (size_t j = 0; j < num_persons; ++j)
for (size_t j = 0; j < num_last_persons; ++j)
{
size_t num_new_persons = poses_2d_mats[i].size();
scored_pasts[i][j] = std::vector<size_t>();
for (size_t k = 0; k < poses_2d_mats[i].size(); ++k)
for (size_t k = 0; k < num_new_persons; ++k)
{
indices.push_back({i, j, k});
indices_ijk.push_back({i, j, k});
}
}
}
#pragma omp parallel for ordered schedule(dynamic)
for (size_t e = 0; e < indices.size(); ++e)
std::vector<std::array<size_t, 2>> indices_ik;
for (size_t i = 0; i < cameras.size(); ++i)
{
const auto &i = indices[e][0];
const auto &j = indices[e][1];
const auto &k = indices[e][2];
size_t num_new_persons = poses_2d_mats[i].size();
for (size_t k = 0; k < num_new_persons; ++k)
{
indices_ik.push_back({i, k});
}
}
const std::vector<cv::Mat> &poses = poses_2d_mats[i];
const std::vector<cv::Mat> &last_poses = std::get<0>(last_poses_2d[i]);
const std::vector<cv::Mat> &dists = std::get<1>(last_poses_2d[i]);
// Precalculate core poses
std::vector<cv::Mat> poses_2d_mats_core_list;
poses_2d_mats_core_list.resize(indices_ik.size());
std::vector<std::vector<size_t>> mats_core_map;
mats_core_map.resize(cameras.size());
for (size_t i = 0; i < cameras.size(); ++i)
{
size_t num_new_persons = poses_2d_mats[i].size();
for (size_t k = 0; k < num_new_persons; ++k)
{
mats_core_map[i].push_back(0);
}
}
#pragma omp parallel for
for (size_t e = 0; e < indices_ik.size(); ++e)
{
const auto [i, k] = indices_ik[e];
// Select core joints
const cv::Mat &last_pose = last_poses[j];
const cv::Mat &pose = poses[k];
const cv::Mat &pose = poses_2d_mats[i][k];
std::vector<int> dims = {(int)core_joint_idx.size(), 3};
cv::Mat pose_core(dims, pose.type());
for (size_t l = 0; l < core_joint_idx.size(); ++l)
for (size_t j = 0; j < core_joint_idx.size(); ++j)
{
size_t idx = core_joint_idx[l];
pose.row(idx).copyTo(pose_core.row(l));
pose.row(core_joint_idx[j]).copyTo(pose_core.row(j));
}
// Calculate score
double score = calc_pose_score(pose_core, last_pose, dists[j], internal_cameras[i]);
poses_2d_mats_core_list[e] = pose_core;
mats_core_map[i][k] = e;
}
// Calculate matching score
#pragma omp parallel for
for (size_t e = 0; e < indices_ijk.size(); ++e)
{
const auto [i, j, k] = indices_ijk[e];
const cv::Mat &last_pose = std::get<0>(last_poses_2d[i])[j];
const cv::Mat &last_dist = std::get<1>(last_poses_2d[i])[j];
const cv::Mat &new_pose = poses_2d_mats_core_list[mats_core_map[i][k]];
double score = calc_pose_score(new_pose, last_pose, last_dist, internal_cameras[i]);
if (score > threshold)
{
#pragma omp ordered
scored_pasts[i][j].push_back(k);
#pragma omp critical
{
scored_pasts[i][j].push_back(k);
}
}
}
}
@ -274,10 +310,15 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
// Create pairs of persons
// Checks if the person was already matched to the last frame and if so only creates pairs
// with those, else it creates all possible pairs
std::vector<int> num_persons;
std::vector<int> num_persons_sum;
for (size_t i = 0; i < cameras.size(); ++i)
{
num_persons.push_back(poses_2d[i].size());
int nsum = poses_2d[i].size();
if (i > 0)
{
nsum += num_persons_sum[i - 1];
}
num_persons_sum.push_back(nsum);
}
std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> all_pairs;
std::vector<std::array<size_t, 4>> indices;
@ -299,8 +340,8 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
const auto [i, j, k, l] = indices[e];
int pid1 = std::accumulate(num_persons.begin(), num_persons.begin() + i, 0) + k;
int pid2 = std::accumulate(num_persons.begin(), num_persons.begin() + j, 0) + l;
int pid1 = num_persons_sum[i] + k;
int pid2 = num_persons_sum[k] + l;
bool match = false;
if (!last_poses_3d.empty())
@ -309,16 +350,19 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
auto &smi = scored_pasts[i][m];
auto &smj = scored_pasts[j][m];
if (std::find(smi.begin(), smi.end(), k) != smi.end() &&
std::find(smj.begin(), smj.end(), l) != smj.end())
bool in_smi = std::find(smi.begin(), smi.end(), k) != smi.end();
bool in_smj = std::find(smj.begin(), smj.end(), l) != smj.end();
if (in_smi && in_smj)
{
match = true;
#pragma omp ordered
all_pairs.emplace_back(
auto item = std::make_pair(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
#pragma omp ordered
all_pairs.push_back(item);
}
else if (std::find(smi.begin(), smi.end(), k) != smi.end() ||
std::find(smj.begin(), smj.end(), l) != smj.end())
else if (in_smi || in_smj)
{
match = true;
}
@ -327,9 +371,12 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
if (!match)
{
#pragma omp ordered
all_pairs.emplace_back(
auto item = std::make_pair(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
// Needed to prevent randomized grouping/merging with slightly different results
#pragma omp ordered
all_pairs.push_back(item);
}
}
@ -413,6 +460,8 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
const auto &group = groups[i];
std::vector<cv::Mat> poses;
poses.reserve(std::get<2>(group).size());
for (const auto &idx : std::get<2>(group))
{
poses.push_back(all_full_poses[idx]);
@ -421,6 +470,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
auto merged_pose = merge_group(poses, min_score);
all_merged_poses[i] = (merged_pose);
}
last_poses_3d = all_merged_poses;
// Convert to output format
std::vector<std::vector<std::array<double, 4>>> poses_3d;
@ -428,6 +478,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
for (size_t i = 0; i < all_merged_poses.size(); ++i)
{
const auto &mat = all_merged_poses[i];
const double *mat_ptr = mat.ptr<double>(0);
std::vector<std::array<double, 4>> pose;
size_t num_joints = mat.rows;
pose.reserve(num_joints);
@ -438,7 +489,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
std::array<double, 4> point;
for (size_t k = 0; k < 4; ++k)
{
point[k] = mat.at<double>(j, k);
point[k] = mat_ptr[j * 4 + k];
}
pose.push_back(point);
@ -454,7 +505,6 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
}
last_poses_3d = all_merged_poses;
return poses_3d;
}
@ -467,7 +517,7 @@ void TriangulatorInternal::reset()
// =================================================================================================
cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam)
void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam)
{
int width = icam.cam.width;
int height = icam.cam.height;
@ -476,55 +526,41 @@ cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &
cv::Mat newK = cv::getOptimalNewCameraMatrix(
icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height));
// Undistort points
cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
return newK;
}
// =================================================================================================
void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam)
{
for (size_t p = 0; p < poses.size(); ++p)
{
int num_joints = poses[p].rows;
// Extract the (x, y) coordinates
cv::Mat points = poses[p].colRange(0, 2).clone();
points = points.reshape(2);
// Undistort the points
cv::Mat newK = undistort_points(points, icam);
if (p == poses.size() - 1)
{
// Update the camera matrix as well
icam.K = newK;
icam.DC = cv::Mat::zeros(5, 1, CV_64F);
}
cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
// Update the original poses with the undistorted points
for (int j = 0; j < num_joints; ++j)
{
poses[p].at<double>(j, 0) = points.at<double>(j, 0);
poses[p].at<double>(j, 1) = points.at<double>(j, 1);
}
points = points.reshape(1);
points.copyTo(poses[p].colRange(0, 2));
// Mask out points that are far outside the image (points slightly outside are still valid)
double offset = (icam.cam.width + icam.cam.height) / 40.0;
double mask_offset = (width + height) / 40.0;
int num_joints = poses[p].rows;
double *poses_ptr = poses[p].ptr<double>(0);
for (int j = 0; j < num_joints; ++j)
{
double x = poses[p].at<double>(j, 0);
double y = poses[p].at<double>(j, 1);
bool in_x = x >= -offset && x < icam.cam.width + offset;
bool in_y = y >= -offset && y < icam.cam.height + offset;
double x = poses_ptr[j * 3 + 0];
double y = poses_ptr[j * 3 + 1];
bool in_x = x >= -mask_offset && x < width + mask_offset;
bool in_y = y >= -mask_offset && y < height + mask_offset;
if (!in_x || !in_y)
{
poses[p].at<double>(j, 0) = 0;
poses[p].at<double>(j, 1) = 0;
poses[p].at<double>(j, 2) = 0;
poses_ptr[j * 3 + 0] = 0;
poses_ptr[j * 3 + 1] = 0;
poses_ptr[j * 3 + 2] = 0;
}
}
}
// Update the camera matrix
icam.K = newK.clone();
icam.DC = cv::Mat::zeros(5, 1, CV_64F);
}
// =================================================================================================
@ -533,29 +569,31 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists)
{
size_t num_persons = bodies3D.size();
size_t num_joints = bodies3D[0].rows;
std::vector<cv::Mat> bodies2D_list(num_persons);
std::vector<cv::Mat> dists_list(num_persons);
cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints);
for (size_t i = 0; i < num_persons; ++i)
{
const cv::Mat &body3D = bodies3D[i];
size_t num_joints = body3D.size[0];
// Split up vector
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);
cv::Mat xyz = (icam.R * (points3d.t() - T_repeated)).t();
// Set points behind the camera to zero
double *xyz_ptr = xyz.ptr<double>(0);
for (size_t i = 0; i < num_joints; ++i)
{
if (xyz.at<double>(i, 2) <= 0)
if (xyz_ptr[i * 3 + 2] <= 0)
{
xyz.at<double>(i, 0) = 0;
xyz.at<double>(i, 1) = 0;
xyz.at<double>(i, 2) = 0;
xyz_ptr[i * 3 + 0] = 0;
xyz_ptr[i * 3 + 1] = 0;
xyz_ptr[i * 3 + 2] = 0;
}
}
@ -584,24 +622,28 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
// Add scores again
std::vector<int> dimsB = {(int)num_joints, 3};
cv::Mat bodies2D = cv::Mat(dimsB, CV_64F);
double *bodies2D_ptr = bodies2D.ptr<double>(0);
const double *uv_ptr = uv.ptr<double>(0);
const double *bodies3D_ptr = body3D.ptr<double>(0);
for (size_t i = 0; i < num_joints; ++i)
{
bodies2D.at<double>(i, 0) = uv.at<double>(i, 0);
bodies2D.at<double>(i, 1) = uv.at<double>(i, 1);
bodies2D.at<double>(i, 2) = body3D.at<double>(i, 3);
bodies2D_ptr[i * 3 + 0] = uv_ptr[i * 2 + 0];
bodies2D_ptr[i * 3 + 1] = uv_ptr[i * 2 + 1];
bodies2D_ptr[i * 3 + 2] = bodies3D_ptr[i * 4 + 3];
}
// Filter invalid projections
cv::Mat valid_x = (bodies2D.col(0) >= 0) & (bodies2D.col(0) < icam.cam.width);
cv::Mat valid_y = (bodies2D.col(1) >= 0) & (bodies2D.col(1) < icam.cam.height);
cv::Mat vis = valid_x & valid_y;
for (size_t i = 0; i < num_joints; ++i)
{
if (!vis.at<uchar>(i))
double x = bodies2D_ptr[i * 3 + 0];
double y = bodies2D_ptr[i * 3 + 1];
bool in_x = x >= 0 && x < icam.cam.width;
bool in_y = y >= 0 && y < icam.cam.height;
if (!in_x || !in_y)
{
bodies2D.at<double>(i, 0) = 0;
bodies2D.at<double>(i, 1) = 0;
bodies2D.at<double>(i, 2) = 0;
bodies2D_ptr[i * 3 + 0] = 0;
bodies2D_ptr[i * 3 + 1] = 0;
bodies2D_ptr[i * 3 + 2] = 0;
}
}
@ -640,11 +682,13 @@ double TriangulatorInternal::calc_pose_score(
size_t drop_k = static_cast<size_t>(pose1.rows * 0.2);
size_t min_k = 3;
std::vector<double> scores_vec;
const double *scores_ptr = scores.ptr<double>(0);
const uchar *mask_ptr = mask.ptr<uchar>(0);
for (int i = 0; i < scores.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
if (mask_ptr[i] > 0)
{
scores_vec.push_back(scores.at<double>(i));
scores_vec.push_back(scores_ptr[i]);
}
}
std::sort(scores_vec.begin(), scores_vec.end());
@ -656,15 +700,14 @@ double TriangulatorInternal::calc_pose_score(
// Calculate final score
double score = 0.0;
size_t items = 0;
for (size_t i = 0; i < scores_vec.size(); i++)
size_t n_items = scores_vec.size();
for (size_t i = 0; i < n_items; i++)
{
score += scores_vec[i];
items++;
}
if (items > 0)
if (n_items > 0)
{
score /= (double)items;
score /= (double)n_items;
}
return score;
@ -682,9 +725,9 @@ cv::Mat TriangulatorInternal::score_projection(
double min_score = 0.1;
// Calculate error
cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2);
cv::Mat error1 = diff.mul(diff);
cv::reduce(error1, error1, 1, cv::REDUCE_SUM, CV_64F);
cv::Mat error1 = pose1.colRange(0, 2) - repro1.colRange(0, 2);
error1 = error1.mul(error1);
error1 = error1.col(0) + error1.col(1);
cv::sqrt(error1, error1);
error1.setTo(0.0, ~mask);
@ -720,6 +763,7 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
cv::Mat mask1a = (pose1.col(2) >= min_score);
cv::Mat mask2a = (pose2.col(2) >= min_score);
const cv::Mat mask = mask1a & mask2a;
const uchar *mask_ptr = mask.ptr<uchar>(0);
// If too few joints are visible, return a low score
int num_visible = cv::countNonZero(mask);
@ -731,39 +775,46 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
return std::make_pair(pose3d, score);
}
// Triangulate points
// Extract coordinates
std::vector<int> dimsA = {2, num_visible};
cv::Mat points1 = cv::Mat(dimsA, CV_64F);
cv::Mat points2 = cv::Mat(dimsA, CV_64F);
const double *pose1_ptr = pose1.ptr<double>(0);
const double *pose2_ptr = pose2.ptr<double>(0);
double *points1_ptr = points1.ptr<double>(0);
double *points2_ptr = points2.ptr<double>(0);
int idx = 0;
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
if (mask_ptr[i] > 0)
{
points1.at<double>(0, idx) = pose1.at<double>(i, 0);
points1.at<double>(1, idx) = pose1.at<double>(i, 1);
points2.at<double>(0, idx) = pose2.at<double>(i, 0);
points2.at<double>(1, idx) = pose2.at<double>(i, 1);
points1_ptr[idx + 0 * num_visible] = pose1_ptr[i * 3 + 0];
points1_ptr[idx + 1 * num_visible] = pose1_ptr[i * 3 + 1];
points2_ptr[idx + 0 * num_visible] = pose2_ptr[i * 3 + 0];
points2_ptr[idx + 1 * num_visible] = pose2_ptr[i * 3 + 1];
idx++;
}
}
cv::Mat points3d_h, points3d;
cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points3d_h);
cv::convertPointsFromHomogeneous(points3d_h.t(), points3d);
// Triangulate points
cv::Mat points4d_h, points3d;
cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points4d_h);
cv::convertPointsFromHomogeneous(points4d_h.t(), points3d);
// Create the 3D pose matrix
std::vector<int> dimsB = {(int)pose1.rows, 4};
cv::Mat pose3d = cv::Mat(dimsB, CV_64F, cv::Scalar(0));
const double *points3d_ptr = points3d.ptr<double>(0);
double *pose3d_ptr = pose3d.ptr<double>(0);
idx = 0;
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
if (mask_ptr[i] > 0)
{
pose3d.at<double>(i, 0) = points3d.at<double>(idx, 0);
pose3d.at<double>(i, 1) = points3d.at<double>(idx, 1);
pose3d.at<double>(i, 2) = points3d.at<double>(idx, 2);
pose3d.at<double>(i, 3) = 1.0;
pose3d_ptr[i * 4 + 0] = points3d_ptr[idx * 3 + 0];
pose3d_ptr[i * 4 + 1] = points3d_ptr[idx * 3 + 1];
pose3d_ptr[i * 4 + 2] = points3d_ptr[idx * 3 + 2];
pose3d_ptr[i * 4 + 3] = 1.0;
idx++;
}
}
@ -772,11 +823,11 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
std::array<double, 3> mean = {0, 0, 0};
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
if (mask_ptr[i] > 0)
{
for (int j = 0; j < 3; ++j)
{
mean[j] += pose3d.at<double>(i, j);
mean[j] += pose3d_ptr[i * 4 + j];
}
}
}
@ -792,10 +843,7 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
mean[i] < center[i] - size[i] / 2)
{
// Very low score if outside room
for (int j = 0; j < pose1.rows; ++j)
{
pose3d.at<double>(j, 3) = 0.001;
}
pose3d.col(3).setTo(0.001);
return {pose3d, 0.001};
}
}
@ -817,13 +865,14 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Combine scores
cv::Mat scores = (score1 + score2) / 2.0;
const double *scores_ptr = scores.ptr<double>(0);
// Add scores to 3D pose
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
if (mask_ptr[i] > 0)
{
pose3d.at<double>(i, 3) = scores.at<double>(i);
pose3d_ptr[i * 4 + 3] = scores_ptr[i];
}
}
@ -831,14 +880,14 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
double wdist = 0.1;
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
if (mask_ptr[i] > 0)
{
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)
if (pose3d_ptr[i * 4 + j] > center[j] + size[j] / 2 + wdist ||
pose3d_ptr[i * 4 + j] < center[j] - size[j] / 2 - wdist)
{
pose3d.at<double>(i, 3) = 0.001;
pose3d_ptr[i * 4 + 3] = 0.001;
}
}
}
@ -852,18 +901,14 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
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)
if (pose3d_ptr[limb[0] * 4 + 3] > min_score &&
pose3d_ptr[limb[1] * 4 + 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);
double dx = pose3d_ptr[limb[0] * 4 + 0] - pose3d_ptr[limb[1] * 4 + 0];
double dy = pose3d_ptr[limb[0] * 4 + 1] - pose3d_ptr[limb[1] * 4 + 1];
double dz = pose3d_ptr[limb[0] * 4 + 2] - pose3d_ptr[limb[1] * 4 + 2];
double length = std::sqrt(dx * dx + dy * dy + dz * dz);
if (length > max_length)
{
invalid_joints.push_back(limb[1]);
@ -872,7 +917,7 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
}
for (size_t i = 0; i < invalid_joints.size(); ++i)
{
pose3d.at<double>(invalid_joints[i], 3) = 0.001;
pose3d_ptr[invalid_joints[i] * 4 + 3] = 0.001;
}
}
@ -882,9 +927,9 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
std::vector<double> scores_vec;
for (int i = 0; i < pose1.rows; ++i)
{
if (pose3d.at<double>(i, 3) > min_score)
if (pose3d_ptr[i * 4 + 3] > min_score)
{
scores_vec.push_back(pose3d.at<double>(i, 3));
scores_vec.push_back(pose3d_ptr[i * 4 + 3]);
}
}
std::sort(scores_vec.begin(), scores_vec.end());
@ -896,15 +941,14 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Calculate final score
double score = 0.0;
size_t items = 0;
for (size_t i = 0; i < scores_vec.size(); i++)
size_t n_items = scores_vec.size();
for (size_t i = 0; i < n_items; i++)
{
score += scores_vec[i];
items++;
}
if (items > 0)
if (n_items > 0)
{
score /= (double)items;
score /= (double)n_items;
}
return std::make_pair(pose3d, score);
@ -926,15 +970,17 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows;
const double *pose_3d_ptr = pose_3d.ptr<double>(0);
cv::Point3d center(0, 0, 0);
size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j)
{
if (pose_3d.at<double>(j, 3) > min_score)
if (pose_3d_ptr[j * 4 + 3] > min_score)
{
center.x += pose_3d.at<double>(j, 0);
center.y += pose_3d.at<double>(j, 1);
center.z += pose_3d.at<double>(j, 2);
center.x += pose_3d_ptr[j * 4 + 0];
center.y += pose_3d_ptr[j * 4 + 1];
center.z += pose_3d_ptr[j * 4 + 2];
num_valid++;
}
}
@ -954,6 +1000,8 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows;
const double *pose_3d_ptr = pose_3d.ptr<double>(0);
const cv::Point3d &center = centers[i];
double best_dist = std::numeric_limits<double>::infinity();
int best_group = -1;
@ -963,6 +1011,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
auto &group = groups[j];
cv::Point3d &group_center = std::get<0>(group);
cv::Mat &group_pose = std::get<1>(group);
const double *group_pose_ptr = group_pose.ptr<double>(0);
// Check if the center is close enough
if (cv::norm(group_center - center) < max_center_dist)
@ -971,18 +1020,14 @@ 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_ptr[row * 4 + 3] > min_score &&
group_pose_ptr[row * 4 + 3] > min_score)
{
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));
double dx = pose_3d_ptr[row * 4 + 0] - group_pose_ptr[row * 4 + 0];
double dy = pose_3d_ptr[row * 4 + 1] - group_pose_ptr[row * 4 + 1];
double dz = pose_3d_ptr[row * 4 + 2] - group_pose_ptr[row * 4 + 2];
double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
dists.push_back(dist);
}
}
double dist = std::numeric_limits<double>::infinity();
@ -1018,20 +1063,23 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
group_center = (group_center * n_elems + center) / (n_elems + 1);
cv::Mat new_pose = group_pose.clone();
const double *group_pose_ptr = group_pose.ptr<double>(0);
const double *pose_3d_ptr = pose_3d.ptr<double>(0);
double *new_pose_ptr = new_pose.ptr<double>(0);
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)) /
new_pose_ptr[row * 4 + 0] =
(group_pose_ptr[row * 4 + 0] * n_elems + pose_3d_ptr[row * 4 + 0]) /
(n_elems + 1);
new_pose.at<double>(row, 1) =
(group_pose.at<double>(row, 1) * n_elems + pose_3d.at<double>(row, 1)) /
new_pose_ptr[row * 4 + 1] =
(group_pose_ptr[row * 4 + 1] * n_elems + pose_3d_ptr[row * 4 + 1]) /
(n_elems + 1);
new_pose.at<double>(row, 2) =
(group_pose.at<double>(row, 2) * n_elems + pose_3d.at<double>(row, 2)) /
new_pose_ptr[row * 4 + 2] =
(group_pose_ptr[row * 4 + 2] * n_elems + pose_3d_ptr[row * 4 + 2]) /
(n_elems + 1);
new_pose.at<double>(row, 3) =
(group_pose.at<double>(row, 3) * n_elems + pose_3d.at<double>(row, 3)) /
new_pose_ptr[row * 4 + 3] =
(group_pose_ptr[row * 4 + 3] * n_elems + pose_3d_ptr[row * 4 + 3]) /
(n_elems + 1);
}
group_pose = new_pose;
@ -1052,41 +1100,50 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
// Merge poses to create initial pose
// Use only those triangulations with a high score
cv::Mat sum_poses = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type());
double *sum_poses_ptr = sum_poses.ptr<double>(0);
std::vector<int> sum_mask(num_joints, 0);
for (int i = 0; i < num_poses; ++i)
{
const cv::Mat &pose = poses_3d[i];
const double *pose_ptr = pose.ptr<double>(0);
for (int j = 0; j < num_joints; ++j)
{
if (pose.at<double>(j, 3) > min_score)
if (pose_ptr[j * 4 + 3] > min_score)
{
sum_poses.row(j) += pose.row(j);
for (int k = 0; k < 4; ++k)
{
sum_poses_ptr[j * 4 + k] += pose_ptr[j * 4 + k];
}
sum_mask[j]++;
}
}
}
cv::Mat initial_pose_3d = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type());
double *initial_pose_3d_ptr = initial_pose_3d.ptr<double>(0);
for (int j = 0; j < num_joints; ++j)
{
if (sum_mask[j] > 0)
{
initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j];
for (int k = 0; k < 4; ++k)
{
initial_pose_3d_ptr[j * 4 + k] = sum_poses_ptr[j * 4 + k] / sum_mask[j];
}
}
}
// Use center as default if the initial pose is empty
cv::Mat jmask = cv::Mat::zeros(num_joints, 1, CV_8U);
std::vector<bool> jmask(num_joints, false);
cv::Point3d center(0, 0, 0);
int valid_joints = 0;
for (int j = 0; j < num_joints; ++j)
{
if (initial_pose_3d.at<double>(j, 3) > 0.0)
if (initial_pose_3d_ptr[j * 4 + 3] > min_score)
{
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));
jmask[j] = true;
center.x += initial_pose_3d_ptr[j * 4 + 0];
center.y += initial_pose_3d_ptr[j * 4 + 1];
center.z += initial_pose_3d_ptr[j * 4 + 2];
valid_joints++;
}
}
@ -1096,11 +1153,11 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
}
for (int j = 0; j < num_joints; ++j)
{
if (jmask.at<uchar>(j) == 0)
if (!jmask[j])
{
initial_pose_3d.at<double>(j, 0) = center.x;
initial_pose_3d.at<double>(j, 1) = center.y;
initial_pose_3d.at<double>(j, 2) = center.z;
initial_pose_3d_ptr[j * 4 + 0] = center.x;
initial_pose_3d_ptr[j * 4 + 1] = center.y;
initial_pose_3d_ptr[j * 4 + 2] = center.z;
}
}
@ -1109,23 +1166,24 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
double max_dist = 1.2;
cv::Mat mask = cv::Mat::zeros(num_poses, num_joints, CV_8U);
cv::Mat distances = cv::Mat::zeros(num_poses, num_joints, CV_64F);
double *distances_ptr = distances.ptr<double>(0);
u_char *mask_ptr = mask.ptr<u_char>(0);
for (int i = 0; i < num_poses; ++i)
{
const cv::Mat &pose = poses_3d[i];
const double *pose_ptr = pose.ptr<double>(0);
for (int j = 0; j < num_joints; ++j)
{
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))
double dx = pose_ptr[j * 4 + 0] - initial_pose_3d_ptr[j * 4 + 0];
double dy = pose_ptr[j * 4 + 1] - initial_pose_3d_ptr[j * 4 + 1];
double dz = pose_ptr[j * 4 + 2] - initial_pose_3d_ptr[j * 4 + 2];
double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
distances_ptr[i * num_joints + j] = dist;
if (dist <= max_dist && pose_ptr[j * 4 + 3] > (min_score - offset))
{
mask.at<uchar>(i, j) = 1;
mask_ptr[i * num_joints + j] = 1;
}
}
}
@ -1138,9 +1196,10 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
std::vector<std::pair<double, int>> valid_indices;
for (int i = 0; i < num_poses; ++i)
{
if (mask.at<uchar>(i, j))
if (mask_ptr[i * num_joints + j])
{
valid_indices.push_back({distances.at<double>(i, j), i});
auto item = std::make_pair(distances_ptr[i * num_joints + j], i);
valid_indices.push_back(item);
}
}
std::sort(valid_indices.begin(), valid_indices.end());
@ -1152,27 +1211,39 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
// Combine masks
mask = mask & best_k_mask;
mask_ptr = mask.ptr<u_char>(0);
// Compute the final pose
sum_poses = cv::Mat::zeros(sum_poses.size(), sum_poses.type());
sum_poses_ptr = sum_poses.ptr<double>(0);
sum_mask = std::vector<int>(num_joints, 0);
for (int i = 0; i < num_poses; ++i)
{
const cv::Mat &pose = poses_3d[i];
const double *pose_ptr = pose.ptr<double>(0);
for (int j = 0; j < num_joints; ++j)
{
if (mask.at<uchar>(i, j))
if (mask_ptr[i * num_joints + j] > 0)
{
sum_poses.row(j) += poses_3d[i].row(j);
for (int k = 0; k < 4; ++k)
{
sum_poses_ptr[j * 4 + k] += pose_ptr[j * 4 + k];
}
sum_mask[j]++;
}
}
}
cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type());
double *final_pose_3d_ptr = final_pose_3d.ptr<double>(0);
for (int j = 0; j < num_joints; ++j)
{
if (sum_mask[j] > 0)
{
final_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j];
for (int k = 0; k < 4; ++k)
{
final_pose_3d_ptr[j * 4 + k] = sum_poses_ptr[j * 4 + k] / sum_mask[j];
}
}
}

1
spt/triangulator.hpp
View File

@ -70,7 +70,6 @@ private:
std::vector<cv::Mat> last_poses_3d;
cv::Mat undistort_points(cv::Mat &points, CameraInternal &icam);
void undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam);
std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(