diff --git a/media/RESULTS.md b/media/RESULTS.md index 2066daf..8280cc0 100644 --- a/media/RESULTS.md +++ b/media/RESULTS.md @@ -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, diff --git a/spt/camera.cpp b/spt/camera.cpp index 0496b43..55d7450 100644 --- a/spt/camera.cpp +++ b/spt/camera.cpp @@ -7,7 +7,7 @@ // ================================================================================================= template -static const std::string print_matrix(const std::array, M> &matrix) +static const std::string print_matrix(const std::array, 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; } - diff --git a/spt/camera.hpp b/spt/camera.hpp index f02b86f..03412f0 100644 --- a/spt/camera.hpp +++ b/spt/camera.hpp @@ -10,11 +10,11 @@ struct Camera { std::string name; - std::array, 3> K; - std::vector DC; - std::array, 3> R; - std::array, 3> T; - std::array, 4> P; + std::array, 3> K; + std::vector DC; + std::array, 3> R; + std::array, 3> T; + std::array, 4> P; int width; int height; std::string type; diff --git a/spt/interface.cpp b/spt/interface.cpp index 940d7af..f483d28 100644 --- a/spt/interface.cpp +++ b/spt/interface.cpp @@ -4,17 +4,17 @@ // ================================================================================================= // ================================================================================================= -Triangulator::Triangulator(double min_score) +Triangulator::Triangulator(float min_score) { this->triangulator = new TriangulatorInternal(min_score); } // ================================================================================================= -std::vector>> Triangulator::triangulate_poses( - const std::vector>>> &poses_2d, +std::vector>> Triangulator::triangulate_poses( + const std::vector>>> &poses_2d, const std::vector &cameras, - const std::array, 2> &roomparams, + const std::array, 2> &roomparams, const std::vector &joint_names) { return this->triangulator->triangulate_poses(poses_2d, cameras, roomparams, joint_names); diff --git a/spt/interface.hpp b/spt/interface.hpp index 0092258..0fb45ab 100644 --- a/spt/interface.hpp +++ b/spt/interface.hpp @@ -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>> triangulate_poses( - const std::vector>>> &poses_2d, + std::vector>> triangulate_poses( + const std::vector>>> &poses_2d, const std::vector &cameras, - const std::array, 2> &roomparams, + const std::array, 2> &roomparams, const std::vector &joint_names); /** Reset the triangulator. */ diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index 00d91ce..3f8ad18 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -31,7 +31,7 @@ std::cout << " ["; for (int j = 0; j < cols; ++j) { - std::cout << std::fixed << std::setprecision(3) << mat.at(i, j); + std::cout << std::fixed << std::setprecision(3) << mat.at(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(&cam.K[0][0])).clone(); - DC = cv::Mat(cam.DC.size(), 1, CV_64FC1, const_cast(cam.DC.data())).clone(); - R = cv::Mat(3, 3, CV_64FC1, const_cast(&cam.R[0][0])).clone(); - T = cv::Mat(3, 1, CV_64FC1, const_cast(&cam.T[0][0])).clone(); + K = cv::Mat(3, 3, CV_32FC1, const_cast(&cam.K[0][0])).clone(); + DC = cv::Mat(cam.DC.size(), 1, CV_32FC1, const_cast(cam.DC.data())).clone(); + R = cv::Mat(3, 3, CV_32FC1, const_cast(&cam.R[0][0])).clone(); + T = cv::Mat(3, 1, CV_32FC1, const_cast(&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>> TriangulatorInternal::triangulate_poses( - const std::vector>>> &poses_2d, +std::vector>> TriangulatorInternal::triangulate_poses( + const std::vector>>> &poses_2d, const std::vector &cameras, - const std::array, 2> &roomparams, + const std::array, 2> &roomparams, const std::vector &joint_names) { // Check inputs @@ -148,12 +148,12 @@ std::vector>> TriangulatorInternal::triangulat for (size_t j = 0; j < num_persons; ++j) { std::vector 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(k); + float *mat_ptr = pose_mat.ptr(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>> TriangulatorInternal::triangulat } // Check matches to old poses - double threshold = min_score - 0.2; + float threshold = min_score - 0.2; std::map>> scored_pasts; if (!last_poses_3d.empty()) { @@ -381,7 +381,7 @@ std::vector>> TriangulatorInternal::triangulat } // Calculate pair scores - std::vector> all_scored_poses; + std::vector> 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>> TriangulatorInternal::triangulat last_poses_3d = all_merged_poses; // Convert to output format - std::vector>> poses_3d; + std::vector>> 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> pose; + std::vector> 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(j); - std::array point; + const float *mat_ptr = mat.ptr(j); + std::array point; for (size_t k = 0; k < 4; ++k) { point[k] = mat_ptr[k]; @@ -540,13 +540,13 @@ void TriangulatorInternal::undistort_poses(std::vector &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(j); - double x = poses_ptr[0]; - double y = poses_ptr[1]; + float *poses_ptr = poses[p].ptr(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 &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> TriangulatorInternal::pro // Set points behind the camera to zero for (size_t j = 0; j < num_joints; ++j) { - double *xyz_row_ptr = xyz.ptr(j); - double z = xyz_row_ptr[2]; + float *xyz_row_ptr = xyz.ptr(j); + float z = xyz_row_ptr[2]; if (z <= 0) { xyz_row_ptr[0] = 0.0; @@ -605,12 +605,12 @@ std::tuple, std::vector> 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> 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 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(j); - const double *uv_row_ptr = uv.ptr(j); - const double *bodies3D_row_ptr = body3D.ptr(j); + float *bodies2D_row_ptr = bodies2D.ptr(j); + const float *uv_row_ptr = uv.ptr(j); + const float *bodies3D_row_ptr = body3D.ptr(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> TriangulatorInternal::pro // Filter invalid projections for (size_t j = 0; j < num_joints; ++j) { - double *bodies2D_row_ptr = bodies2D.ptr(j); - double x = bodies2D_row_ptr[0]; - double y = bodies2D_row_ptr[1]; + float *bodies2D_row_ptr = bodies2D.ptr(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> 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(i); - const double *pose1_ptr = pose1.ptr(i); - const double *pose2_ptr = pose2.ptr(i); + const float *pose1_ptr = pose1.ptr(i); + const float *pose2_ptr = pose2.ptr(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(pose1.rows * 0.2); const size_t min_k = 3; - std::vector scores_vec; + std::vector scores_vec; scores_vec.reserve(valid_count); for (int i = 0; i < scores.rows; ++i) { - const double *scores_ptr = scores.ptr(i); - const uchar *mask_ptr = mask.ptr(i); + const float *scores_ptr = scores.ptr(i); + const u_char *mask_ptr = mask.ptr(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(n_items); + float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); + score = sum_scores / static_cast(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(i); - const double *pose_ptr = pose.ptr(i); - const double *repro_ptr = repro.ptr(i); - double *error_ptr = error.ptr(i); + const u_char *mask_ptr = mask.ptr(i); + const float *pose_ptr = pose.ptr(i); + const float *repro_ptr = repro.ptr(i); + float *error_ptr = error.ptr(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(i); + float *error_ptr = error.ptr(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(i); - const double *dists_ptr = dists.ptr(i); + const u_char *mask_ptr = mask.ptr(i); + const float *dists_ptr = dists.ptr(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(i); + float *error_ptr = error.ptr(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(i); + float *score_ptr = score.ptr(i); score_ptr[0] = 1.0 / (1.0 + score_ptr[0] * 10.0); } @@ -822,15 +822,15 @@ cv::Mat TriangulatorInternal::score_projection( // ================================================================================================= -std::pair TriangulatorInternal::triangulate_and_score( +std::pair TriangulatorInternal::triangulate_and_score( const cv::Mat &pose1, const cv::Mat &pose2, const CameraInternal &cam1, const CameraInternal &cam2, - const std::array, 2> &roomparams, + const std::array, 2> &roomparams, const std::vector> &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 TriangulatorInternal::triangulate_and_score( for (size_t i = 0; i < num_joints; ++i) { u_char *mask_ptr = mask.ptr(i); - const double *pose1_ptr = pose1.ptr(i); - const double *pose2_ptr = pose2.ptr(i); + const float *pose1_ptr = pose1.ptr(i); + const float *pose2_ptr = pose2.ptr(i); mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score); } @@ -848,23 +848,23 @@ std::pair 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 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(0); - double *points2_ptr = points2.ptr(0); + float *points1_ptr = points1.ptr(0); + float *points2_ptr = points2.ptr(0); for (size_t i = 0; i < num_joints; ++i) { - const uchar *mask_ptr = mask.ptr(i); - const double *pose1_ptr = pose1.ptr(i); - const double *pose2_ptr = pose2.ptr(i); + const u_char *mask_ptr = mask.ptr(i); + const float *pose1_ptr = pose1.ptr(i); + const float *pose2_ptr = pose2.ptr(i); if (mask_ptr[0]) { @@ -882,13 +882,13 @@ std::pair 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(i); - double *pose3d_ptr = pose3d.ptr(i); - const double *points3d_ptr = points3d.ptr(idx); + const u_char *mask_ptr = mask.ptr(i); + float *pose3d_ptr = pose3d.ptr(i); + const float *points3d_ptr = points3d.ptr(idx); if (mask_ptr[0]) { @@ -901,11 +901,11 @@ std::pair TriangulatorInternal::triangulate_and_score( } // Check if mean of the points is inside the room bounds - std::array mean = {0.0, 0.0, 0.0}; + std::array mean = {0.0, 0.0, 0.0}; for (size_t i = 0; i < num_joints; ++i) { - const uchar *mask_ptr = mask.ptr(i); - const double *pose3d_ptr = pose3d.ptr(i); + const u_char *mask_ptr = mask.ptr(i); + const float *pose3d_ptr = pose3d.ptr(i); if (mask_ptr[0]) { @@ -914,13 +914,13 @@ std::pair 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 ¢er = roomparams[0]; - const std::array &size = roomparams[1]; + const std::array ¢er = roomparams[0]; + const std::array &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 TriangulatorInternal::triangulate_and_score( // Very low score if outside room for (size_t i = 0; i < num_joints; ++i) { - double *pose3d_ptr = pose3d.ptr(i); + float *pose3d_ptr = pose3d.ptr(i); pose3d_ptr[3] = 0.001; } return {pose3d, 0.001}; @@ -947,7 +947,7 @@ std::pair 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 TriangulatorInternal::triangulate_and_score( // Add scores to 3D pose for (size_t i = 0; i < num_joints; ++i) { - const uchar *mask_ptr = mask.ptr(i); - double *pose3d_ptr = pose3d.ptr(i); - const double *scores_ptr = scores.ptr(i); + const u_char *mask_ptr = mask.ptr(i); + float *pose3d_ptr = pose3d.ptr(i); + const float *scores_ptr = scores.ptr(i); if (mask_ptr[0]) { @@ -968,11 +968,11 @@ std::pair 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(i); - double *pose3d_ptr = pose3d.ptr(i); + const u_char *mask_ptr = mask.ptr(i); + float *pose3d_ptr = pose3d.ptr(i); if (mask_ptr[0]) { @@ -991,20 +991,20 @@ std::pair 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(limb1); - double *pose3d_ptr2 = pose3d.ptr(limb2); + float *pose3d_ptr1 = pose3d.ptr(limb1); + float *pose3d_ptr2 = pose3d.ptr(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 TriangulatorInternal::triangulate_and_score( // Drop lowest scores size_t drop_k = static_cast(num_joints * 0.2); const size_t min_k = 3; - std::vector scores_vec; + std::vector scores_vec; for (size_t i = 0; i < num_joints; ++i) { - const double *pose3d_ptr = pose3d.ptr(i); + const float *pose3d_ptr = pose3d.ptr(i); if (pose3d_ptr[3] > min_score) { scores_vec.push_back(pose3d_ptr[3]); @@ -1035,12 +1035,12 @@ std::pair 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(n_items); + float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); + score = sum_scores / static_cast(n_items); } return std::make_pair(pose3d, score); @@ -1050,11 +1050,11 @@ std::pair TriangulatorInternal::triangulate_and_score( std::vector>> TriangulatorInternal::calc_grouping( const std::vector, std::pair>> &all_pairs, - const std::vector> &all_scored_poses, - double min_score) + const std::vector> &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>> TriangulatorInte size_t num_valid = 0; for (size_t j = 0; j < num_joints; ++j) { - const double *pose_3d_ptr = pose_3d.ptr(j); + const float *pose_3d_ptr = pose_3d.ptr(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>> 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>> TriangulatorInte { const cv::Mat &pose_3d = all_scored_poses[i].first; const cv::Point3d ¢er = centers[i]; - double best_dist = std::numeric_limits::infinity(); + float best_dist = std::numeric_limits::infinity(); int best_group = -1; for (size_t j = 0; j < groups.size(); ++j) @@ -1106,32 +1106,32 @@ std::vector>> 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(row); - const double *group_pose_ptr = group_pose.ptr(row); + const float *pose_3d_ptr = pose_3d.ptr(row); + const float *group_pose_ptr = group_pose.ptr(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>> 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>> TriangulatorInte cv::Mat &group_pose = std::get<1>(group); std::vector &group_indices = std::get<2>(group); - double n_elems = static_cast(group_indices.size()); - double inv_n1 = 1.0 / (n_elems + 1.0); + float n_elems = static_cast(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>> TriangulatorInte // Update group pose for (size_t row = 0; row < num_joints; ++row) { - const double *pose_3d_ptr = pose_3d.ptr(row); - double *group_pose_ptr = group_pose.ptr(row); + const float *pose_3d_ptr = pose_3d.ptr(row); + float *group_pose_ptr = group_pose.ptr(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>> TriangulatorInte // ================================================================================================= -cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, double min_score) +cv::Mat TriangulatorInternal::merge_group(const std::vector &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 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 &poses_3d, for (int j = 0; j < num_joints; ++j) { - const double *pose_ptr = pose.ptr(j); - double *sum_ptr = sum_poses.ptr(j); + const float *pose_ptr = pose.ptr(j); + float *sum_ptr = sum_poses.ptr(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 &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(j); - const double *sum_ptr = sum_poses.ptr(j); + float *initial_ptr = initial_pose_3d.ptr(j); + const float *sum_ptr = sum_poses.ptr(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 &poses_3d, int valid_joints = 0; for (int j = 0; j < num_joints; ++j) { - const double *initial_ptr = initial_pose_3d.ptr(j); - double score = initial_ptr[3]; + const float *initial_ptr = initial_pose_3d.ptr(j); + float score = initial_ptr[3]; if (score > min_score) { @@ -1263,7 +1263,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, { if (!jmask[j]) { - double *initial_ptr = initial_pose_3d.ptr(j); + float *initial_ptr = initial_pose_3d.ptr(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 &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(i); + float *distances_ptr = distances.ptr(i); u_char *mask_ptr = mask.ptr(i); const cv::Mat &pose = poses_3d[i]; for (int j = 0; j < num_joints; ++j) { - const double *initial_ptr = initial_pose_3d.ptr(j); - const double *pose_ptr = pose.ptr(j); + const float *initial_ptr = initial_pose_3d.ptr(j); + const float *pose_ptr = pose.ptr(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 &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> valid_indices; + std::vector> valid_indices; valid_indices.reserve(num_poses); for (int i = 0; i < num_poses; ++i) { if (mask.at(i, j)) { - valid_indices.emplace_back(distances.at(i, j), i); + valid_indices.emplace_back(distances.at(i, j), i); } } std::partial_sort( @@ -1329,7 +1329,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &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 &poses_3d, { if (mask_row_ptr[j]) { - double *sum_ptr = sum_poses.ptr(j); - const double *pose_ptr = pose.ptr(j); + float *sum_ptr = sum_poses.ptr(j); + const float *pose_ptr = pose.ptr(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 &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(j); - const double *sum_ptr = sum_poses.ptr(j); + float *final_pose_ptr = final_pose_3d.ptr(j); + const float *sum_ptr = sum_poses.ptr(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; diff --git a/spt/triangulator.hpp b/spt/triangulator.hpp index 33a05f7..31dc48b 100644 --- a/spt/triangulator.hpp +++ b/spt/triangulator.hpp @@ -31,18 +31,18 @@ public: class TriangulatorInternal { public: - TriangulatorInternal(double min_score); + TriangulatorInternal(float min_score); - std::vector>> triangulate_poses( - const std::vector>>> &poses_2d, + std::vector>> triangulate_poses( + const std::vector>>> &poses_2d, const std::vector &cameras, - const std::array, 2> &roomparams, + const std::array, 2> &roomparams, const std::vector &joint_names); void reset(); private: - double min_score; + float min_score; const std::vector core_joints = { "shoulder_left", "shoulder_right", @@ -75,7 +75,7 @@ private: std::tuple, std::vector> project_poses( const std::vector &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 triangulate_and_score( + std::pair triangulate_and_score( const cv::Mat &pose1, const cv::Mat &pose2, const CameraInternal &cam1, const CameraInternal &cam2, - const std::array, 2> &roomparams, + const std::array, 2> &roomparams, const std::vector> &core_limbs_idx); std::vector>> calc_grouping( const std::vector, std::pair>> &all_pairs, - const std::vector> &all_scored_poses, - double min_score); + const std::vector> &all_scored_poses, + float min_score); - cv::Mat merge_group(const std::vector &poses_3d, double min_score); + cv::Mat merge_group(const std::vector &poses_3d, float min_score); }; diff --git a/swig/Makefile b/swig/Makefile index add0b44..8ae5c14 100644 --- a/swig/Makefile +++ b/swig/Makefile @@ -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/ - 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 +all: + 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) -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 diff --git a/swig/spt.i b/swig/spt.i index d383757..91ea2d0 100644 --- a/swig/spt.i +++ b/swig/spt.i @@ -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, 3>; - %template(VectorDouble) vector; - %template(DoubleMatrix_3x1) array, 3>; - %template(DoubleMatrix_3x4) array, 4>; - %template(Matrix_Jx4) vector>; - %template(Matrix_NxJx4) vector>>; - %template(Matrix_Jx3) vector>; - %template(Matrix_VxNxJx3) vector>>>; + %template(FloatMatrix_3x3) array, 3>; + %template(VectorFloat) vector; + %template(FloatMatrix_3x1) array, 3>; + %template(FloatMatrix_3x4) array, 4>; + %template(Matrix_Jx4) vector>; + %template(Matrix_NxJx4) vector>>; + %template(Matrix_Jx3) vector>; + %template(Matrix_VxNxJx3) vector>>>; %template(VectorCamera) vector; - %template(DoubleMatrix_2x3) array, 2>; + %template(FloatMatrix_2x3) array, 2>; %template(VectorString) vector; }