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

106
media/RESULTS.md
View File

@ -278,9 +278,9 @@ Results of the model in various experiments on different datasets.
(duration 00:00:56)
```json
{
"avg_time_2d": 0.101639888540576,
"avg_time_3d": 0.0018720938168030833,
"avg_fps": 9.660717312392508
"avg_time_2d": 0.1016948051059369,
"avg_time_3d": 0.0012720597978310077,
"avg_fps": 9.711862169782414
}
{
"person_nums": {
@ -299,33 +299,33 @@ Results of the model in various experiments on different datasets.
},
"mpjpe": {
"count": 477,
"mean": 0.048309,
"median": 0.04301,
"std": 0.014754,
"mean": 0.048328,
"median": 0.043186,
"std": 0.01475,
"sem": 0.000676,
"min": 0.029586,
"max": 0.106389,
"max": 0.106122,
"recall-0.025": 0.0,
"recall-0.05": 0.677149,
"recall-0.05": 0.675052,
"recall-0.1": 0.989518,
"recall-0.15": 1.0,
"recall-0.25": 1.0,
"recall-0.5": 1.0,
"num_labels": 477,
"ap-0.025": 0.0,
"ap-0.05": 0.342622,
"ap-0.1": 0.692726,
"ap-0.15": 0.703931,
"ap-0.25": 0.703931,
"ap-0.5": 0.703931
"ap-0.05": 0.340331,
"ap-0.1": 0.693196,
"ap-0.15": 0.704408,
"ap-0.25": 0.704408,
"ap-0.5": 0.704408
},
"head": {
"count": 477,
"mean": 0.05522,
"mean": 0.055213,
"median": 0.049788,
"std": 0.025981,
"sem": 0.001191,
"min": 0.006374,
"std": 0.025973,
"sem": 0.00119,
"min": 0.006373,
"max": 0.147303,
"recall-0.025": 0.077568,
"recall-0.05": 0.505241,
@ -369,10 +369,10 @@ Results of the model in various experiments on different datasets.
},
"elbow_left": {
"count": 477,
"mean": 0.040846,
"median": 0.034194,
"std": 0.025936,
"sem": 0.001189,
"mean": 0.040792,
"median": 0.034056,
"std": 0.025923,
"sem": 0.001188,
"min": 0.005533,
"max": 0.152226,
"recall-0.025": 0.301887,
@ -385,15 +385,15 @@ Results of the model in various experiments on different datasets.
},
"elbow_right": {
"count": 477,
"mean": 0.053511,
"mean": 0.05355,
"median": 0.044068,
"std": 0.041975,
"sem": 0.001924,
"std": 0.042016,
"sem": 0.001926,
"min": 0.002858,
"max": 0.244706,
"recall-0.025": 0.259958,
"recall-0.05": 0.559748,
"recall-0.1": 0.890985,
"recall-0.1": 0.888889,
"recall-0.15": 0.958071,
"recall-0.25": 1.0,
"recall-0.5": 1.0,
@ -401,10 +401,10 @@ Results of the model in various experiments on different datasets.
},
"wrist_left": {
"count": 477,
"mean": 0.061566,
"mean": 0.061591,
"median": 0.053276,
"std": 0.046136,
"sem": 0.002115,
"std": 0.046099,
"sem": 0.002113,
"min": 0.003745,
"max": 0.359369,
"recall-0.025": 0.12369,
@ -417,14 +417,14 @@ Results of the model in various experiments on different datasets.
},
"wrist_right": {
"count": 477,
"mean": 0.058012,
"median": 0.05377,
"std": 0.029967,
"sem": 0.001374,
"mean": 0.058138,
"median": 0.053924,
"std": 0.030037,
"sem": 0.001377,
"min": 0.009013,
"max": 0.202256,
"recall-0.025": 0.109015,
"recall-0.05": 0.446541,
"recall-0.05": 0.444444,
"recall-0.1": 0.897275,
"recall-0.15": 0.989518,
"recall-0.25": 1.0,
@ -433,9 +433,9 @@ Results of the model in various experiments on different datasets.
},
"hip_left": {
"count": 477,
"mean": 0.047812,
"mean": 0.047816,
"median": 0.043075,
"std": 0.02636,
"std": 0.026357,
"sem": 0.001208,
"min": 0.005757,
"max": 0.140667,
@ -449,14 +449,14 @@ Results of the model in various experiments on different datasets.
},
"hip_right": {
"count": 477,
"mean": 0.057973,
"median": 0.056146,
"std": 0.025504,
"sem": 0.001169,
"mean": 0.058044,
"median": 0.056365,
"std": 0.025492,
"sem": 0.001168,
"min": 0.005189,
"max": 0.136207,
"recall-0.025": 0.109015,
"recall-0.05": 0.419287,
"recall-0.05": 0.415094,
"recall-0.1": 0.930818,
"recall-0.15": 1.0,
"recall-0.25": 1.0,
@ -465,14 +465,14 @@ Results of the model in various experiments on different datasets.
},
"knee_left": {
"count": 477,
"mean": 0.040006,
"mean": 0.039973,
"median": 0.03745,
"std": 0.024382,
"sem": 0.001118,
"std": 0.024369,
"sem": 0.001117,
"min": 0.005407,
"max": 0.195502,
"recall-0.025": 0.262055,
"recall-0.05": 0.763103,
"recall-0.05": 0.765199,
"recall-0.1": 0.974843,
"recall-0.15": 0.989518,
"recall-0.25": 1.0,
@ -497,14 +497,14 @@ Results of the model in various experiments on different datasets.
},
"ankle_left": {
"count": 477,
"mean": 0.037236,
"median": 0.031,
"std": 0.030833,
"mean": 0.03731,
"median": 0.03105,
"std": 0.030824,
"sem": 0.001413,
"min": 0.001693,
"max": 0.224454,
"recall-0.025": 0.433962,
"recall-0.05": 0.821803,
"recall-0.025": 0.431866,
"recall-0.05": 0.819706,
"recall-0.1": 0.943396,
"recall-0.15": 0.983229,
"recall-0.25": 1.0,
@ -513,10 +513,10 @@ Results of the model in various experiments on different datasets.
},
"ankle_right": {
"count": 477,
"mean": 0.041431,
"mean": 0.041422,
"median": 0.031787,
"std": 0.037989,
"sem": 0.001741,
"std": 0.038009,
"sem": 0.001742,
"min": 0.004747,
"max": 0.298397,
"recall-0.025": 0.312369,
@ -530,7 +530,7 @@ Results of the model in various experiments on different datasets.
"joint_recalls": {
"num_labels": 6201,
"recall-0.025": 0.21109,
"recall-0.05": 0.6199,
"recall-0.05": 0.61893,
"recall-0.1": 0.9384,
"recall-0.15": 0.98742,
"recall-0.25": 0.99823,

3
spt/camera.cpp
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;
}

10
spt/camera.hpp
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;

8
spt/interface.cpp
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);

8
spt/interface.hpp
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. */

340
spt/triangulator.cpp
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;

24
spt/triangulator.hpp
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);
};

12
swig/Makefile
View File

@ -1,13 +1,13 @@
# Standard compile options for the c++ executable
FLAGS = -fPIC
# Standard compile options for the C++ executable
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/
PYTHONL = -Xlinker -export-dynamic
# Default super-target
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
g++ $(FLAGS) $(PYTHONI) -fopenmp -I/usr/include/opencv4 -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) $(PYTHONI) -c spt_wrap.cxx -o spt_wrap.o
g++ $(FLAGS) $(PYTHONL) -shared ../spt/*.o spt_wrap.o -lopencv_core -lopencv_imgproc -lopencv_calib3d -o _spt.so

18
swig/spt.i
View File

@ -13,16 +13,16 @@
// Instantiate templates used by example
// If the template is too nested (>2), parts of it need to be declared as well
namespace std {
%template(DoubleMatrix_3x3) array<array<double, 3>, 3>;
%template(VectorDouble) vector<double>;
%template(DoubleMatrix_3x1) array<array<double, 1>, 3>;
%template(DoubleMatrix_3x4) array<array<double, 3>, 4>;
%template(Matrix_Jx4) vector<array<double, 4>>;
%template(Matrix_NxJx4) vector<vector<array<double, 4>>>;
%template(Matrix_Jx3) vector<array<double, 3>>;
%template(Matrix_VxNxJx3) vector<vector<vector<array<double, 3>>>>;
%template(FloatMatrix_3x3) array<array<float, 3>, 3>;
%template(VectorFloat) vector<float>;
%template(FloatMatrix_3x1) array<array<float, 1>, 3>;
%template(FloatMatrix_3x4) array<array<float, 3>, 4>;
%template(Matrix_Jx4) vector<array<float, 4>>;
%template(Matrix_NxJx4) vector<vector<array<float, 4>>>;
%template(Matrix_Jx3) vector<array<float, 3>>;
%template(Matrix_VxNxJx3) vector<vector<vector<array<float, 3>>>>;
%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>;
}