Renamed project.

rpt/camera.cpp

@@ -0,0 +1,75 @@
+#include <iomanip>
+#include <sstream>
+
+#include "camera.hpp"
+
+// =================================================================================================
+// =================================================================================================
+
+template <size_t M, size_t N>
+static const std::string print_matrix(const std::array<std::array<float, N>, M> &matrix)
+{
+    std::ostringstream out;
+    out << "[";
+    for (size_t j = 0; j < M; ++j)
+    {
+        out << "[";
+        for (size_t i = 0; i < N; ++i)
+        {
+            out << matrix[j][i];
+            if (i < N - 1)
+            {
+                out << ", ";
+            }
+        }
+        out << "]";
+        if (j < M - 1)
+        {
+            out << ", ";
+        }
+    }
+    out << "]";
+    return out.str();
+}
+
+// =================================================================================================
+// =================================================================================================
+
+std::string Camera::to_string() const
+{
+    std::ostringstream out;
+    out << std::fixed << std::setprecision(6);
+
+    out << "{";
+    out << "'name': '" << name << "', ";
+
+    out << "'K': " << print_matrix(K) << ", ";
+
+    out << "'DC': [";
+    for (size_t i = 0; i < DC.size(); ++i)
+    {
+        out << DC[i];
+        if (i < DC.size() - 1)
+            out << ", ";
+    }
+    out << "], ";
+
+    out << "'R': " << print_matrix(R) << ", ";
+    out << "'T': " << print_matrix(T) << ", ";
+    out << "'P': " << print_matrix(P) << ", ";
+
+    out << "'width': " << width << ", ";
+    out << "'height': " << height << ", ";
+    out << "'type': " << type;
+
+    out << "}";
+    return out.str();
+}
+
+// =================================================================================================
+
+std::ostream &operator<<(std::ostream &out, const Camera &cam)
+{
+    out << cam.to_string();
+    return out;
+}

rpt/camera.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <array>
+#include <iostream>
+#include <string>
+#include <vector>
+
+// =================================================================================================
+
+struct Camera
+{
+    std::string name;
+    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;
+
+    friend std::ostream &operator<<(std::ostream &out, Camera const &camera);
+    std::string to_string() const;
+};

rpt/interface.cpp

@@ -0,0 +1,35 @@
+#include "triangulator.hpp"
+#include "interface.hpp"
+
+// =================================================================================================
+// =================================================================================================
+
+Triangulator::Triangulator(float min_score, size_t min_group_size)
+{
+    this->triangulator = new TriangulatorInternal(min_score, min_group_size);
+}
+
+// =================================================================================================
+
+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<float, 3>, 2> &roomparams,
+    const std::vector<std::string> &joint_names)
+{
+    return this->triangulator->triangulate_poses(poses_2d, cameras, roomparams, joint_names);
+}
+
+// =================================================================================================
+
+void Triangulator::reset()
+{
+    this->triangulator->reset();
+}
+
+// =================================================================================================
+
+void Triangulator::print_stats()
+{
+    this->triangulator->print_stats();
+}

rpt/interface.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "camera.hpp"
+
+// =================================================================================================
+
+// Forward declaration of the class, that swig does not try to parse all its dependencies.
+class TriangulatorInternal;
+
+// =================================================================================================
+
+class Triangulator
+{
+public:
+    /**
+     * Triangulator to predict poses from multiple views.
+     *
+     *
+     * @param min_score Minimum score to consider a triangulated joint as valid.
+     * @param min_group_size Minimum number of camera pairs that need to see a person.
+     */
+    Triangulator(
+        float min_score = 0.95,
+        size_t min_group_size = 1);
+
+    /**
+     * Calculate a triangulation.
+     *
+     *
+     * @param poses_2d List of shape [views, persons, joints, 3], containing the 2D poses.
+     * @param cameras List of cameras.
+     * @param roomparams Room parameters (room size, room center).
+     * @param joint_names List of 2D joint names.
+     *
+     * @return List of shape [persons, joints, 4], containing the 3D poses.
+     */
+    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<float, 3>, 2> &roomparams,
+        const std::vector<std::string> &joint_names);
+
+    /** Reset the triangulator. */
+    void reset();
+
+    /** Print triangulation statistics. */
+    void print_stats();
+
+private:
+    TriangulatorInternal *triangulator;
+};

rpt/triangulator.hpp

@@ -0,0 +1,133 @@
+#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(float min_score, size_t min_group_size);
+
+    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<float, 3>, 2> &roomparams,
+        const std::vector<std::string> &joint_names);
+
+    void reset();
+    void print_stats();
+
+private:
+    float min_score;
+    float min_group_size;
+
+    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",
+    };
+    const std::vector<std::pair<std::string, std::string>> core_limbs = {
+        {"knee_left", "ankle_left"},
+        {"hip_left", "knee_left"},
+        {"hip_right", "knee_right"},
+        {"knee_right", "ankle_right"},
+        {"elbow_left", "wrist_left"},
+        {"elbow_right", "wrist_right"},
+        {"shoulder_left", "elbow_left"},
+        {"shoulder_right", "elbow_right"},
+    };
+    std::vector<cv::Mat> last_poses_3d;
+
+    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);
+
+    float calc_pose_score(
+        const cv::Mat &pose1,
+        const cv::Mat &pose2,
+        const cv::Mat &dist1,
+        const CameraInternal &icam);
+
+    cv::Mat score_projection(
+        const cv::Mat &pose1,
+        const cv::Mat &repro1,
+        const cv::Mat &dists1,
+        const cv::Mat &mask,
+        float iscale);
+
+    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<float, 3>, 2> &roomparams,
+        const std::vector<std::array<size_t, 2>> &core_limbs_idx);
+
+    std::vector<std::tuple<cv::Point3f, 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, float>> &all_scored_poses,
+        float min_score);
+
+    cv::Mat merge_group(const std::vector<cv::Mat> &poses_3d, float min_score);
+
+    void add_extra_joints(std::vector<cv::Mat> &poses, const std::vector<std::string> &joint_names);
+
+    void filter_poses(
+        std::vector<cv::Mat> &poses,
+        const std::array<std::array<float, 3>, 2> &roomparams,
+        const std::vector<size_t> &core_joint_idx,
+        const std::vector<std::array<size_t, 2>> &core_limbs_idx);
+
+    void add_missing_joints(
+        std::vector<cv::Mat> &poses, const std::vector<std::string> &joint_names);
+
+    // Statistics
+    float num_calls = 0;
+    float total_time = 0;
+    float init_time = 0;
+    float undistort_time = 0;
+    float project_time = 0;
+    float match_time = 0;
+    float pairs_time = 0;
+    float pair_scoring_time = 0;
+    float grouping_time = 0;
+    float full_time = 0;
+    float merge_time = 0;
+    float post_time = 0;
+    float convert_time = 0;
+};