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

spt/triangulator.cpp

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