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

Fix a few conversion errors to make the results close to the python version.

Browse Source
This commit is contained in:
Daniel
2024-09-13 10:45:52 +02:00
parent b9949bfe4a
commit 91a502811f
1 changed files with 91 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
spt/triangulator.cpp
View File

@ -147,7 +147,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
for (size_t j = 0; j < num_persons; ++j)
{
std::vector<int> dims = {(int)num_joints, 3};
cv::Mat pose_mat(dims, CV_64F);
cv::Mat pose_mat = cv::Mat(dims, CV_64F);
for (size_t k = 0; k < num_joints; ++k)
{
for (size_t l = 0; l < 3; ++l)
@ -391,6 +391,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
std::vector<std::array<double, 4>> pose;
size_t num_joints = mat.rows;
pose.reserve(num_joints);
size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j)
{
@ -400,10 +401,18 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
point[k] = mat.at<double>(j, k);
}
pose.push_back(point);
if (point[3] > min_score)
{
num_valid++;
}
}
if (num_valid > 0)
{
poses_3d.push_back(std::move(pose));
}
}
last_poses_3d = all_merged_poses;
return poses_3d;
@ -443,7 +452,7 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
// Extract the (x, y) coordinates
std::vector<int> dims = {num_joints, 2};
cv::Mat points(dims, CV_64F);
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);
@ -500,7 +509,7 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
// Split up vector
std::vector<int> dimsA = {(int)num_joints, 3};
cv::Mat points3d(dimsA, CV_64F);
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);
@ -547,7 +556,7 @@ 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(dimsB, CV_64F);
cv::Mat bodies2D = cv::Mat(dimsB, CV_64F);
for (size_t i = 0; i < num_joints; ++i)
{
bodies2D.at<double>(i, 0) = uv.at<double>(i, 0);
@ -598,20 +607,17 @@ double TriangulatorInternal::calc_pose_score(
// Calculate scores
double iscale = (icam.cam.width + icam.cam.height) / 2;
cv::Mat scores = score_projection(
pose1, pose2, dist1, mask, iscale);
cv::Mat scores = score_projection(pose1, pose2, dist1, mask, iscale);
// Drop lowest scores
int drop_k = static_cast<int>(pose1.rows * 0.2);
std::sort(scores.begin<double>(), scores.end<double>());
// Calculate final score
double score = 0.0;
for (int i = drop_k; i < scores.rows; i++)
for (int i = 0; i < scores.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
{
score += scores.at<double>(i);
}
score /= (pose1.rows - drop_k);
}
score /= valid_count;
return score;
}
@ -626,7 +632,6 @@ cv::Mat TriangulatorInternal::score_projection(
double iscale)
{
double min_score = 0.1;
double penalty = iscale;
// Calculate error
cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2);
@ -637,16 +642,14 @@ cv::Mat TriangulatorInternal::score_projection(
error1.setTo(0.0, ~mask);
// Set errors of invisible reprojections to a high value
cv::Mat mask_invisible = (repro1.col(2) < min_score);
double penalty = iscale;
cv::Mat mask_invisible = (repro1.col(2) < min_score) & mask;
error1.setTo(penalty, mask_invisible);
// Scale error by image size and distance to the camera
error1 = cv::min(error1, (iscale / 4.0)) / iscale;
// Compute scaling factor
double dscale1 = std::sqrt(cv::mean(dists1).val[0] / 3.5);
// Scale errors
error1 = cv::max(0, cv::min(error1, (iscale / 4.0))) / iscale;
cv::Scalar mean_dist = cv::mean(dists1, mask);
double dscale1 = std::sqrt(mean_dist[0] / 3.5);
error1 *= dscale1;
// Convert errors to a score
@ -668,22 +671,22 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
double min_score = 0.1;
cv::Mat mask1a = (pose1.col(2) >= min_score);
cv::Mat mask2a = (pose2.col(2) >= min_score);
cv::Mat mask = mask1a & mask2a;
const cv::Mat mask = mask1a & mask2a;
// If too few joints are visible, return a low score
int num_visible = cv::countNonZero(mask);
if (num_visible < 3)
{
std::vector<int> dims = {(int)pose1.rows, 4};
cv::Mat pose3d(dims, CV_64F);
cv::Mat pose3d(dims, CV_64F, cv::Scalar(0));
double score = 0.0;
return std::make_pair(pose3d, score);
}
// Triangulate points
std::vector<int> dimsA = {2, num_visible};
cv::Mat points1(dimsA, CV_64F);
cv::Mat points2(dimsA, CV_64F);
cv::Mat points1 = cv::Mat(dimsA, CV_64F);
cv::Mat points2 = cv::Mat(dimsA, CV_64F);
int idx = 0;
for (int i = 0; i < pose1.rows; ++i)
{
@ -703,34 +706,55 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Create the 3D pose matrix
std::vector<int> dimsB = {(int)pose1.rows, 4};
cv::Mat pose3d(dimsB, CV_64F);
cv::Mat pose3d = cv::Mat(dimsB, CV_64F, cv::Scalar(0));
idx = 0;
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
{
pose3d.at<double>(i, 0) = points3d.at<double>(i, 0);
pose3d.at<double>(i, 1) = points3d.at<double>(i, 1);
pose3d.at<double>(i, 2) = points3d.at<double>(i, 2);
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;
idx++;
}
}
// Check if points are inside the room bounds
cv::Mat mean, mins, maxs;
cv::reduce(pose3d.colRange(0, 3), mean, 0, cv::REDUCE_AVG);
cv::reduce(pose3d.colRange(0, 3), mins, 0, cv::REDUCE_MIN);
cv::reduce(pose3d.colRange(0, 3), maxs, 0, cv::REDUCE_MAX);
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)
{
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));
}
}
}
for (int j = 0; j < 3; ++j)
{
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.at<double>(i) > center[i] + size[i] / 2 ||
mean.at<double>(i) < center[i] - size[i] / 2 ||
maxs.at<double>(i) > center[i] + size[i] / 2 + wdist ||
mins.at<double>(i) < center[i] - size[i] / 2 - wdist)
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)
{
// Very low score if outside room
for (int j = 0; j < pose1.rows; ++j)
{
pose3d.at<double>(j, 3) = 0.001;
}
return {pose3d, 0.001};
}
}
@ -764,15 +788,28 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Drop lowest scores
int drop_k = static_cast<int>(pose1.rows * 0.2);
std::sort(scores.begin<double>(), scores.end<double>());
std::vector<double> scores_vec;
for (int i = 0; i < pose1.rows; ++i)
{
if (mask.at<uchar>(i) > 0)
{
scores_vec.push_back(scores.at<double>(i));
}
}
std::sort(scores_vec.begin(), scores_vec.end());
// Calculate final score
double score = 0.0;
for (int i = drop_k; i < scores.rows; i++)
size_t items = 0;
for (size_t i = drop_k; i < scores_vec.size(); i++)
{
score += scores.at<double>(i);
score += scores_vec[i];
items++;
}
if (items > 0)
{
score /= (double)items;
}
score /= (pose1.rows - drop_k);
return std::make_pair(pose3d, score);
}
@ -902,24 +939,25 @@ 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());
cv::Mat sum_mask = cv::Mat::zeros(poses_3d[0].size(), CV_32S);
for (const auto &pose : poses_3d)
std::vector<int> sum_mask(num_joints, 0);
for (int i = 0; i < num_poses; ++i)
{
const cv::Mat &pose = poses_3d[i];
for (int j = 0; j < num_joints; ++j)
{
if (pose.at<double>(j, 3) > min_score)
{
sum_poses.row(j) += pose.row(j);
sum_mask.at<int>(j, 3) += 1;
sum_mask[j]++;
}
}
}
cv::Mat initial_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type());
cv::Mat initial_pose_3d = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type());
for (int j = 0; j < num_joints; ++j)
{
if (sum_mask.at<int>(j, 3) > 0)
if (sum_mask[j] > 0)
{
initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask.at<int>(j, 3);
initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j];
}
}
@ -995,7 +1033,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
// Compute the final pose
sum_poses = cv::Mat::zeros(sum_poses.size(), sum_poses.type());
sum_mask = cv::Mat::zeros(sum_mask.size(), CV_32S);
sum_mask = std::vector<int>(num_joints, 0);
for (int i = 0; i < num_poses; ++i)
{
for (int j = 0; j < num_joints; ++j)
@ -1003,16 +1041,16 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
if (mask.at<uchar>(i, j))
{
sum_poses.row(j) += poses_3d[i].row(j);
sum_mask.at<int>(j, 3) += 1;
sum_mask[j]++;
}
}
}
cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type());
for (int j = 0; j < num_joints; ++j)
{
if (sum_mask.at<int>(j, 3) > 0)
if (sum_mask[j] > 0)
{
final_pose_3d.row(j) = sum_poses.row(j) / sum_mask.at<int>(j, 3);
final_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j];
}
}