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

118
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)
{
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];
}
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
if (icam.cam.type == "fisheye")
{
cv::fisheye::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
}
else
{
cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
}
// Update the original poses with the undistorted points
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)
float mask_offset = (width + height) / 20.0;
int num_joints = poses[p].rows;
for (int j = 0; j < num_joints; ++j)
{
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)
for (size_t k = 0; k < 2; ++k)
{
point[k] = mat_ptr[k];
mat_ptr[i * num_joints * 2 + j * 2 + k] = poses_2d[i][j][k];
}
pose.push_back(point);
}
}
poses_2d.push_back(std::move(pose));
// 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_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK);
}
else
{
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));
// 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;
for (size_t i = 0; i < num_persons; ++i)
{
for (size_t j = 0; j < num_joints; ++j)
{
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)
{
poses_2d[i][j][0] = 0.0;
poses_2d[i][j][1] = 0.0;
poses_2d[i][j][2] = 0.0;
}
}
}
// Update the camera matrix