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) (duration 00:00:56)
```json ```json
{ {
"avg_time_2d": 0.10053871915102824, "avg_time_2d": 0.101639888540576,
"avg_time_3d": 0.0020945760392651115, "avg_time_3d": 0.0018720938168030833,
"avg_fps": 9.743426810431163 "avg_fps": 9.660717312392508
} }
{ {
"person_nums": { "person_nums": {

479
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}; std::vector<int> dims = {(int)num_joints, 3};
cv::Mat pose_mat = cv::Mat(dims, CV_64F); 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 k = 0; k < num_joints; ++k)
{ {
for (size_t l = 0; l < 3; ++l) const auto &joint = poses_2d[i][j][k];
{ mat_ptr[k * 3 + 0] = joint[0];
pose_mat.at<double>(k, l) = poses_2d[i][j][k][l]; mat_ptr[k * 3 + 1] = joint[1];
mat_ptr[k * 3 + 2] = joint[2];
} }
camera_poses.push_back(std::move(pose_mat));
} }
camera_poses.push_back(pose_mat); poses_2d_mats.push_back(std::move(camera_poses));
}
poses_2d_mats.push_back(camera_poses);
} }
std::vector<CameraInternal> internal_cameras; std::vector<CameraInternal> internal_cameras;
for (size_t i = 0; i < cameras.size(); ++i) 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 // Select core joints
std::vector<cv::Mat> last_core_poses; 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) 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}; std::vector<int> dims = {(int)core_joint_idx.size(), 4};
cv::Mat last_poses_core(dims, pose.type()); 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)); 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 // Project
@ -223,61 +228,97 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
if (!last_poses_3d.empty()) if (!last_poses_3d.empty())
{ {
// Calculate index pairs and initialize vectors // 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) 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>>(); 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>(); 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});
} }
} }
} }
std::vector<std::array<size_t, 2>> indices_ik;
#pragma omp parallel for ordered schedule(dynamic) for (size_t i = 0; i < cameras.size(); ++i)
for (size_t e = 0; e < indices.size(); ++e)
{ {
const auto &i = indices[e][0]; size_t num_new_persons = poses_2d_mats[i].size();
const auto &j = indices[e][1]; for (size_t k = 0; k < num_new_persons; ++k)
const auto &k = indices[e][2]; {
indices_ik.push_back({i, k});
}
}
const std::vector<cv::Mat> &poses = poses_2d_mats[i]; // Precalculate core poses
const std::vector<cv::Mat> &last_poses = std::get<0>(last_poses_2d[i]); std::vector<cv::Mat> poses_2d_mats_core_list;
const std::vector<cv::Mat> &dists = std::get<1>(last_poses_2d[i]); 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 &pose = poses_2d_mats[i][k];
const cv::Mat &last_pose = last_poses[j];
const cv::Mat &pose = poses[k];
std::vector<int> dims = {(int)core_joint_idx.size(), 3}; std::vector<int> dims = {(int)core_joint_idx.size(), 3};
cv::Mat pose_core(dims, pose.type()); 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(core_joint_idx[j]).copyTo(pose_core.row(j));
pose.row(idx).copyTo(pose_core.row(l));
} }
// Calculate score poses_2d_mats_core_list[e] = pose_core;
double score = calc_pose_score(pose_core, last_pose, dists[j], internal_cameras[i]); 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) if (score > threshold)
{ {
#pragma omp ordered #pragma omp critical
{
scored_pasts[i][j].push_back(k); scored_pasts[i][j].push_back(k);
} }
} }
} }
}
// Create pairs of persons // Create pairs of persons
// Checks if the person was already matched to the last frame and if so only creates pairs // 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 // 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) 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::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> all_pairs;
std::vector<std::array<size_t, 4>> indices; 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]; const auto [i, j, k, l] = indices[e];
int pid1 = std::accumulate(num_persons.begin(), num_persons.begin() + i, 0) + k; int pid1 = num_persons_sum[i] + k;
int pid2 = std::accumulate(num_persons.begin(), num_persons.begin() + j, 0) + l; int pid2 = num_persons_sum[k] + l;
bool match = false; bool match = false;
if (!last_poses_3d.empty()) 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 &smi = scored_pasts[i][m];
auto &smj = scored_pasts[j][m]; auto &smj = scored_pasts[j][m];
if (std::find(smi.begin(), smi.end(), k) != smi.end() && bool in_smi = std::find(smi.begin(), smi.end(), k) != smi.end();
std::find(smj.begin(), smj.end(), l) != smj.end()) bool in_smj = std::find(smj.begin(), smj.end(), l) != smj.end();
if (in_smi && in_smj)
{ {
match = true; match = true;
#pragma omp ordered auto item = std::make_pair(
all_pairs.emplace_back(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2)); 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() || else if (in_smi || in_smj)
std::find(smj.begin(), smj.end(), l) != smj.end())
{ {
match = true; match = true;
} }
@ -327,9 +371,12 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
if (!match) if (!match)
{ {
#pragma omp ordered auto item = std::make_pair(
all_pairs.emplace_back(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2)); 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]; const auto &group = groups[i];
std::vector<cv::Mat> poses; std::vector<cv::Mat> poses;
poses.reserve(std::get<2>(group).size());
for (const auto &idx : std::get<2>(group)) for (const auto &idx : std::get<2>(group))
{ {
poses.push_back(all_full_poses[idx]); 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); auto merged_pose = merge_group(poses, min_score);
all_merged_poses[i] = (merged_pose); all_merged_poses[i] = (merged_pose);
} }
last_poses_3d = all_merged_poses;
// Convert to output format // Convert to output format
std::vector<std::vector<std::array<double, 4>>> poses_3d; 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) for (size_t i = 0; i < all_merged_poses.size(); ++i)
{ {
const auto &mat = all_merged_poses[i]; const auto &mat = all_merged_poses[i];
const double *mat_ptr = mat.ptr<double>(0);
std::vector<std::array<double, 4>> pose; std::vector<std::array<double, 4>> pose;
size_t num_joints = mat.rows; size_t num_joints = mat.rows;
pose.reserve(num_joints); pose.reserve(num_joints);
@ -438,7 +489,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
std::array<double, 4> point; std::array<double, 4> point;
for (size_t k = 0; k < 4; ++k) 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); 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; 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 width = icam.cam.width;
int height = icam.cam.height; int height = icam.cam.height;
@ -476,55 +526,41 @@ cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &
cv::Mat newK = cv::getOptimalNewCameraMatrix( cv::Mat newK = cv::getOptimalNewCameraMatrix(
icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); 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) for (size_t p = 0; p < poses.size(); ++p)
{ {
int num_joints = poses[p].rows;
// Extract the (x, y) coordinates // Extract the (x, y) coordinates
cv::Mat points = poses[p].colRange(0, 2).clone(); cv::Mat points = poses[p].colRange(0, 2).clone();
points = points.reshape(2);
// Undistort the points // Undistort the points
cv::Mat newK = undistort_points(points, icam); cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
if (p == poses.size() - 1)
{
// Update the camera matrix as well
icam.K = newK;
icam.DC = cv::Mat::zeros(5, 1, CV_64F);
}
// Update the original poses with the undistorted points // Update the original poses with the undistorted points
for (int j = 0; j < num_joints; ++j) points = points.reshape(1);
{ points.copyTo(poses[p].colRange(0, 2));
poses[p].at<double>(j, 0) = points.at<double>(j, 0);
poses[p].at<double>(j, 1) = points.at<double>(j, 1);
}
// Mask out points that are far outside the image (points slightly outside are still valid) // 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) for (int j = 0; j < num_joints; ++j)
{ {
double x = poses[p].at<double>(j, 0); double x = poses_ptr[j * 3 + 0];
double y = poses[p].at<double>(j, 1); double y = poses_ptr[j * 3 + 1];
bool in_x = x >= -offset && x < icam.cam.width + offset; bool in_x = x >= -mask_offset && x < width + mask_offset;
bool in_y = y >= -offset && y < icam.cam.height + offset; bool in_y = y >= -mask_offset && y < height + mask_offset;
if (!in_x || !in_y) if (!in_x || !in_y)
{ {
poses[p].at<double>(j, 0) = 0; poses_ptr[j * 3 + 0] = 0;
poses[p].at<double>(j, 1) = 0; poses_ptr[j * 3 + 1] = 0;
poses[p].at<double>(j, 2) = 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) const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists)
{ {
size_t num_persons = bodies3D.size(); 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> bodies2D_list(num_persons);
std::vector<cv::Mat> dists_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) for (size_t i = 0; i < num_persons; ++i)
{ {
const cv::Mat &body3D = bodies3D[i]; const cv::Mat &body3D = bodies3D[i];
size_t num_joints = body3D.size[0];
// Split up vector // Split up vector
cv::Mat points3d = body3D.colRange(0, 3).clone(); cv::Mat points3d = body3D.colRange(0, 3).clone();
// Project from world to camera coordinate system // 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(); cv::Mat xyz = (icam.R * (points3d.t() - T_repeated)).t();
// Set points behind the camera to zero // Set points behind the camera to zero
double *xyz_ptr = xyz.ptr<double>(0);
for (size_t i = 0; i < num_joints; ++i) 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_ptr[i * 3 + 0] = 0;
xyz.at<double>(i, 1) = 0; xyz_ptr[i * 3 + 1] = 0;
xyz.at<double>(i, 2) = 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 // Add scores again
std::vector<int> dimsB = {(int)num_joints, 3}; std::vector<int> dimsB = {(int)num_joints, 3};
cv::Mat bodies2D = cv::Mat(dimsB, CV_64F); 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) for (size_t i = 0; i < num_joints; ++i)
{ {
bodies2D.at<double>(i, 0) = uv.at<double>(i, 0); bodies2D_ptr[i * 3 + 0] = uv_ptr[i * 2 + 0];
bodies2D.at<double>(i, 1) = uv.at<double>(i, 1); bodies2D_ptr[i * 3 + 1] = uv_ptr[i * 2 + 1];
bodies2D.at<double>(i, 2) = body3D.at<double>(i, 3); bodies2D_ptr[i * 3 + 2] = bodies3D_ptr[i * 4 + 3];
} }
// Filter invalid projections // 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) 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_ptr[i * 3 + 0] = 0;
bodies2D.at<double>(i, 1) = 0; bodies2D_ptr[i * 3 + 1] = 0;
bodies2D.at<double>(i, 2) = 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 drop_k = static_cast<size_t>(pose1.rows * 0.2);
size_t min_k = 3; size_t min_k = 3;
std::vector<double> scores_vec; 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) 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()); std::sort(scores_vec.begin(), scores_vec.end());
@ -656,15 +700,14 @@ double TriangulatorInternal::calc_pose_score(
// Calculate final score // Calculate final score
double score = 0.0; double score = 0.0;
size_t items = 0; size_t n_items = scores_vec.size();
for (size_t i = 0; i < scores_vec.size(); i++) for (size_t i = 0; i < n_items; i++)
{ {
score += scores_vec[i]; score += scores_vec[i];
items++;
} }
if (items > 0) if (n_items > 0)
{ {
score /= (double)items; score /= (double)n_items;
} }
return score; return score;
@ -682,9 +725,9 @@ cv::Mat TriangulatorInternal::score_projection(
double min_score = 0.1; double min_score = 0.1;
// Calculate error // Calculate error
cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2); cv::Mat error1 = pose1.colRange(0, 2) - repro1.colRange(0, 2);
cv::Mat error1 = diff.mul(diff); error1 = error1.mul(error1);
cv::reduce(error1, error1, 1, cv::REDUCE_SUM, CV_64F); error1 = error1.col(0) + error1.col(1);
cv::sqrt(error1, error1); cv::sqrt(error1, error1);
error1.setTo(0.0, ~mask); 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 mask1a = (pose1.col(2) >= min_score);
cv::Mat mask2a = (pose2.col(2) >= min_score); cv::Mat mask2a = (pose2.col(2) >= min_score);
const cv::Mat mask = mask1a & mask2a; const cv::Mat mask = mask1a & mask2a;
const uchar *mask_ptr = mask.ptr<uchar>(0);
// If too few joints are visible, return a low score // If too few joints are visible, return a low score
int num_visible = cv::countNonZero(mask); 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); return std::make_pair(pose3d, score);
} }
// Triangulate points // Extract coordinates
std::vector<int> dimsA = {2, num_visible}; std::vector<int> dimsA = {2, num_visible};
cv::Mat points1 = cv::Mat(dimsA, CV_64F); cv::Mat points1 = cv::Mat(dimsA, CV_64F);
cv::Mat points2 = 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; int idx = 0;
for (int i = 0; i < pose1.rows; ++i) 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_ptr[idx + 0 * num_visible] = pose1_ptr[i * 3 + 0];
points1.at<double>(1, idx) = pose1.at<double>(i, 1); points1_ptr[idx + 1 * num_visible] = pose1_ptr[i * 3 + 1];
points2.at<double>(0, idx) = pose2.at<double>(i, 0); points2_ptr[idx + 0 * num_visible] = pose2_ptr[i * 3 + 0];
points2.at<double>(1, idx) = pose2.at<double>(i, 1); points2_ptr[idx + 1 * num_visible] = pose2_ptr[i * 3 + 1];
idx++; idx++;
} }
} }
cv::Mat points3d_h, points3d; // Triangulate points
cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points3d_h); cv::Mat points4d_h, points3d;
cv::convertPointsFromHomogeneous(points3d_h.t(), points3d); cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points4d_h);
cv::convertPointsFromHomogeneous(points4d_h.t(), points3d);
// Create the 3D pose matrix // Create the 3D pose matrix
std::vector<int> dimsB = {(int)pose1.rows, 4}; std::vector<int> dimsB = {(int)pose1.rows, 4};
cv::Mat pose3d = cv::Mat(dimsB, CV_64F, cv::Scalar(0)); 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; idx = 0;
for (int i = 0; i < pose1.rows; ++i) 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_ptr[i * 4 + 0] = points3d_ptr[idx * 3 + 0];
pose3d.at<double>(i, 1) = points3d.at<double>(idx, 1); pose3d_ptr[i * 4 + 1] = points3d_ptr[idx * 3 + 1];
pose3d.at<double>(i, 2) = points3d.at<double>(idx, 2); pose3d_ptr[i * 4 + 2] = points3d_ptr[idx * 3 + 2];
pose3d.at<double>(i, 3) = 1.0; pose3d_ptr[i * 4 + 3] = 1.0;
idx++; idx++;
} }
} }
@ -772,11 +823,11 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
std::array<double, 3> mean = {0, 0, 0}; std::array<double, 3> mean = {0, 0, 0};
for (int i = 0; i < pose1.rows; ++i) 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) 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) mean[i] < center[i] - size[i] / 2)
{ {
// Very low score if outside room // Very low score if outside room
for (int j = 0; j < pose1.rows; ++j) pose3d.col(3).setTo(0.001);
{
pose3d.at<double>(j, 3) = 0.001;
}
return {pose3d, 0.001}; return {pose3d, 0.001};
} }
} }
@ -817,13 +865,14 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Combine scores // Combine scores
cv::Mat scores = (score1 + score2) / 2.0; cv::Mat scores = (score1 + score2) / 2.0;
const double *scores_ptr = scores.ptr<double>(0);
// Add scores to 3D pose // Add scores to 3D pose
for (int i = 0; i < pose1.rows; ++i) 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; double wdist = 0.1;
for (int i = 0; i < pose1.rows; ++i) 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) for (int j = 0; j < 3; ++j)
{ {
if (pose3d.at<double>(i, j) > center[j] + size[j] / 2 + wdist || if (pose3d_ptr[i * 4 + j] > center[j] + size[j] / 2 + wdist ||
pose3d.at<double>(i, 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) for (size_t i = 0; i < core_limbs_idx.size(); ++i)
{ {
auto limb = core_limbs_idx[i]; auto limb = core_limbs_idx[i];
if (pose3d.at<double>(limb[0], 3) > min_score && if (pose3d_ptr[limb[0] * 4 + 3] > min_score &&
pose3d.at<double>(limb[1], 3) > min_score) pose3d_ptr[limb[1] * 4 + 3] > min_score)
{ {
cv::Point3d p1 = cv::Point3d( double dx = pose3d_ptr[limb[0] * 4 + 0] - pose3d_ptr[limb[1] * 4 + 0];
pose3d.at<double>(limb[0], 0), double dy = pose3d_ptr[limb[0] * 4 + 1] - pose3d_ptr[limb[1] * 4 + 1];
pose3d.at<double>(limb[0], 1), double dz = pose3d_ptr[limb[0] * 4 + 2] - pose3d_ptr[limb[1] * 4 + 2];
pose3d.at<double>(limb[0], 2)); double length = std::sqrt(dx * dx + dy * dy + dz * dz);
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) if (length > max_length)
{ {
invalid_joints.push_back(limb[1]); 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) 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; std::vector<double> scores_vec;
for (int i = 0; i < pose1.rows; ++i) 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()); std::sort(scores_vec.begin(), scores_vec.end());
@ -896,15 +941,14 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Calculate final score // Calculate final score
double score = 0.0; double score = 0.0;
size_t items = 0; size_t n_items = scores_vec.size();
for (size_t i = 0; i < scores_vec.size(); i++) for (size_t i = 0; i < n_items; i++)
{ {
score += scores_vec[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); 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; const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows; size_t num_joints = pose_3d.rows;
const double *pose_3d_ptr = pose_3d.ptr<double>(0);
cv::Point3d center(0, 0, 0); cv::Point3d center(0, 0, 0);
size_t num_valid = 0; size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j) 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.x += pose_3d_ptr[j * 4 + 0];
center.y += pose_3d.at<double>(j, 1); center.y += pose_3d_ptr[j * 4 + 1];
center.z += pose_3d.at<double>(j, 2); center.z += pose_3d_ptr[j * 4 + 2];
num_valid++; 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; const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows; size_t num_joints = pose_3d.rows;
const double *pose_3d_ptr = pose_3d.ptr<double>(0);
const cv::Point3d &center = centers[i]; const cv::Point3d &center = centers[i];
double best_dist = std::numeric_limits<double>::infinity(); double best_dist = std::numeric_limits<double>::infinity();
int best_group = -1; int best_group = -1;
@ -963,6 +1011,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
auto &group = groups[j]; auto &group = groups[j];
cv::Point3d &group_center = std::get<0>(group); cv::Point3d &group_center = std::get<0>(group);
cv::Mat &group_pose = std::get<1>(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 // Check if the center is close enough
if (cv::norm(group_center - center) < max_center_dist) 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; std::vector<double> dists;
for (size_t row = 0; row < num_joints; row++) for (size_t row = 0; row < num_joints; row++)
{ {
if (pose_3d.at<double>(row, 3) > min_score && if (pose_3d_ptr[row * 4 + 3] > min_score &&
group_pose.at<double>(row, 3) > min_score) group_pose_ptr[row * 4 + 3] > min_score)
{ {
cv::Point3d p1 = cv::Point3d( double dx = pose_3d_ptr[row * 4 + 0] - group_pose_ptr[row * 4 + 0];
pose_3d.at<double>(row, 0), double dy = pose_3d_ptr[row * 4 + 1] - group_pose_ptr[row * 4 + 1];
pose_3d.at<double>(row, 1), double dz = pose_3d_ptr[row * 4 + 2] - group_pose_ptr[row * 4 + 2];
pose_3d.at<double>(row, 2)); double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
cv::Point3d p2 = cv::Point3d( dists.push_back(dist);
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 dist = std::numeric_limits<double>::infinity(); 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); group_center = (group_center * n_elems + center) / (n_elems + 1);
cv::Mat new_pose = group_pose.clone(); 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++) for (size_t row = 0; row < num_joints; row++)
{ {
new_pose_ptr[row * 4 + 0] =
new_pose.at<double>(row, 0) = (group_pose_ptr[row * 4 + 0] * n_elems + pose_3d_ptr[row * 4 + 0]) /
(group_pose.at<double>(row, 0) * n_elems + pose_3d.at<double>(row, 0)) /
(n_elems + 1); (n_elems + 1);
new_pose.at<double>(row, 1) = new_pose_ptr[row * 4 + 1] =
(group_pose.at<double>(row, 1) * n_elems + pose_3d.at<double>(row, 1)) / (group_pose_ptr[row * 4 + 1] * n_elems + pose_3d_ptr[row * 4 + 1]) /
(n_elems + 1); (n_elems + 1);
new_pose.at<double>(row, 2) = new_pose_ptr[row * 4 + 2] =
(group_pose.at<double>(row, 2) * n_elems + pose_3d.at<double>(row, 2)) / (group_pose_ptr[row * 4 + 2] * n_elems + pose_3d_ptr[row * 4 + 2]) /
(n_elems + 1); (n_elems + 1);
new_pose.at<double>(row, 3) = new_pose_ptr[row * 4 + 3] =
(group_pose.at<double>(row, 3) * n_elems + pose_3d.at<double>(row, 3)) / (group_pose_ptr[row * 4 + 3] * n_elems + pose_3d_ptr[row * 4 + 3]) /
(n_elems + 1); (n_elems + 1);
} }
group_pose = new_pose; 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 // Merge poses to create initial pose
// Use only those triangulations with a high score // Use only those triangulations with a high score
cv::Mat sum_poses = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type()); 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); std::vector<int> sum_mask(num_joints, 0);
for (int i = 0; i < num_poses; ++i) for (int i = 0; i < num_poses; ++i)
{ {
const cv::Mat &pose = poses_3d[i]; const cv::Mat &pose = poses_3d[i];
const double *pose_ptr = pose.ptr<double>(0);
for (int j = 0; j < num_joints; ++j) 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]++; sum_mask[j]++;
} }
} }
} }
cv::Mat initial_pose_3d = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type()); 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) for (int j = 0; j < num_joints; ++j)
{ {
if (sum_mask[j] > 0) 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 // 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); cv::Point3d center(0, 0, 0);
int valid_joints = 0; int valid_joints = 0;
for (int j = 0; j < num_joints; ++j) 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; jmask[j] = true;
center += cv::Point3d( center.x += initial_pose_3d_ptr[j * 4 + 0];
initial_pose_3d.at<double>(j, 0), center.y += initial_pose_3d_ptr[j * 4 + 1];
initial_pose_3d.at<double>(j, 1), center.z += initial_pose_3d_ptr[j * 4 + 2];
initial_pose_3d.at<double>(j, 2));
valid_joints++; 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) 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_ptr[j * 4 + 0] = center.x;
initial_pose_3d.at<double>(j, 1) = center.y; initial_pose_3d_ptr[j * 4 + 1] = center.y;
initial_pose_3d.at<double>(j, 2) = center.z; 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; double max_dist = 1.2;
cv::Mat mask = cv::Mat::zeros(num_poses, num_joints, CV_8U); cv::Mat mask = cv::Mat::zeros(num_poses, num_joints, CV_8U);
cv::Mat distances = cv::Mat::zeros(num_poses, num_joints, CV_64F); 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) 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) for (int j = 0; j < num_joints; ++j)
{ {
cv::Point3d joint_i = cv::Point3d( double dx = pose_ptr[j * 4 + 0] - initial_pose_3d_ptr[j * 4 + 0];
poses_3d[i].at<double>(j, 0), double dy = pose_ptr[j * 4 + 1] - initial_pose_3d_ptr[j * 4 + 1];
poses_3d[i].at<double>(j, 1), double dz = pose_ptr[j * 4 + 2] - initial_pose_3d_ptr[j * 4 + 2];
poses_3d[i].at<double>(j, 2)); double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
cv::Point3d joint_initial = cv::Point3d( distances_ptr[i * num_joints + j] = dist;
initial_pose_3d.at<double>(j, 0),
initial_pose_3d.at<double>(j, 1), if (dist <= max_dist && pose_ptr[j * 4 + 3] > (min_score - offset))
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))
{ {
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; std::vector<std::pair<double, int>> valid_indices;
for (int i = 0; i < num_poses; ++i) 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()); 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 // Combine masks
mask = mask & best_k_mask; mask = mask & best_k_mask;
mask_ptr = mask.ptr<u_char>(0);
// Compute the final pose // Compute the final pose
sum_poses = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); 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); sum_mask = std::vector<int>(num_joints, 0);
for (int i = 0; i < num_poses; ++i) 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) 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]++; sum_mask[j]++;
} }
} }
} }
cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); 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) for (int j = 0; j < num_joints; ++j)
{ {
if (sum_mask[j] > 0) 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; 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); void undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam);
std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses( std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(