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

Optimized undistortion.

Browse Source
This commit is contained in:
Daniel
2025-02-11 14:35:22 +01:00
parent 24d951f31d
commit 7ce02c44c8
2 changed files with 63 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
media/RESULTS.md
View File

@ -294,26 +294,26 @@ Results of the model in various experiments on different datasets. \
```json
{
"img_loading": 0.0422137,
"demosaicing": 0.000738378,
"avg_time_2d": 0.0148837,
"avg_time_3d": 0.000220359,
"fps": 63.1215
"img_loading": 0.0424014,
"demosaicing": 0.000721194,
"avg_time_2d": 0.0148998,
"avg_time_3d": 0.000186437,
"fps": 63.2615
}
{
"triangulator_calls": 301,
"init_time": 7.04818e-06,
"undistort_time": 3.18955e-05,
"project_time": 3.92248e-06,
"match_time": 1.17322e-05,
"pairs_time": 6.12719e-06,
"pair_scoring_time": 4.9701e-05,
"grouping_time": 8.03308e-06,
"full_time": 5.63869e-05,
"merge_time": 1.32789e-05,
"post_time": 9.21993e-06,
"convert_time": 2.10834e-07,
"total_time": 0.000197817
"init_time": 7.22995e-06,
"undistort_time": 2.88306e-05,
"project_time": 3.50066e-06,
"match_time": 1.16445e-05,
"pairs_time": 6.11458e-06,
"pair_scoring_time": 4.69781e-05,
"grouping_time": 7.95871e-06,
"full_time": 4.9667e-05,
"merge_time": 1.32834e-05,
"post_time": 9.24932e-06,
"convert_time": 2.06937e-07,
"total_time": 0.000184941
}
{
"person_nums": {

90
rpt/triangulator.cpp
View File

@ -624,87 +624,61 @@ void TriangulatorInternal::undistort_poses(
icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height));
}
// Convert vectors to mats
// Convert vectors to single mat
size_t num_persons = poses_2d.size();
size_t num_joints = poses_2d[0].size();
std::vector<cv::Mat> poses;
for (size_t j = 0; j < num_persons; ++j)
std::vector<int> dims = {(int)(num_persons * num_joints), 2};
cv::Mat points_mat = cv::Mat(dims, CV_32F);
float *mat_ptr = points_mat.ptr<float>(0);
for (size_t i = 0; i < num_persons; ++i)
{
std::vector<int> dims = {(int)num_joints, 3};
cv::Mat pose_mat = cv::Mat(dims, CV_32F);
// Use pointer to copy data efficiently
for (size_t k = 0; k < num_joints; ++k)
for (size_t j = 0; j < num_joints; ++j)
{
float *mat_ptr = pose_mat.ptr<float>(k);
const auto &joint = poses_2d[j][k];
mat_ptr[0] = joint[0];
mat_ptr[1] = joint[1];
mat_ptr[2] = joint[2];
for (size_t k = 0; k < 2; ++k)
{
mat_ptr[i * num_joints * 2 + j * 2 + k] = poses_2d[i][j][k];
}
}
poses.push_back(std::move(pose_mat));
}
for (size_t p = 0; p < poses.size(); ++p)
{
// Extract the (x, y) coordinates
cv::Mat points = poses[p].colRange(0, 2).clone();
points = points.reshape(2);
// Undistort the points
// Undistort all the points
points_mat = points_mat.reshape(2, static_cast<int>(points_mat.rows));
if (icam.cam.type == "fisheye")
{
cv::fisheye::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
cv::fisheye::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK);
}
else
{
cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
cv::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK);
}
points_mat = points_mat.reshape(1, static_cast<int>(points_mat.rows));
// Update the original poses with the undistorted points
points = points.reshape(1);
points.copyTo(poses[p].colRange(0, 2));
// Overwrite the old coordinates with the undistorted ones
for (size_t i = 0; i < num_persons; ++i)
{
for (size_t j = 0; j < num_joints; ++j)
{
for (size_t k = 0; k < 2; ++k)
{
poses_2d[i][j][k] = mat_ptr[i * num_joints * 2 + j * 2 + k];
}
}
}
// Mask out points that are far outside the image (points slightly outside are still valid)
float mask_offset = (width + height) / 20.0;
int num_joints = poses[p].rows;
for (int j = 0; j < num_joints; ++j)
for (size_t i = 0; i < num_persons; ++i)
{
float *poses_ptr = poses[p].ptr<float>(j);
float x = poses_ptr[0];
float y = poses_ptr[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_ptr[0] = 0.0;
poses_ptr[1] = 0.0;
poses_ptr[2] = 0.0;
}
}
}
// Convert mats to vectors
poses_2d.clear();
poses_2d.reserve(poses.size());
for (size_t i = 0; i < poses.size(); ++i)
{
std::vector<std::array<float, 3>> pose;
size_t num_joints = poses[i].rows;
pose.reserve(num_joints);
for (size_t j = 0; j < num_joints; ++j)
{
const float *mat_ptr = poses[i].ptr<float>(j);
std::array<float, 3> point;
for (size_t k = 0; k < 3; ++k)
if (poses_2d[i][j][0] < -mask_offset || poses_2d[i][j][0] >= width + mask_offset ||
poses_2d[i][j][1] < -mask_offset || poses_2d[i][j][1] >= height + mask_offset)
{
point[k] = mat_ptr[k];
poses_2d[i][j][0] = 0.0;
poses_2d[i][j][1] = 0.0;
poses_2d[i][j][2] = 0.0;
}
pose.push_back(point);
}
poses_2d.push_back(std::move(pose));
}
// Update the camera matrix