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

Better compiler optimizations.

Browse Source
This commit is contained in:
Daniel
2024-09-17 13:04:37 +02:00
parent fd0e872b33
commit 75840cf045
9 changed files with 266 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files
spt/camera.cpp
+1 -2
View File
@@ -7,7 +7,7 @@
// =================================================================================================
template <size_t M, size_t N>
static const std::string print_matrix(const std::array<std::array<double, N>, M> &matrix)
static const std::string print_matrix(const std::array<std::array<float, N>, M> &matrix)
{
std::ostringstream out;
out << "[";
@@ -73,4 +73,3 @@ std::ostream &operator<<(std::ostream &out, const Camera &cam)
out << cam.to_string();
return out;
}
spt/camera.hpp
+5 -5
View File
@@ -10,11 +10,11 @@
struct Camera
{
std::string name;
std::array<std::array<double, 3>, 3> K;
std::vector<double> DC;
std::array<std::array<double, 3>, 3> R;
std::array<std::array<double, 1>, 3> T;
std::array<std::array<double, 3>, 4> P;
std::array<std::array<float, 3>, 3> K;
std::vector<float> DC;
std::array<std::array<float, 3>, 3> R;
std::array<std::array<float, 1>, 3> T;
std::array<std::array<float, 3>, 4> P;
int width;
int height;
std::string type;
spt/interface.cpp
+4 -4
View File
@@ -4,17 +4,17 @@
// =================================================================================================
// =================================================================================================
Triangulator::Triangulator(double min_score)
Triangulator::Triangulator(float min_score)
{
this->triangulator = new TriangulatorInternal(min_score);
}
// =================================================================================================
std::vector<std::vector<std::array<double, 4>>> Triangulator::triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d,
std::vector<std::vector<std::array<float, 4>>> Triangulator::triangulate_poses(
const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras,
const std::array<std::array<double, 3>, 2> &roomparams,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names)
{
return this->triangulator->triangulate_poses(poses_2d, cameras, roomparams, joint_names);
spt/interface.hpp
+4 -4
View File
@@ -22,7 +22,7 @@ public:
* @param min_score Minimum score to consider a triangulated joint as valid.
*/
Triangulator(
double min_score = 0.95);
float min_score = 0.95);
/**
* Calculate a triangulation.
@@ -35,10 +35,10 @@ public:
*
* @return List of shape [persons, joints, 4], containing the 3D poses.
*/
std::vector<std::vector<std::array<double, 4>>> triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d,
std::vector<std::vector<std::array<float, 4>>> triangulate_poses(
const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras,
const std::array<std::array<double, 3>, 2> &roomparams,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names);
/** Reset the triangulator. */
spt/triangulator.cpp
+170 -170
View File
@@ -31,7 +31,7 @@
std::cout << " [";
for (int j = 0; j < cols; ++j)
{
std::cout << std::fixed << std::setprecision(3) << mat.at<double>(i, j);
std::cout << std::fixed << std::setprecision(3) << mat.at<float>(i, j);
if (j < cols - 1)
{
std::cout << ", ";
@@ -81,10 +81,10 @@ CameraInternal::CameraInternal(const Camera &cam)
this->cam = cam;
// Convert camera matrix to cv::Mat for OpenCV
K = cv::Mat(3, 3, CV_64FC1, const_cast<double *>(&cam.K[0][0])).clone();
DC = cv::Mat(cam.DC.size(), 1, CV_64FC1, const_cast<double *>(cam.DC.data())).clone();
R = cv::Mat(3, 3, CV_64FC1, const_cast<double *>(&cam.R[0][0])).clone();
T = cv::Mat(3, 1, CV_64FC1, const_cast<double *>(&cam.T[0][0])).clone();
K = cv::Mat(3, 3, CV_32FC1, const_cast<float *>(&cam.K[0][0])).clone();
DC = cv::Mat(cam.DC.size(), 1, CV_32FC1, const_cast<float *>(cam.DC.data())).clone();
R = cv::Mat(3, 3, CV_32FC1, const_cast<float *>(&cam.R[0][0])).clone();
T = cv::Mat(3, 1, CV_32FC1, const_cast<float *>(&cam.T[0][0])).clone();
}
// =================================================================================================
@@ -101,17 +101,17 @@ void CameraInternal::update_projection_matrix()
// =================================================================================================
// =================================================================================================
TriangulatorInternal::TriangulatorInternal(double min_score)
TriangulatorInternal::TriangulatorInternal(float min_score)
{
this->min_score = min_score;
}
// =================================================================================================
std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d,
std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate_poses(
const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras,
const std::array<std::array<double, 3>, 2> &roomparams,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names)
{
// Check inputs
@@ -148,12 +148,12 @@ 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 = cv::Mat(dims, CV_64F);
cv::Mat pose_mat = cv::Mat(dims, CV_32F);
// Use pointer to copy data efficiently
for (size_t k = 0; k < num_joints; ++k)
{
double *mat_ptr = pose_mat.ptr<double>(k);
float *mat_ptr = pose_mat.ptr<float>(k);
const auto &joint = poses_2d[i][j][k];
mat_ptr[0] = joint[0];
mat_ptr[1] = joint[1];
@@ -223,7 +223,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
// Check matches to old poses
double threshold = min_score - 0.2;
float threshold = min_score - 0.2;
std::map<size_t, std::map<size_t, std::vector<size_t>>> scored_pasts;
if (!last_poses_3d.empty())
{
@@ -381,7 +381,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
// Calculate pair scores
std::vector<std::pair<cv::Mat, double>> all_scored_poses;
std::vector<std::pair<cv::Mat, float>> all_scored_poses;
all_scored_poses.resize(all_pairs.size());
#pragma omp parallel for
for (size_t i = 0; i < all_pairs.size(); ++i)
@@ -473,20 +473,20 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
last_poses_3d = all_merged_poses;
// Convert to output format
std::vector<std::vector<std::array<double, 4>>> poses_3d;
std::vector<std::vector<std::array<float, 4>>> poses_3d;
poses_3d.reserve(all_merged_poses.size());
for (size_t i = 0; i < all_merged_poses.size(); ++i)
{
const auto &mat = all_merged_poses[i];
std::vector<std::array<double, 4>> pose;
std::vector<std::array<float, 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)
{
const double *mat_ptr = mat.ptr<double>(j);
std::array<double, 4> point;
const float *mat_ptr = mat.ptr<float>(j);
std::array<float, 4> point;
for (size_t k = 0; k < 4; ++k)
{
point[k] = mat_ptr[k];
@@ -540,13 +540,13 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
points.copyTo(poses[p].colRange(0, 2));
// Mask out points that are far outside the image (points slightly outside are still valid)
double mask_offset = (width + height) / 40.0;
float mask_offset = (width + height) / 40.0;
int num_joints = poses[p].rows;
for (int j = 0; j < num_joints; ++j)
{
double *poses_ptr = poses[p].ptr<double>(j);
double x = poses_ptr[0];
double y = poses_ptr[1];
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)
@@ -560,7 +560,7 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
// Update the camera matrix
icam.K = newK.clone();
icam.DC = cv::Mat::zeros(5, 1, CV_64F);
icam.DC = cv::Mat::zeros(5, 1, CV_32F);
}
// =================================================================================================
@@ -590,8 +590,8 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
// Set points behind the camera to zero
for (size_t j = 0; j < num_joints; ++j)
{
double *xyz_row_ptr = xyz.ptr<double>(j);
double z = xyz_row_ptr[2];
float *xyz_row_ptr = xyz.ptr<float>(j);
float z = xyz_row_ptr[2];
if (z <= 0)
{
xyz_row_ptr[0] = 0.0;
@@ -605,12 +605,12 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
if (calc_dists)
{
cv::multiply(xyz, xyz, dists);
cv::reduce(dists, dists, 1, cv::REDUCE_SUM, CV_64F);
cv::reduce(dists, dists, 1, cv::REDUCE_SUM, CV_32F);
cv::sqrt(dists, dists);
}
else
{
dists = cv::Mat::zeros(num_joints, 1, CV_64F);
dists = cv::Mat::zeros(num_joints, 1, CV_32F);
}
// Project points to image plane
@@ -622,18 +622,18 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
{
cv::Mat DCc = icam.DC.rowRange(0, 5);
cv::projectPoints(
xyz, cv::Mat::zeros(3, 1, CV_64F), cv::Mat::zeros(3, 1, CV_64F), icam.K, DCc, uv);
xyz, cv::Mat::zeros(3, 1, CV_32F), cv::Mat::zeros(3, 1, CV_32F), icam.K, DCc, uv);
}
uv = uv.reshape(1, {xyz.rows, 2});
// Add scores again
std::vector<int> dimsB = {(int)num_joints, 3};
cv::Mat bodies2D = cv::Mat(dimsB, CV_64F);
cv::Mat bodies2D = cv::Mat(dimsB, CV_32F);
for (size_t j = 0; j < num_joints; ++j)
{
double *bodies2D_row_ptr = bodies2D.ptr<double>(j);
const double *uv_row_ptr = uv.ptr<double>(j);
const double *bodies3D_row_ptr = body3D.ptr<double>(j);
float *bodies2D_row_ptr = bodies2D.ptr<float>(j);
const float *uv_row_ptr = uv.ptr<float>(j);
const float *bodies3D_row_ptr = body3D.ptr<float>(j);
bodies2D_row_ptr[0] = uv_row_ptr[0];
bodies2D_row_ptr[1] = uv_row_ptr[1];
@@ -643,9 +643,9 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
// Filter invalid projections
for (size_t j = 0; j < num_joints; ++j)
{
double *bodies2D_row_ptr = bodies2D.ptr<double>(j);
double x = bodies2D_row_ptr[0];
double y = bodies2D_row_ptr[1];
float *bodies2D_row_ptr = bodies2D.ptr<float>(j);
float x = bodies2D_row_ptr[0];
float y = bodies2D_row_ptr[1];
bool in_x = x >= 0.0 && x < icam.cam.width;
bool in_y = y >= 0.0 && y < icam.cam.height;
if (!in_x || !in_y)
@@ -666,13 +666,13 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
// =================================================================================================
double TriangulatorInternal::calc_pose_score(
float TriangulatorInternal::calc_pose_score(
const cv::Mat &pose1,
const cv::Mat &pose2,
const cv::Mat &dist1,
const CameraInternal &icam)
{
const double min_score = 0.1;
const float min_score = 0.1;
// Create mask for valid points
size_t num_joints = pose1.rows;
@@ -680,8 +680,8 @@ double TriangulatorInternal::calc_pose_score(
for (size_t i = 0; i < num_joints; ++i)
{
u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose1_ptr = pose1.ptr<double>(i);
const double *pose2_ptr = pose2.ptr<double>(i);
const float *pose1_ptr = pose1.ptr<float>(i);
const float *pose2_ptr = pose2.ptr<float>(i);
mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score);
}
@@ -694,18 +694,18 @@ double TriangulatorInternal::calc_pose_score(
}
// Calculate scores
double iscale = (icam.cam.width + icam.cam.height) / 2;
float iscale = (icam.cam.width + icam.cam.height) / 2;
cv::Mat scores = score_projection(pose1, pose2, dist1, mask, iscale);
// Drop lowest scores
size_t drop_k = static_cast<size_t>(pose1.rows * 0.2);
const size_t min_k = 3;
std::vector<double> scores_vec;
std::vector<float> scores_vec;
scores_vec.reserve(valid_count);
for (int i = 0; i < scores.rows; ++i)
{
const double *scores_ptr = scores.ptr<double>(i);
const uchar *mask_ptr = mask.ptr<uchar>(i);
const float *scores_ptr = scores.ptr<float>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
if (mask_ptr[0] > 0)
{
scores_vec.push_back(scores_ptr[0]);
@@ -720,12 +720,12 @@ double TriangulatorInternal::calc_pose_score(
}
// Calculate final score
double score = 0.0;
float score = 0.0;
size_t n_items = scores_vec.size();
if (n_items > 0)
{
double sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0);
score = sum_scores / static_cast<double>(n_items);
float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0);
score = sum_scores / static_cast<float>(n_items);
}
return score;
@@ -738,28 +738,28 @@ cv::Mat TriangulatorInternal::score_projection(
const cv::Mat &repro,
const cv::Mat &dists,
const cv::Mat &mask,
double iscale)
float iscale)
{
const double min_score = 0.1;
const float min_score = 0.1;
const size_t num_joints = pose.rows;
// Calculate error
cv::Mat error = cv::Mat::zeros(num_joints, 1, CV_64F);
cv::Mat error = cv::Mat::zeros(num_joints, 1, CV_32F);
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
const double *pose_ptr = pose.ptr<double>(i);
const double *repro_ptr = repro.ptr<double>(i);
double *error_ptr = error.ptr<double>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
const float *pose_ptr = pose.ptr<float>(i);
const float *repro_ptr = repro.ptr<float>(i);
float *error_ptr = error.ptr<float>(i);
if (mask_ptr)
{
double dx = pose_ptr[0] - repro_ptr[0];
double dy = pose_ptr[1] - repro_ptr[1];
double err = std::sqrt(dx * dx + dy * dy);
float dx = pose_ptr[0] - repro_ptr[0];
float dy = pose_ptr[1] - repro_ptr[1];
float err = std::sqrt(dx * dx + dy * dy);
// Set errors of invisible reprojections to a high value
double score = repro_ptr[2];
float score = repro_ptr[2];
if (score < min_score)
{
err = iscale;
@@ -770,24 +770,24 @@ cv::Mat TriangulatorInternal::score_projection(
}
// Scale error by image size
const double inv_iscale = 1.0 / iscale;
const double iscale_quarter = iscale / 4.0;
const float inv_iscale = 1.0 / iscale;
const float iscale_quarter = iscale / 4.0;
for (size_t i = 0; i < num_joints; ++i)
{
double *error_ptr = error.ptr<double>(i);
float *error_ptr = error.ptr<float>(i);
double err = error_ptr[0];
err = std::max(0.0, std::min(err, iscale_quarter)) * inv_iscale;
float err = error_ptr[0];
err = std::max(0.0f, std::min(err, iscale_quarter)) * inv_iscale;
error_ptr[0] = err;
}
// Scale error by distance to camera
double mean_dist = 0.0;
float mean_dist = 0.0;
int count = 0;
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
const double *dists_ptr = dists.ptr<double>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
const float *dists_ptr = dists.ptr<float>(i);
if (mask_ptr)
{
@@ -799,12 +799,12 @@ cv::Mat TriangulatorInternal::score_projection(
{
mean_dist /= count;
}
const double dscale = std::sqrt(mean_dist / 3.5);
const float dscale = std::sqrt(mean_dist / 3.5);
for (size_t i = 0; i < num_joints; ++i)
{
double *error_ptr = error.ptr<double>(i);
float *error_ptr = error.ptr<float>(i);
double err = error_ptr[0];
float err = error_ptr[0];
err *= dscale;
error_ptr[0] = err;
}
@@ -813,7 +813,7 @@ cv::Mat TriangulatorInternal::score_projection(
cv::Mat score = error;
for (size_t i = 0; i < num_joints; ++i)
{
double *score_ptr = score.ptr<double>(i);
float *score_ptr = score.ptr<float>(i);
score_ptr[0] = 1.0 / (1.0 + score_ptr[0] * 10.0);
}
@@ -822,15 +822,15 @@ cv::Mat TriangulatorInternal::score_projection(
// =================================================================================================
std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
std::pair<cv::Mat, float> TriangulatorInternal::triangulate_and_score(
const cv::Mat &pose1,
const cv::Mat &pose2,
const CameraInternal &cam1,
const CameraInternal &cam2,
const std::array<std::array<double, 3>, 2> &roomparams,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx)
{
const double min_score = 0.1;
const float min_score = 0.1;
const size_t num_joints = pose1.rows;
// Create mask for valid points
@@ -838,8 +838,8 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
for (size_t i = 0; i < num_joints; ++i)
{
u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose1_ptr = pose1.ptr<double>(i);
const double *pose2_ptr = pose2.ptr<double>(i);
const float *pose1_ptr = pose1.ptr<float>(i);
const float *pose2_ptr = pose2.ptr<float>(i);
mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score);
}
@@ -848,23 +848,23 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
int num_visible = cv::countNonZero(mask);
if (num_visible < 3)
{
cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_64F);
double score = 0.0;
cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_32F);
float score = 0.0;
return std::make_pair(pose3d, score);
}
// Extract coordinates of visible joints
std::vector<int> dims = {2, num_visible};
cv::Mat points1 = cv::Mat(dims, CV_64F);
cv::Mat points2 = cv::Mat(dims, CV_64F);
cv::Mat points1 = cv::Mat(dims, CV_32F);
cv::Mat points2 = cv::Mat(dims, CV_32F);
int idx = 0;
double *points1_ptr = points1.ptr<double>(0);
double *points2_ptr = points2.ptr<double>(0);
float *points1_ptr = points1.ptr<float>(0);
float *points2_ptr = points2.ptr<float>(0);
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
const double *pose1_ptr = pose1.ptr<double>(i);
const double *pose2_ptr = pose2.ptr<double>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
const float *pose1_ptr = pose1.ptr<float>(i);
const float *pose2_ptr = pose2.ptr<float>(i);
if (mask_ptr[0])
{
@@ -882,13 +882,13 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
cv::convertPointsFromHomogeneous(points4d_h.t(), points3d);
// Create the 3D pose matrix
cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_64F);
cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_32F);
idx = 0;
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
double *pose3d_ptr = pose3d.ptr<double>(i);
const double *points3d_ptr = points3d.ptr<double>(idx);
const u_char *mask_ptr = mask.ptr<u_char>(i);
float *pose3d_ptr = pose3d.ptr<float>(i);
const float *points3d_ptr = points3d.ptr<float>(idx);
if (mask_ptr[0])
{
@@ -901,11 +901,11 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
}
// Check if mean of the points is inside the room bounds
std::array<double, 3> mean = {0.0, 0.0, 0.0};
std::array<float, 3> mean = {0.0, 0.0, 0.0};
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
const double *pose3d_ptr = pose3d.ptr<double>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
const float *pose3d_ptr = pose3d.ptr<float>(i);
if (mask_ptr[0])
{
@@ -914,13 +914,13 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
mean[2] += pose3d_ptr[2];
}
}
double inv_num_vis = 1.0 / num_visible;
float inv_num_vis = 1.0 / num_visible;
for (int j = 0; j < 3; ++j)
{
mean[j] *= inv_num_vis;
}
const std::array<double, 3> &center = roomparams[0];
const std::array<double, 3> &size = roomparams[1];
const std::array<float, 3> &center = roomparams[0];
const std::array<float, 3> &size = roomparams[1];
for (int j = 0; j < 3; ++j)
{
if (mean[j] > center[j] + size[j] / 2.0 ||
@@ -929,7 +929,7 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Very low score if outside room
for (size_t i = 0; i < num_joints; ++i)
{
double *pose3d_ptr = pose3d.ptr<double>(i);
float *pose3d_ptr = pose3d.ptr<float>(i);
pose3d_ptr[3] = 0.001;
}
return {pose3d, 0.001};
@@ -947,7 +947,7 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
dists2 = dists2s[0];
// Calculate scores for each view
double iscale = (cam1.cam.width + cam1.cam.height) / 2.0;
float iscale = (cam1.cam.width + cam1.cam.height) / 2.0;
cv::Mat score1 = score_projection(pose1, repro1, dists1, mask, iscale);
cv::Mat score2 = score_projection(pose2, repro2, dists2, mask, iscale);
@@ -957,9 +957,9 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Add scores to 3D pose
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
double *pose3d_ptr = pose3d.ptr<double>(i);
const double *scores_ptr = scores.ptr<double>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
float *pose3d_ptr = pose3d.ptr<float>(i);
const float *scores_ptr = scores.ptr<float>(i);
if (mask_ptr[0])
{
@@ -968,11 +968,11 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
}
// Set scores outside the room to a low value
const double wdist = 0.1;
const float wdist = 0.1;
for (size_t i = 0; i < num_joints; ++i)
{
const uchar *mask_ptr = mask.ptr<uchar>(i);
double *pose3d_ptr = pose3d.ptr<double>(i);
const u_char *mask_ptr = mask.ptr<u_char>(i);
float *pose3d_ptr = pose3d.ptr<float>(i);
if (mask_ptr[0])
{
@@ -991,20 +991,20 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Filter clearly wrong limbs
if (!core_limbs_idx.empty())
{
const double max_length_sq = 0.9 * 0.9;
const float max_length_sq = 0.9 * 0.9;
for (size_t i = 0; i < core_limbs_idx.size(); ++i)
{
size_t limb1 = core_limbs_idx[i][0];
size_t limb2 = core_limbs_idx[i][1];
double *pose3d_ptr1 = pose3d.ptr<double>(limb1);
double *pose3d_ptr2 = pose3d.ptr<double>(limb2);
float *pose3d_ptr1 = pose3d.ptr<float>(limb1);
float *pose3d_ptr2 = pose3d.ptr<float>(limb2);
if (pose3d_ptr1[3] > min_score && pose3d_ptr2[3] > min_score)
{
double dx = pose3d_ptr1[0] - pose3d_ptr2[0];
double dy = pose3d_ptr1[1] - pose3d_ptr2[1];
double dz = pose3d_ptr1[2] - pose3d_ptr2[2];
double length_sq = dx * dx + dy * dy + dz * dz;
float dx = pose3d_ptr1[0] - pose3d_ptr2[0];
float dy = pose3d_ptr1[1] - pose3d_ptr2[1];
float dz = pose3d_ptr1[2] - pose3d_ptr2[2];
float length_sq = dx * dx + dy * dy + dz * dz;
if (length_sq > max_length_sq)
{
@@ -1017,10 +1017,10 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Drop lowest scores
size_t drop_k = static_cast<size_t>(num_joints * 0.2);
const size_t min_k = 3;
std::vector<double> scores_vec;
std::vector<float> scores_vec;
for (size_t i = 0; i < num_joints; ++i)
{
const double *pose3d_ptr = pose3d.ptr<double>(i);
const float *pose3d_ptr = pose3d.ptr<float>(i);
if (pose3d_ptr[3] > min_score)
{
scores_vec.push_back(pose3d_ptr[3]);
@@ -1035,12 +1035,12 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
}
// Calculate final score
double score = 0.0;
float score = 0.0;
size_t n_items = scores_vec.size();
if (n_items > 0)
{
double sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0);
score = sum_scores / static_cast<double>(n_items);
float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0);
score = sum_scores / static_cast<float>(n_items);
}
return std::make_pair(pose3d, score);
@@ -1050,11 +1050,11 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInternal::calc_grouping(
const std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
const std::vector<std::pair<cv::Mat, double>> &all_scored_poses,
double min_score)
const std::vector<std::pair<cv::Mat, float>> &all_scored_poses,
float min_score)
{
double max_center_dist_sq = 0.6 * 0.6;
double max_joint_avg_dist = 0.3;
float max_center_dist_sq = 0.6 * 0.6;
float max_joint_avg_dist = 0.3;
size_t num_pairs = all_pairs.size();
size_t num_joints = all_scored_poses[0].first.rows;
@@ -1069,9 +1069,9 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j)
{
const double *pose_3d_ptr = pose_3d.ptr<double>(j);
const float *pose_3d_ptr = pose_3d.ptr<float>(j);
double score = pose_3d_ptr[3];
float score = pose_3d_ptr[3];
if (score > min_score)
{
center.x += pose_3d_ptr[0];
@@ -1082,7 +1082,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
}
if (num_valid > 0)
{
double inv_num_valid = 1.0 / num_valid;
float inv_num_valid = 1.0 / num_valid;
center.x *= inv_num_valid;
center.y *= inv_num_valid;
center.z *= inv_num_valid;
@@ -1097,7 +1097,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
const cv::Point3d &center = centers[i];
double best_dist = std::numeric_limits<double>::infinity();
float best_dist = std::numeric_limits<float>::infinity();
int best_group = -1;
for (size_t j = 0; j < groups.size(); ++j)
@@ -1106,32 +1106,32 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
cv::Point3d &group_center = std::get<0>(group);
// Check if the center is close enough
double dx = group_center.x - center.x;
double dy = group_center.y - center.y;
double dz = group_center.z - center.z;
double center_dist_sq = dx * dx + dy * dy + dz * dz;
float dx = group_center.x - center.x;
float dy = group_center.y - center.y;
float dz = group_center.z - center.z;
float center_dist_sq = dx * dx + dy * dy + dz * dz;
if (center_dist_sq < max_center_dist_sq)
{
cv::Mat &group_pose = std::get<1>(group);
// Calculate average joint distance
double dist_sum = 0.0;
float dist_sum = 0.0;
size_t count = 0;
for (size_t row = 0; row < num_joints; ++row)
{
const double *pose_3d_ptr = pose_3d.ptr<double>(row);
const double *group_pose_ptr = group_pose.ptr<double>(row);
const float *pose_3d_ptr = pose_3d.ptr<float>(row);
const float *group_pose_ptr = group_pose.ptr<float>(row);
double score1 = pose_3d_ptr[3];
double score2 = group_pose_ptr[3];
float score1 = pose_3d_ptr[3];
float score2 = group_pose_ptr[3];
if (score1 > min_score && score2 > min_score)
{
double dx = pose_3d_ptr[0] - group_pose_ptr[0];
double dy = pose_3d_ptr[1] - group_pose_ptr[1];
double dz = pose_3d_ptr[2] - group_pose_ptr[2];
double dist_sq = dx * dx + dy * dy + dz * dz;
float dx = pose_3d_ptr[0] - group_pose_ptr[0];
float dy = pose_3d_ptr[1] - group_pose_ptr[1];
float dz = pose_3d_ptr[2] - group_pose_ptr[2];
float dist_sq = dx * dx + dy * dy + dz * dz;
dist_sum += std::sqrt(dist_sq);
count++;
}
@@ -1140,7 +1140,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
if (count > 0)
{
// Check if the average joint distance is close enough
double avg_dist = dist_sum / count;
float avg_dist = dist_sum / count;
if (avg_dist < max_joint_avg_dist && avg_dist < best_dist)
{
best_dist = avg_dist;
@@ -1164,8 +1164,8 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
cv::Mat &group_pose = std::get<1>(group);
std::vector<int> &group_indices = std::get<2>(group);
double n_elems = static_cast<double>(group_indices.size());
double inv_n1 = 1.0 / (n_elems + 1.0);
float n_elems = static_cast<float>(group_indices.size());
float inv_n1 = 1.0 / (n_elems + 1.0);
// Update group center
group_center.x = (group_center.x * n_elems + center.x) * inv_n1;
@@ -1175,8 +1175,8 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
// Update group pose
for (size_t row = 0; row < num_joints; ++row)
{
const double *pose_3d_ptr = pose_3d.ptr<double>(row);
double *group_pose_ptr = group_pose.ptr<double>(row);
const float *pose_3d_ptr = pose_3d.ptr<float>(row);
float *group_pose_ptr = group_pose.ptr<float>(row);
group_pose_ptr[0] = (group_pose_ptr[0] * n_elems + pose_3d_ptr[0]) * inv_n1;
group_pose_ptr[1] = (group_pose_ptr[1] * n_elems + pose_3d_ptr[1]) * inv_n1;
@@ -1192,14 +1192,14 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
// =================================================================================================
cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d, double min_score)
cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d, float min_score)
{
int num_poses = poses_3d.size();
int num_joints = poses_3d[0].rows;
// Merge poses to create initial pose
// Use only those triangulations with a high score
cv::Mat sum_poses = cv::Mat::zeros(num_joints, 4, CV_64F);
cv::Mat sum_poses = cv::Mat::zeros(num_joints, 4, CV_32F);
std::vector<int> sum_mask(num_joints, 0);
for (int i = 0; i < num_poses; ++i)
{
@@ -1207,10 +1207,10 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
for (int j = 0; j < num_joints; ++j)
{
const double *pose_ptr = pose.ptr<double>(j);
double *sum_ptr = sum_poses.ptr<double>(j);
const float *pose_ptr = pose.ptr<float>(j);
float *sum_ptr = sum_poses.ptr<float>(j);
double score = pose_ptr[3];
float score = pose_ptr[3];
if (score > min_score)
{
sum_ptr[0] += pose_ptr[0];
@@ -1221,15 +1221,15 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
}
}
}
cv::Mat initial_pose_3d = cv::Mat::zeros(num_joints, 4, CV_64F);
cv::Mat initial_pose_3d = cv::Mat::zeros(num_joints, 4, CV_32F);
for (int j = 0; j < num_joints; ++j)
{
if (sum_mask[j] > 0)
{
double *initial_ptr = initial_pose_3d.ptr<double>(j);
const double *sum_ptr = sum_poses.ptr<double>(j);
float *initial_ptr = initial_pose_3d.ptr<float>(j);
const float *sum_ptr = sum_poses.ptr<float>(j);
double inv_count = 1.0 / sum_mask[j];
float inv_count = 1.0 / sum_mask[j];
initial_ptr[0] = sum_ptr[0] * inv_count;
initial_ptr[1] = sum_ptr[1] * inv_count;
initial_ptr[2] = sum_ptr[2] * inv_count;
@@ -1243,8 +1243,8 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
int valid_joints = 0;
for (int j = 0; j < num_joints; ++j)
{
const double *initial_ptr = initial_pose_3d.ptr<double>(j);
double score = initial_ptr[3];
const float *initial_ptr = initial_pose_3d.ptr<float>(j);
float score = initial_ptr[3];
if (score > min_score)
{
@@ -1263,7 +1263,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
{
if (!jmask[j])
{
double *initial_ptr = initial_pose_3d.ptr<double>(j);
float *initial_ptr = initial_pose_3d.ptr<float>(j);
initial_ptr[0] = center.x;
initial_ptr[1] = center.y;
initial_ptr[2] = center.z;
@@ -1271,28 +1271,28 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
}
// Drop joints with low scores and filter outlying joints using distance threshold
double offset = 0.1;
double max_dist_sq = 1.2 * 1.2;
float offset = 0.1;
float max_dist_sq = 1.2 * 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);
cv::Mat distances = cv::Mat::zeros(num_poses, num_joints, CV_32F);
for (int i = 0; i < num_poses; ++i)
{
double *distances_ptr = distances.ptr<double>(i);
float *distances_ptr = distances.ptr<float>(i);
u_char *mask_ptr = mask.ptr<u_char>(i);
const cv::Mat &pose = poses_3d[i];
for (int j = 0; j < num_joints; ++j)
{
const double *initial_ptr = initial_pose_3d.ptr<double>(j);
const double *pose_ptr = pose.ptr<double>(j);
const float *initial_ptr = initial_pose_3d.ptr<float>(j);
const float *pose_ptr = pose.ptr<float>(j);
double dx = pose_ptr[0] - initial_ptr[0];
double dy = pose_ptr[1] - initial_ptr[1];
double dz = pose_ptr[2] - initial_ptr[2];
double dist_sq = dx * dx + dy * dy + dz * dz;
float dx = pose_ptr[0] - initial_ptr[0];
float dy = pose_ptr[1] - initial_ptr[1];
float dz = pose_ptr[2] - initial_ptr[2];
float dist_sq = dx * dx + dy * dy + dz * dz;
distances_ptr[j] = dist_sq;
double score = pose_ptr[3];
float score = pose_ptr[3];
if (dist_sq <= max_dist_sq && score > (min_score - offset))
{
mask_ptr[j] = 1;
@@ -1305,13 +1305,13 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
cv::Mat best_k_mask = cv::Mat::zeros(num_poses, num_joints, CV_8U);
for (int j = 0; j < num_joints; ++j)
{
std::vector<std::pair<double, int>> valid_indices;
std::vector<std::pair<float, int>> valid_indices;
valid_indices.reserve(num_poses);
for (int i = 0; i < num_poses; ++i)
{
if (mask.at<u_char>(i, j))
{
valid_indices.emplace_back(distances.at<double>(i, j), i);
valid_indices.emplace_back(distances.at<float>(i, j), i);
}
}
std::partial_sort(
@@ -1329,7 +1329,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
mask &= best_k_mask;
// Compute the final pose
sum_poses = cv::Mat::zeros(num_joints, 4, CV_64F);
sum_poses = cv::Mat::zeros(num_joints, 4, CV_32F);
sum_mask.assign(num_joints, 0);
for (int i = 0; i < num_poses; ++i)
{
@@ -1340,8 +1340,8 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
{
if (mask_row_ptr[j])
{
double *sum_ptr = sum_poses.ptr<double>(j);
const double *pose_ptr = pose.ptr<double>(j);
float *sum_ptr = sum_poses.ptr<float>(j);
const float *pose_ptr = pose.ptr<float>(j);
sum_ptr[0] += pose_ptr[0];
sum_ptr[1] += pose_ptr[1];
@@ -1352,15 +1352,15 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
}
}
}
cv::Mat final_pose_3d = cv::Mat::zeros(num_joints, 4, CV_64F);
cv::Mat final_pose_3d = cv::Mat::zeros(num_joints, 4, CV_32F);
for (int j = 0; j < num_joints; ++j)
{
if (sum_mask[j] > 0)
{
double *final_pose_ptr = final_pose_3d.ptr<double>(j);
const double *sum_ptr = sum_poses.ptr<double>(j);
float *final_pose_ptr = final_pose_3d.ptr<float>(j);
const float *sum_ptr = sum_poses.ptr<float>(j);
double inv_count = 1.0 / sum_mask[j];
float inv_count = 1.0 / sum_mask[j];
final_pose_ptr[0] = sum_ptr[0] * inv_count;
final_pose_ptr[1] = sum_ptr[1] * inv_count;
final_pose_ptr[2] = sum_ptr[2] * inv_count;
spt/triangulator.hpp
+12 -12
View File
@@ -31,18 +31,18 @@ public:
class TriangulatorInternal
{
public:
TriangulatorInternal(double min_score);
TriangulatorInternal(float min_score);
std::vector<std::vector<std::array<double, 4>>> triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d,
std::vector<std::vector<std::array<float, 4>>> triangulate_poses(
const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras,
const std::array<std::array<double, 3>, 2> &roomparams,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names);
void reset();
private:
double min_score;
float min_score;
const std::vector<std::string> core_joints = {
"shoulder_left",
"shoulder_right",
@@ -75,7 +75,7 @@ private:
std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(
const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists);
double calc_pose_score(
float calc_pose_score(
const cv::Mat &pose1,
const cv::Mat &pose2,
const cv::Mat &dist1,
@@ -86,20 +86,20 @@ private:
const cv::Mat &repro1,
const cv::Mat &dists1,
const cv::Mat &mask,
double iscale);
float iscale);
std::pair<cv::Mat, double> triangulate_and_score(
std::pair<cv::Mat, float> triangulate_and_score(
const cv::Mat &pose1,
const cv::Mat &pose2,
const CameraInternal &cam1,
const CameraInternal &cam2,
const std::array<std::array<double, 3>, 2> &roomparams,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> calc_grouping(
const std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
const std::vector<std::pair<cv::Mat, double>> &all_scored_poses,
double min_score);
const std::vector<std::pair<cv::Mat, float>> &all_scored_poses,
float min_score);
cv::Mat merge_group(const std::vector<cv::Mat> &poses_3d, double min_score);
cv::Mat merge_group(const std::vector<cv::Mat> &poses_3d, float min_score);
};