Initial cpp reimplementation.

spt/triangulator.hpp

@@ -0,0 +1,89 @@
+#pragma once
+
+#include <array>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include <opencv2/opencv.hpp>
+
+#include "camera.hpp"
+
+// =================================================================================================
+
+class CameraInternal
+{
+public:
+    CameraInternal(const Camera &cam);
+
+    Camera cam;
+    cv::Mat K;
+    cv::Mat DC;
+    cv::Mat R;
+    cv::Mat T;
+    cv::Mat P;
+
+    void update_projection_matrix();
+};
+
+// =================================================================================================
+
+class TriangulatorInternal
+{
+public:
+    TriangulatorInternal(double 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,
+        const std::vector<Camera> &cameras,
+        const std::array<std::array<double, 3>, 2> &roomparams,
+        const std::vector<std::string> &joint_names);
+
+    void reset();
+
+private:
+    double min_score;
+    const std::vector<std::string> core_joints = {
+        "shoulder_left",
+        "shoulder_right",
+        "hip_left",
+        "hip_right",
+        "elbow_left",
+        "elbow_right",
+        "knee_left",
+        "knee_right",
+        "wrist_left",
+        "wrist_right",
+        "ankle_left",
+        "ankle_right",
+    };
+
+    std::vector<cv::Mat> last_poses_3d;
+
+    void undistort_points(cv::Mat &points, CameraInternal &icam);
+    void undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam);
+
+    std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(
+        const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists);
+
+    cv::Mat score_projection(
+        const cv::Mat &pose1,
+        const cv::Mat &repro1,
+        const cv::Mat &dists1,
+        const cv::Mat &mask,
+        double iscale);
+
+    std::pair<cv::Mat, double> 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);
+
+    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);
+
+    cv::Mat merge_group(const std::vector<cv::Mat> &poses_3d, double min_score);
+};