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
media/RESULTS.md
+53 -53
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.101639888540576, "avg_time_2d": 0.1016948051059369,
"avg_time_3d": 0.0018720938168030833, "avg_time_3d": 0.0012720597978310077,
"avg_fps": 9.660717312392508 "avg_fps": 9.711862169782414
} }
{ {
"person_nums": { "person_nums": {
@@ -299,33 +299,33 @@ Results of the model in various experiments on different datasets.
}, },
"mpjpe": { "mpjpe": {
"count": 477, "count": 477,
"mean": 0.048309, "mean": 0.048328,
"median": 0.04301, "median": 0.043186,
"std": 0.014754, "std": 0.01475,
"sem": 0.000676, "sem": 0.000676,
"min": 0.029586, "min": 0.029586,
"max": 0.106389, "max": 0.106122,
"recall-0.025": 0.0, "recall-0.025": 0.0,
"recall-0.05": 0.677149, "recall-0.05": 0.675052,
"recall-0.1": 0.989518, "recall-0.1": 0.989518,
"recall-0.15": 1.0, "recall-0.15": 1.0,
"recall-0.25": 1.0, "recall-0.25": 1.0,
"recall-0.5": 1.0, "recall-0.5": 1.0,
"num_labels": 477, "num_labels": 477,
"ap-0.025": 0.0, "ap-0.025": 0.0,
"ap-0.05": 0.342622, "ap-0.05": 0.340331,
"ap-0.1": 0.692726, "ap-0.1": 0.693196,
"ap-0.15": 0.703931, "ap-0.15": 0.704408,
"ap-0.25": 0.703931, "ap-0.25": 0.704408,
"ap-0.5": 0.703931 "ap-0.5": 0.704408
}, },
"head": { "head": {
"count": 477, "count": 477,
"mean": 0.05522, "mean": 0.055213,
"median": 0.049788, "median": 0.049788,
"std": 0.025981, "std": 0.025973,
"sem": 0.001191, "sem": 0.00119,
"min": 0.006374, "min": 0.006373,
"max": 0.147303, "max": 0.147303,
"recall-0.025": 0.077568, "recall-0.025": 0.077568,
"recall-0.05": 0.505241, "recall-0.05": 0.505241,
@@ -369,10 +369,10 @@ Results of the model in various experiments on different datasets.
}, },
"elbow_left": { "elbow_left": {
"count": 477, "count": 477,
"mean": 0.040846, "mean": 0.040792,
"median": 0.034194, "median": 0.034056,
"std": 0.025936, "std": 0.025923,
"sem": 0.001189, "sem": 0.001188,
"min": 0.005533, "min": 0.005533,
"max": 0.152226, "max": 0.152226,
"recall-0.025": 0.301887, "recall-0.025": 0.301887,
@@ -385,15 +385,15 @@ Results of the model in various experiments on different datasets.
}, },
"elbow_right": { "elbow_right": {
"count": 477, "count": 477,
"mean": 0.053511, "mean": 0.05355,
"median": 0.044068, "median": 0.044068,
"std": 0.041975, "std": 0.042016,
"sem": 0.001924, "sem": 0.001926,
"min": 0.002858, "min": 0.002858,
"max": 0.244706, "max": 0.244706,
"recall-0.025": 0.259958, "recall-0.025": 0.259958,
"recall-0.05": 0.559748, "recall-0.05": 0.559748,
"recall-0.1": 0.890985, "recall-0.1": 0.888889,
"recall-0.15": 0.958071, "recall-0.15": 0.958071,
"recall-0.25": 1.0, "recall-0.25": 1.0,
"recall-0.5": 1.0, "recall-0.5": 1.0,
@@ -401,10 +401,10 @@ Results of the model in various experiments on different datasets.
}, },
"wrist_left": { "wrist_left": {
"count": 477, "count": 477,
"mean": 0.061566, "mean": 0.061591,
"median": 0.053276, "median": 0.053276,
"std": 0.046136, "std": 0.046099,
"sem": 0.002115, "sem": 0.002113,
"min": 0.003745, "min": 0.003745,
"max": 0.359369, "max": 0.359369,
"recall-0.025": 0.12369, "recall-0.025": 0.12369,
@@ -417,14 +417,14 @@ Results of the model in various experiments on different datasets.
}, },
"wrist_right": { "wrist_right": {
"count": 477, "count": 477,
"mean": 0.058012, "mean": 0.058138,
"median": 0.05377, "median": 0.053924,
"std": 0.029967, "std": 0.030037,
"sem": 0.001374, "sem": 0.001377,
"min": 0.009013, "min": 0.009013,
"max": 0.202256, "max": 0.202256,
"recall-0.025": 0.109015, "recall-0.025": 0.109015,
"recall-0.05": 0.446541, "recall-0.05": 0.444444,
"recall-0.1": 0.897275, "recall-0.1": 0.897275,
"recall-0.15": 0.989518, "recall-0.15": 0.989518,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@@ -433,9 +433,9 @@ Results of the model in various experiments on different datasets.
}, },
"hip_left": { "hip_left": {
"count": 477, "count": 477,
"mean": 0.047812, "mean": 0.047816,
"median": 0.043075, "median": 0.043075,
"std": 0.02636, "std": 0.026357,
"sem": 0.001208, "sem": 0.001208,
"min": 0.005757, "min": 0.005757,
"max": 0.140667, "max": 0.140667,
@@ -449,14 +449,14 @@ Results of the model in various experiments on different datasets.
}, },
"hip_right": { "hip_right": {
"count": 477, "count": 477,
"mean": 0.057973, "mean": 0.058044,
"median": 0.056146, "median": 0.056365,
"std": 0.025504, "std": 0.025492,
"sem": 0.001169, "sem": 0.001168,
"min": 0.005189, "min": 0.005189,
"max": 0.136207, "max": 0.136207,
"recall-0.025": 0.109015, "recall-0.025": 0.109015,
"recall-0.05": 0.419287, "recall-0.05": 0.415094,
"recall-0.1": 0.930818, "recall-0.1": 0.930818,
"recall-0.15": 1.0, "recall-0.15": 1.0,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@@ -465,14 +465,14 @@ Results of the model in various experiments on different datasets.
}, },
"knee_left": { "knee_left": {
"count": 477, "count": 477,
"mean": 0.040006, "mean": 0.039973,
"median": 0.03745, "median": 0.03745,
"std": 0.024382, "std": 0.024369,
"sem": 0.001118, "sem": 0.001117,
"min": 0.005407, "min": 0.005407,
"max": 0.195502, "max": 0.195502,
"recall-0.025": 0.262055, "recall-0.025": 0.262055,
"recall-0.05": 0.763103, "recall-0.05": 0.765199,
"recall-0.1": 0.974843, "recall-0.1": 0.974843,
"recall-0.15": 0.989518, "recall-0.15": 0.989518,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@@ -497,14 +497,14 @@ Results of the model in various experiments on different datasets.
}, },
"ankle_left": { "ankle_left": {
"count": 477, "count": 477,
"mean": 0.037236, "mean": 0.03731,
"median": 0.031, "median": 0.03105,
"std": 0.030833, "std": 0.030824,
"sem": 0.001413, "sem": 0.001413,
"min": 0.001693, "min": 0.001693,
"max": 0.224454, "max": 0.224454,
"recall-0.025": 0.433962, "recall-0.025": 0.431866,
"recall-0.05": 0.821803, "recall-0.05": 0.819706,
"recall-0.1": 0.943396, "recall-0.1": 0.943396,
"recall-0.15": 0.983229, "recall-0.15": 0.983229,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@@ -513,10 +513,10 @@ Results of the model in various experiments on different datasets.
}, },
"ankle_right": { "ankle_right": {
"count": 477, "count": 477,
"mean": 0.041431, "mean": 0.041422,
"median": 0.031787, "median": 0.031787,
"std": 0.037989, "std": 0.038009,
"sem": 0.001741, "sem": 0.001742,
"min": 0.004747, "min": 0.004747,
"max": 0.298397, "max": 0.298397,
"recall-0.025": 0.312369, "recall-0.025": 0.312369,
@@ -530,7 +530,7 @@ Results of the model in various experiments on different datasets.
"joint_recalls": { "joint_recalls": {
"num_labels": 6201, "num_labels": 6201,
"recall-0.025": 0.21109, "recall-0.025": 0.21109,
"recall-0.05": 0.6199, "recall-0.05": 0.61893,
"recall-0.1": 0.9384, "recall-0.1": 0.9384,
"recall-0.15": 0.98742, "recall-0.15": 0.98742,
"recall-0.25": 0.99823, "recall-0.25": 0.99823,
spt/camera.cpp
+1 -2
View File
@@ -7,7 +7,7 @@
// ================================================================================================= // =================================================================================================
template <size_t M, size_t N> 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; std::ostringstream out;
out << "["; out << "[";
@@ -73,4 +73,3 @@ std::ostream &operator<<(std::ostream &out, const Camera &cam)
out << cam.to_string(); out << cam.to_string();
return out; return out;
} }
spt/camera.hpp
+5 -5
View File
@@ -10,11 +10,11 @@
struct Camera struct Camera
{ {
std::string name; std::string name;
std::array<std::array<double, 3>, 3> K; std::array<std::array<float, 3>, 3> K;
std::vector<double> DC; std::vector<float> DC;
std::array<std::array<double, 3>, 3> R; std::array<std::array<float, 3>, 3> R;
std::array<std::array<double, 1>, 3> T; std::array<std::array<float, 1>, 3> T;
std::array<std::array<double, 3>, 4> P; std::array<std::array<float, 3>, 4> P;
int width; int width;
int height; int height;
std::string type; 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); this->triangulator = new TriangulatorInternal(min_score);
} }
// ================================================================================================= // =================================================================================================
std::vector<std::vector<std::array<double, 4>>> Triangulator::triangulate_poses( std::vector<std::vector<std::array<float, 4>>> Triangulator::triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d, const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras, 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) const std::vector<std::string> &joint_names)
{ {
return this->triangulator->triangulate_poses(poses_2d, cameras, roomparams, 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. * @param min_score Minimum score to consider a triangulated joint as valid.
*/ */
Triangulator( Triangulator(
double min_score = 0.95); float min_score = 0.95);
/** /**
* Calculate a triangulation. * Calculate a triangulation.
@@ -35,10 +35,10 @@ public:
* *
* @return List of shape [persons, joints, 4], containing the 3D poses. * @return List of shape [persons, joints, 4], containing the 3D poses.
*/ */
std::vector<std::vector<std::array<double, 4>>> triangulate_poses( std::vector<std::vector<std::array<float, 4>>> triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d, const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras, 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); const std::vector<std::string> &joint_names);
/** Reset the triangulator. */ /** Reset the triangulator. */
spt/triangulator.cpp
+170 -170
View File
@@ -31,7 +31,7 @@
std::cout << " ["; std::cout << " [";
for (int j = 0; j < cols; ++j) 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) if (j < cols - 1)
{ {
std::cout << ", "; std::cout << ", ";
@@ -81,10 +81,10 @@ CameraInternal::CameraInternal(const Camera &cam)
this->cam = cam; this->cam = cam;
// Convert camera matrix to cv::Mat for OpenCV // Convert camera matrix to cv::Mat for OpenCV
K = cv::Mat(3, 3, CV_64FC1, const_cast<double *>(&cam.K[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_64FC1, const_cast<double *>(cam.DC.data())).clone(); DC = cv::Mat(cam.DC.size(), 1, CV_32FC1, const_cast<float *>(cam.DC.data())).clone();
R = cv::Mat(3, 3, CV_64FC1, const_cast<double *>(&cam.R[0][0])).clone(); R = cv::Mat(3, 3, CV_32FC1, const_cast<float *>(&cam.R[0][0])).clone();
T = cv::Mat(3, 1, CV_64FC1, const_cast<double *>(&cam.T[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; this->min_score = min_score;
} }
// ================================================================================================= // =================================================================================================
std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulate_poses( std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d, const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras, 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) const std::vector<std::string> &joint_names)
{ {
// Check inputs // 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) for (size_t j = 0; j < num_persons; ++j)
{ {
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_32F);
// Use pointer to copy data efficiently // Use pointer to copy data efficiently
for (size_t k = 0; k < num_joints; ++k) 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]; const auto &joint = poses_2d[i][j][k];
mat_ptr[0] = joint[0]; mat_ptr[0] = joint[0];
mat_ptr[1] = joint[1]; mat_ptr[1] = joint[1];
@@ -223,7 +223,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
} }
// Check matches to old poses // 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; std::map<size_t, std::map<size_t, std::vector<size_t>>> scored_pasts;
if (!last_poses_3d.empty()) if (!last_poses_3d.empty())
{ {
@@ -381,7 +381,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
} }
// Calculate pair scores // 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()); all_scored_poses.resize(all_pairs.size());
#pragma omp parallel for #pragma omp parallel for
for (size_t i = 0; i < all_pairs.size(); ++i) 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; 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<float, 4>>> poses_3d;
poses_3d.reserve(all_merged_poses.size()); poses_3d.reserve(all_merged_poses.size());
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];
std::vector<std::array<double, 4>> pose; std::vector<std::array<float, 4>> pose;
size_t num_joints = mat.rows; size_t num_joints = mat.rows;
pose.reserve(num_joints); pose.reserve(num_joints);
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)
{ {
const double *mat_ptr = mat.ptr<double>(j); const float *mat_ptr = mat.ptr<float>(j);
std::array<double, 4> point; std::array<float, 4> point;
for (size_t k = 0; k < 4; ++k) for (size_t k = 0; k < 4; ++k)
{ {
point[k] = mat_ptr[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)); points.copyTo(poses[p].colRange(0, 2));
// 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 mask_offset = (width + height) / 40.0; float mask_offset = (width + height) / 40.0;
int num_joints = poses[p].rows; int num_joints = poses[p].rows;
for (int j = 0; j < num_joints; ++j) for (int j = 0; j < num_joints; ++j)
{ {
double *poses_ptr = poses[p].ptr<double>(j); float *poses_ptr = poses[p].ptr<float>(j);
double x = poses_ptr[0]; float x = poses_ptr[0];
double y = poses_ptr[1]; float y = poses_ptr[1];
bool in_x = x >= -mask_offset && x < width + mask_offset; bool in_x = x >= -mask_offset && x < width + mask_offset;
bool in_y = y >= -mask_offset && y < height + mask_offset; bool in_y = y >= -mask_offset && y < height + mask_offset;
if (!in_x || !in_y) if (!in_x || !in_y)
@@ -560,7 +560,7 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
// Update the camera matrix // Update the camera matrix
icam.K = newK.clone(); 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 // Set points behind the camera to zero
for (size_t j = 0; j < num_joints; ++j) for (size_t j = 0; j < num_joints; ++j)
{ {
double *xyz_row_ptr = xyz.ptr<double>(j); float *xyz_row_ptr = xyz.ptr<float>(j);
double z = xyz_row_ptr[2]; float z = xyz_row_ptr[2];
if (z <= 0) if (z <= 0)
{ {
xyz_row_ptr[0] = 0.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) if (calc_dists)
{ {
cv::multiply(xyz, xyz, 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); cv::sqrt(dists, dists);
} }
else else
{ {
dists = cv::Mat::zeros(num_joints, 1, CV_64F); dists = cv::Mat::zeros(num_joints, 1, CV_32F);
} }
// Project points to image plane // 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::Mat DCc = icam.DC.rowRange(0, 5);
cv::projectPoints( 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}); uv = uv.reshape(1, {xyz.rows, 2});
// 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_32F);
for (size_t j = 0; j < num_joints; ++j) for (size_t j = 0; j < num_joints; ++j)
{ {
double *bodies2D_row_ptr = bodies2D.ptr<double>(j); float *bodies2D_row_ptr = bodies2D.ptr<float>(j);
const double *uv_row_ptr = uv.ptr<double>(j); const float *uv_row_ptr = uv.ptr<float>(j);
const double *bodies3D_row_ptr = body3D.ptr<double>(j); const float *bodies3D_row_ptr = body3D.ptr<float>(j);
bodies2D_row_ptr[0] = uv_row_ptr[0]; bodies2D_row_ptr[0] = uv_row_ptr[0];
bodies2D_row_ptr[1] = uv_row_ptr[1]; 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 // Filter invalid projections
for (size_t j = 0; j < num_joints; ++j) for (size_t j = 0; j < num_joints; ++j)
{ {
double *bodies2D_row_ptr = bodies2D.ptr<double>(j); float *bodies2D_row_ptr = bodies2D.ptr<float>(j);
double x = bodies2D_row_ptr[0]; float x = bodies2D_row_ptr[0];
double y = bodies2D_row_ptr[1]; float y = bodies2D_row_ptr[1];
bool in_x = x >= 0.0 && x < icam.cam.width; bool in_x = x >= 0.0 && x < icam.cam.width;
bool in_y = y >= 0.0 && y < icam.cam.height; bool in_y = y >= 0.0 && y < icam.cam.height;
if (!in_x || !in_y) 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 &pose1,
const cv::Mat &pose2, const cv::Mat &pose2,
const cv::Mat &dist1, const cv::Mat &dist1,
const CameraInternal &icam) const CameraInternal &icam)
{ {
const double min_score = 0.1; const float min_score = 0.1;
// Create mask for valid points // Create mask for valid points
size_t num_joints = pose1.rows; size_t num_joints = pose1.rows;
@@ -680,8 +680,8 @@ double TriangulatorInternal::calc_pose_score(
for (size_t i = 0; i < num_joints; ++i) for (size_t i = 0; i < num_joints; ++i)
{ {
u_char *mask_ptr = mask.ptr<u_char>(i); u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose1_ptr = pose1.ptr<double>(i); const float *pose1_ptr = pose1.ptr<float>(i);
const double *pose2_ptr = pose2.ptr<double>(i); const float *pose2_ptr = pose2.ptr<float>(i);
mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score); mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score);
} }
@@ -694,18 +694,18 @@ double TriangulatorInternal::calc_pose_score(
} }
// Calculate scores // 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); cv::Mat scores = score_projection(pose1, pose2, dist1, mask, iscale);
// Drop lowest scores // Drop lowest scores
size_t drop_k = static_cast<size_t>(pose1.rows * 0.2); size_t drop_k = static_cast<size_t>(pose1.rows * 0.2);
const size_t min_k = 3; const size_t min_k = 3;
std::vector<double> scores_vec; std::vector<float> scores_vec;
scores_vec.reserve(valid_count); scores_vec.reserve(valid_count);
for (int i = 0; i < scores.rows; ++i) for (int i = 0; i < scores.rows; ++i)
{ {
const double *scores_ptr = scores.ptr<double>(i); const float *scores_ptr = scores.ptr<float>(i);
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
if (mask_ptr[0] > 0) if (mask_ptr[0] > 0)
{ {
scores_vec.push_back(scores_ptr[0]); scores_vec.push_back(scores_ptr[0]);
@@ -720,12 +720,12 @@ double TriangulatorInternal::calc_pose_score(
} }
// Calculate final score // Calculate final score
double score = 0.0; float score = 0.0;
size_t n_items = scores_vec.size(); size_t n_items = scores_vec.size();
if (n_items > 0) if (n_items > 0)
{ {
double sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0);
score = sum_scores / static_cast<double>(n_items); score = sum_scores / static_cast<float>(n_items);
} }
return score; return score;
@@ -738,28 +738,28 @@ cv::Mat TriangulatorInternal::score_projection(
const cv::Mat &repro, const cv::Mat &repro,
const cv::Mat &dists, const cv::Mat &dists,
const cv::Mat &mask, 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; const size_t num_joints = pose.rows;
// Calculate error // 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) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose_ptr = pose.ptr<double>(i); const float *pose_ptr = pose.ptr<float>(i);
const double *repro_ptr = repro.ptr<double>(i); const float *repro_ptr = repro.ptr<float>(i);
double *error_ptr = error.ptr<double>(i); float *error_ptr = error.ptr<float>(i);
if (mask_ptr) if (mask_ptr)
{ {
double dx = pose_ptr[0] - repro_ptr[0]; float dx = pose_ptr[0] - repro_ptr[0];
double dy = pose_ptr[1] - repro_ptr[1]; float dy = pose_ptr[1] - repro_ptr[1];
double err = std::sqrt(dx * dx + dy * dy); float err = std::sqrt(dx * dx + dy * dy);
// Set errors of invisible reprojections to a high value // Set errors of invisible reprojections to a high value
double score = repro_ptr[2]; float score = repro_ptr[2];
if (score < min_score) if (score < min_score)
{ {
err = iscale; err = iscale;
@@ -770,24 +770,24 @@ cv::Mat TriangulatorInternal::score_projection(
} }
// Scale error by image size // Scale error by image size
const double inv_iscale = 1.0 / iscale; const float inv_iscale = 1.0 / iscale;
const double iscale_quarter = iscale / 4.0; const float iscale_quarter = iscale / 4.0;
for (size_t i = 0; i < num_joints; ++i) 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 = std::max(0.0, std::min(err, iscale_quarter)) * inv_iscale; err = std::max(0.0f, std::min(err, iscale_quarter)) * inv_iscale;
error_ptr[0] = err; error_ptr[0] = err;
} }
// Scale error by distance to camera // Scale error by distance to camera
double mean_dist = 0.0; float mean_dist = 0.0;
int count = 0; int count = 0;
for (size_t i = 0; i < num_joints; ++i) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
const double *dists_ptr = dists.ptr<double>(i); const float *dists_ptr = dists.ptr<float>(i);
if (mask_ptr) if (mask_ptr)
{ {
@@ -799,12 +799,12 @@ cv::Mat TriangulatorInternal::score_projection(
{ {
mean_dist /= count; 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) 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; err *= dscale;
error_ptr[0] = err; error_ptr[0] = err;
} }
@@ -813,7 +813,7 @@ cv::Mat TriangulatorInternal::score_projection(
cv::Mat score = error; cv::Mat score = error;
for (size_t i = 0; i < num_joints; ++i) 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); 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 &pose1,
const cv::Mat &pose2, const cv::Mat &pose2,
const CameraInternal &cam1, const CameraInternal &cam1,
const CameraInternal &cam2, 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 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; const size_t num_joints = pose1.rows;
// Create mask for valid points // 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) for (size_t i = 0; i < num_joints; ++i)
{ {
u_char *mask_ptr = mask.ptr<u_char>(i); u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose1_ptr = pose1.ptr<double>(i); const float *pose1_ptr = pose1.ptr<float>(i);
const double *pose2_ptr = pose2.ptr<double>(i); const float *pose2_ptr = pose2.ptr<float>(i);
mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score); 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); int num_visible = cv::countNonZero(mask);
if (num_visible < 3) if (num_visible < 3)
{ {
cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_64F); cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_32F);
double score = 0.0; float score = 0.0;
return std::make_pair(pose3d, score); return std::make_pair(pose3d, score);
} }
// Extract coordinates of visible joints // Extract coordinates of visible joints
std::vector<int> dims = {2, num_visible}; std::vector<int> dims = {2, num_visible};
cv::Mat points1 = cv::Mat(dims, CV_64F); cv::Mat points1 = cv::Mat(dims, CV_32F);
cv::Mat points2 = cv::Mat(dims, CV_64F); cv::Mat points2 = cv::Mat(dims, CV_32F);
int idx = 0; int idx = 0;
double *points1_ptr = points1.ptr<double>(0); float *points1_ptr = points1.ptr<float>(0);
double *points2_ptr = points2.ptr<double>(0); float *points2_ptr = points2.ptr<float>(0);
for (size_t i = 0; i < num_joints; ++i) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose1_ptr = pose1.ptr<double>(i); const float *pose1_ptr = pose1.ptr<float>(i);
const double *pose2_ptr = pose2.ptr<double>(i); const float *pose2_ptr = pose2.ptr<float>(i);
if (mask_ptr[0]) if (mask_ptr[0])
{ {
@@ -882,13 +882,13 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
cv::convertPointsFromHomogeneous(points4d_h.t(), points3d); cv::convertPointsFromHomogeneous(points4d_h.t(), points3d);
// Create the 3D pose matrix // 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; idx = 0;
for (size_t i = 0; i < num_joints; ++i) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
double *pose3d_ptr = pose3d.ptr<double>(i); float *pose3d_ptr = pose3d.ptr<float>(i);
const double *points3d_ptr = points3d.ptr<double>(idx); const float *points3d_ptr = points3d.ptr<float>(idx);
if (mask_ptr[0]) 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 // 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) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
const double *pose3d_ptr = pose3d.ptr<double>(i); const float *pose3d_ptr = pose3d.ptr<float>(i);
if (mask_ptr[0]) if (mask_ptr[0])
{ {
@@ -914,13 +914,13 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
mean[2] += pose3d_ptr[2]; 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) for (int j = 0; j < 3; ++j)
{ {
mean[j] *= inv_num_vis; mean[j] *= inv_num_vis;
} }
const std::array<double, 3> &center = roomparams[0]; const std::array<float, 3> &center = roomparams[0];
const std::array<double, 3> &size = roomparams[1]; const std::array<float, 3> &size = roomparams[1];
for (int j = 0; j < 3; ++j) for (int j = 0; j < 3; ++j)
{ {
if (mean[j] > center[j] + size[j] / 2.0 || 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 // Very low score if outside room
for (size_t i = 0; i < num_joints; ++i) 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; pose3d_ptr[3] = 0.001;
} }
return {pose3d, 0.001}; return {pose3d, 0.001};
@@ -947,7 +947,7 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
dists2 = dists2s[0]; dists2 = dists2s[0];
// Calculate scores for each view // 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 score1 = score_projection(pose1, repro1, dists1, mask, iscale);
cv::Mat score2 = score_projection(pose2, repro2, dists2, 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 // Add scores to 3D pose
for (size_t i = 0; i < num_joints; ++i) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
double *pose3d_ptr = pose3d.ptr<double>(i); float *pose3d_ptr = pose3d.ptr<float>(i);
const double *scores_ptr = scores.ptr<double>(i); const float *scores_ptr = scores.ptr<float>(i);
if (mask_ptr[0]) 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 // 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) for (size_t i = 0; i < num_joints; ++i)
{ {
const uchar *mask_ptr = mask.ptr<uchar>(i); const u_char *mask_ptr = mask.ptr<u_char>(i);
double *pose3d_ptr = pose3d.ptr<double>(i); float *pose3d_ptr = pose3d.ptr<float>(i);
if (mask_ptr[0]) if (mask_ptr[0])
{ {
@@ -991,20 +991,20 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Filter clearly wrong limbs // Filter clearly wrong limbs
if (!core_limbs_idx.empty()) 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) for (size_t i = 0; i < core_limbs_idx.size(); ++i)
{ {
size_t limb1 = core_limbs_idx[i][0]; size_t limb1 = core_limbs_idx[i][0];
size_t limb2 = core_limbs_idx[i][1]; size_t limb2 = core_limbs_idx[i][1];
double *pose3d_ptr1 = pose3d.ptr<double>(limb1); float *pose3d_ptr1 = pose3d.ptr<float>(limb1);
double *pose3d_ptr2 = pose3d.ptr<double>(limb2); float *pose3d_ptr2 = pose3d.ptr<float>(limb2);
if (pose3d_ptr1[3] > min_score && pose3d_ptr2[3] > min_score) if (pose3d_ptr1[3] > min_score && pose3d_ptr2[3] > min_score)
{ {
double dx = pose3d_ptr1[0] - pose3d_ptr2[0]; float dx = pose3d_ptr1[0] - pose3d_ptr2[0];
double dy = pose3d_ptr1[1] - pose3d_ptr2[1]; float dy = pose3d_ptr1[1] - pose3d_ptr2[1];
double dz = pose3d_ptr1[2] - pose3d_ptr2[2]; float dz = pose3d_ptr1[2] - pose3d_ptr2[2];
double length_sq = dx * dx + dy * dy + dz * dz; float length_sq = dx * dx + dy * dy + dz * dz;
if (length_sq > max_length_sq) if (length_sq > max_length_sq)
{ {
@@ -1017,10 +1017,10 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
// Drop lowest scores // Drop lowest scores
size_t drop_k = static_cast<size_t>(num_joints * 0.2); size_t drop_k = static_cast<size_t>(num_joints * 0.2);
const size_t min_k = 3; 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) 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) if (pose3d_ptr[3] > min_score)
{ {
scores_vec.push_back(pose3d_ptr[3]); scores_vec.push_back(pose3d_ptr[3]);
@@ -1035,12 +1035,12 @@ std::pair<cv::Mat, double> TriangulatorInternal::triangulate_and_score(
} }
// Calculate final score // Calculate final score
double score = 0.0; float score = 0.0;
size_t n_items = scores_vec.size(); size_t n_items = scores_vec.size();
if (n_items > 0) if (n_items > 0)
{ {
double sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0);
score = sum_scores / static_cast<double>(n_items); score = sum_scores / static_cast<float>(n_items);
} }
return std::make_pair(pose3d, score); 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( 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<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
const std::vector<std::pair<cv::Mat, double>> &all_scored_poses, const std::vector<std::pair<cv::Mat, float>> &all_scored_poses,
double min_score) float min_score)
{ {
double max_center_dist_sq = 0.6 * 0.6; float max_center_dist_sq = 0.6 * 0.6;
double max_joint_avg_dist = 0.3; float max_joint_avg_dist = 0.3;
size_t num_pairs = all_pairs.size(); size_t num_pairs = all_pairs.size();
size_t num_joints = all_scored_poses[0].first.rows; 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; size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j) 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) if (score > min_score)
{ {
center.x += pose_3d_ptr[0]; 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) 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.x *= inv_num_valid;
center.y *= inv_num_valid; center.y *= inv_num_valid;
center.z *= 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::Mat &pose_3d = all_scored_poses[i].first;
const cv::Point3d &center = centers[i]; 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; int best_group = -1;
for (size_t j = 0; j < groups.size(); ++j) 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); cv::Point3d &group_center = std::get<0>(group);
// Check if the center is close enough // Check if the center is close enough
double dx = group_center.x - center.x; float dx = group_center.x - center.x;
double dy = group_center.y - center.y; float dy = group_center.y - center.y;
double dz = group_center.z - center.z; float dz = group_center.z - center.z;
double center_dist_sq = dx * dx + dy * dy + dz * dz; float center_dist_sq = dx * dx + dy * dy + dz * dz;
if (center_dist_sq < max_center_dist_sq) if (center_dist_sq < max_center_dist_sq)
{ {
cv::Mat &group_pose = std::get<1>(group); cv::Mat &group_pose = std::get<1>(group);
// Calculate average joint distance // Calculate average joint distance
double dist_sum = 0.0; float dist_sum = 0.0;
size_t count = 0; size_t count = 0;
for (size_t row = 0; row < num_joints; ++row) for (size_t row = 0; row < num_joints; ++row)
{ {
const double *pose_3d_ptr = pose_3d.ptr<double>(row); const float *pose_3d_ptr = pose_3d.ptr<float>(row);
const double *group_pose_ptr = group_pose.ptr<double>(row); const float *group_pose_ptr = group_pose.ptr<float>(row);
double score1 = pose_3d_ptr[3]; float score1 = pose_3d_ptr[3];
double score2 = group_pose_ptr[3]; float score2 = group_pose_ptr[3];
if (score1 > min_score && score2 > min_score) if (score1 > min_score && score2 > min_score)
{ {
double dx = pose_3d_ptr[0] - group_pose_ptr[0]; float dx = pose_3d_ptr[0] - group_pose_ptr[0];
double dy = pose_3d_ptr[1] - group_pose_ptr[1]; float dy = pose_3d_ptr[1] - group_pose_ptr[1];
double dz = pose_3d_ptr[2] - group_pose_ptr[2]; float dz = pose_3d_ptr[2] - group_pose_ptr[2];
double dist_sq = dx * dx + dy * dy + dz * dz; float dist_sq = dx * dx + dy * dy + dz * dz;
dist_sum += std::sqrt(dist_sq); dist_sum += std::sqrt(dist_sq);
count++; count++;
} }
@@ -1140,7 +1140,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
if (count > 0) if (count > 0)
{ {
// Check if the average joint distance is close enough // 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) if (avg_dist < max_joint_avg_dist && avg_dist < best_dist)
{ {
best_dist = avg_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); cv::Mat &group_pose = std::get<1>(group);
std::vector<int> &group_indices = std::get<2>(group); std::vector<int> &group_indices = std::get<2>(group);
double n_elems = static_cast<double>(group_indices.size()); float n_elems = static_cast<float>(group_indices.size());
double inv_n1 = 1.0 / (n_elems + 1.0); float inv_n1 = 1.0 / (n_elems + 1.0);
// Update group center // Update group center
group_center.x = (group_center.x * n_elems + center.x) * inv_n1; 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 // Update group pose
for (size_t row = 0; row < num_joints; ++row) for (size_t row = 0; row < num_joints; ++row)
{ {
const double *pose_3d_ptr = pose_3d.ptr<double>(row); const float *pose_3d_ptr = pose_3d.ptr<float>(row);
double *group_pose_ptr = group_pose.ptr<double>(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[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; 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_poses = poses_3d.size();
int num_joints = poses_3d[0].rows; int num_joints = poses_3d[0].rows;
// 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(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); std::vector<int> sum_mask(num_joints, 0);
for (int i = 0; i < num_poses; ++i) 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) for (int j = 0; j < num_joints; ++j)
{ {
const double *pose_ptr = pose.ptr<double>(j); const float *pose_ptr = pose.ptr<float>(j);
double *sum_ptr = sum_poses.ptr<double>(j); float *sum_ptr = sum_poses.ptr<float>(j);
double score = pose_ptr[3]; float score = pose_ptr[3];
if (score > min_score) if (score > min_score)
{ {
sum_ptr[0] += pose_ptr[0]; 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) for (int j = 0; j < num_joints; ++j)
{ {
if (sum_mask[j] > 0) if (sum_mask[j] > 0)
{ {
double *initial_ptr = initial_pose_3d.ptr<double>(j); float *initial_ptr = initial_pose_3d.ptr<float>(j);
const double *sum_ptr = sum_poses.ptr<double>(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[0] = sum_ptr[0] * inv_count;
initial_ptr[1] = sum_ptr[1] * inv_count; initial_ptr[1] = sum_ptr[1] * inv_count;
initial_ptr[2] = sum_ptr[2] * 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; int valid_joints = 0;
for (int j = 0; j < num_joints; ++j) for (int j = 0; j < num_joints; ++j)
{ {
const double *initial_ptr = initial_pose_3d.ptr<double>(j); const float *initial_ptr = initial_pose_3d.ptr<float>(j);
double score = initial_ptr[3]; float score = initial_ptr[3];
if (score > min_score) if (score > min_score)
{ {
@@ -1263,7 +1263,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
{ {
if (!jmask[j]) 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[0] = center.x;
initial_ptr[1] = center.y; initial_ptr[1] = center.y;
initial_ptr[2] = center.z; 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 // Drop joints with low scores and filter outlying joints using distance threshold
double offset = 0.1; float offset = 0.1;
double max_dist_sq = 1.2 * 1.2; float max_dist_sq = 1.2 * 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_32F);
for (int i = 0; i < num_poses; ++i) 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); u_char *mask_ptr = mask.ptr<u_char>(i);
const cv::Mat &pose = poses_3d[i]; const cv::Mat &pose = poses_3d[i];
for (int j = 0; j < num_joints; ++j) for (int j = 0; j < num_joints; ++j)
{ {
const double *initial_ptr = initial_pose_3d.ptr<double>(j); const float *initial_ptr = initial_pose_3d.ptr<float>(j);
const double *pose_ptr = pose.ptr<double>(j); const float *pose_ptr = pose.ptr<float>(j);
double dx = pose_ptr[0] - initial_ptr[0]; float dx = pose_ptr[0] - initial_ptr[0];
double dy = pose_ptr[1] - initial_ptr[1]; float dy = pose_ptr[1] - initial_ptr[1];
double dz = pose_ptr[2] - initial_ptr[2]; float dz = pose_ptr[2] - initial_ptr[2];
double dist_sq = dx * dx + dy * dy + dz * dz; float dist_sq = dx * dx + dy * dy + dz * dz;
distances_ptr[j] = dist_sq; 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)) if (dist_sq <= max_dist_sq && score > (min_score - offset))
{ {
mask_ptr[j] = 1; 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); cv::Mat best_k_mask = cv::Mat::zeros(num_poses, num_joints, CV_8U);
for (int j = 0; j < num_joints; ++j) 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); valid_indices.reserve(num_poses);
for (int i = 0; i < num_poses; ++i) for (int i = 0; i < num_poses; ++i)
{ {
if (mask.at<u_char>(i, j)) 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( std::partial_sort(
@@ -1329,7 +1329,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
mask &= best_k_mask; mask &= best_k_mask;
// Compute the final pose // 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); sum_mask.assign(num_joints, 0);
for (int i = 0; i < num_poses; ++i) 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]) if (mask_row_ptr[j])
{ {
double *sum_ptr = sum_poses.ptr<double>(j); float *sum_ptr = sum_poses.ptr<float>(j);
const double *pose_ptr = pose.ptr<double>(j); const float *pose_ptr = pose.ptr<float>(j);
sum_ptr[0] += pose_ptr[0]; sum_ptr[0] += pose_ptr[0];
sum_ptr[1] += pose_ptr[1]; 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) for (int j = 0; j < num_joints; ++j)
{ {
if (sum_mask[j] > 0) if (sum_mask[j] > 0)
{ {
double *final_pose_ptr = final_pose_3d.ptr<double>(j); float *final_pose_ptr = final_pose_3d.ptr<float>(j);
const double *sum_ptr = sum_poses.ptr<double>(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[0] = sum_ptr[0] * inv_count;
final_pose_ptr[1] = sum_ptr[1] * inv_count; final_pose_ptr[1] = sum_ptr[1] * inv_count;
final_pose_ptr[2] = sum_ptr[2] * 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 class TriangulatorInternal
{ {
public: public:
TriangulatorInternal(double min_score); TriangulatorInternal(float min_score);
std::vector<std::vector<std::array<double, 4>>> triangulate_poses( std::vector<std::vector<std::array<float, 4>>> triangulate_poses(
const std::vector<std::vector<std::vector<std::array<double, 3>>>> &poses_2d, const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
const std::vector<Camera> &cameras, 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); const std::vector<std::string> &joint_names);
void reset(); void reset();
private: private:
double min_score; float min_score;
const std::vector<std::string> core_joints = { const std::vector<std::string> core_joints = {
"shoulder_left", "shoulder_left",
"shoulder_right", "shoulder_right",
@@ -75,7 +75,7 @@ private:
std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses( std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(
const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists); 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 &pose1,
const cv::Mat &pose2, const cv::Mat &pose2,
const cv::Mat &dist1, const cv::Mat &dist1,
@@ -86,20 +86,20 @@ private:
const cv::Mat &repro1, const cv::Mat &repro1,
const cv::Mat &dists1, const cv::Mat &dists1,
const cv::Mat &mask, 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 &pose1,
const cv::Mat &pose2, const cv::Mat &pose2,
const CameraInternal &cam1, const CameraInternal &cam1,
const CameraInternal &cam2, 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 std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> calc_grouping( 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<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
const std::vector<std::pair<cv::Mat, double>> &all_scored_poses, const std::vector<std::pair<cv::Mat, float>> &all_scored_poses,
double min_score); 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);
}; };
swig/Makefile
+8 -8
View File
@@ -1,13 +1,13 @@
# Standard compile options for the c++ executable # Standard compile options for the C++ executable
FLAGS = -fPIC FLAGS = -fPIC -O3 -march=native -Wall -Werror -flto -fopenmp -fopenmp-simd
# The python interface through swig # The Python interface through SWIG
PYTHONI = -I/usr/include/python3.8/ PYTHONI = -I/usr/include/python3.8/
PYTHONL = -Xlinker -export-dynamic PYTHONL = -Xlinker -export-dynamic
# Default super-target # Default super-target
all: all:
cd ../spt/ && g++ -fPIC -std=c++2a -Wall -fopenmp -I/usr/include/opencv4 -c *.cpp ; cd ../swig/ cd ../spt/ && g++ $(FLAGS) -std=c++2a -I/usr/include/opencv4 -c *.cpp ; cd ../swig/
swig -c++ -python -keyword -o spt_wrap.cxx spt.i swig -c++ -python -keyword -o spt_wrap.cxx spt.i
g++ $(FLAGS) $(PYTHONI) -fopenmp -I/usr/include/opencv4 -c spt_wrap.cxx -o spt_wrap.o g++ $(FLAGS) $(PYTHONI) -c spt_wrap.cxx -o spt_wrap.o
g++ $(PYTHONL) $(LIBFLAGS) -fopenmp -shared ../spt/*.o spt_wrap.o -lopencv_core -lopencv_imgproc -lopencv_calib3d -o _spt.so g++ $(FLAGS) $(PYTHONL) -shared ../spt/*.o spt_wrap.o -lopencv_core -lopencv_imgproc -lopencv_calib3d -o _spt.so
swig/spt.i
+9 -9
View File
@@ -13,16 +13,16 @@
// Instantiate templates used by example // Instantiate templates used by example
// If the template is too nested (>2), parts of it need to be declared as well // If the template is too nested (>2), parts of it need to be declared as well
namespace std { namespace std {
%template(DoubleMatrix_3x3) array<array<double, 3>, 3>; %template(FloatMatrix_3x3) array<array<float, 3>, 3>;
%template(VectorDouble) vector<double>; %template(VectorFloat) vector<float>;
%template(DoubleMatrix_3x1) array<array<double, 1>, 3>; %template(FloatMatrix_3x1) array<array<float, 1>, 3>;
%template(DoubleMatrix_3x4) array<array<double, 3>, 4>; %template(FloatMatrix_3x4) array<array<float, 3>, 4>;
%template(Matrix_Jx4) vector<array<double, 4>>; %template(Matrix_Jx4) vector<array<float, 4>>;
%template(Matrix_NxJx4) vector<vector<array<double, 4>>>; %template(Matrix_NxJx4) vector<vector<array<float, 4>>>;
%template(Matrix_Jx3) vector<array<double, 3>>; %template(Matrix_Jx3) vector<array<float, 3>>;
%template(Matrix_VxNxJx3) vector<vector<vector<array<double, 3>>>>; %template(Matrix_VxNxJx3) vector<vector<vector<array<float, 3>>>>;
%template(VectorCamera) vector<Camera>; %template(VectorCamera) vector<Camera>;
%template(DoubleMatrix_2x3) array<array<double, 3>, 2>; %template(FloatMatrix_2x3) array<array<float, 3>, 2>;
%template(VectorString) vector<std::string>; %template(VectorString) vector<std::string>;
} }