134 lines
3.8 KiB
C++
134 lines
3.8 KiB
C++
#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::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, 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;
|
|
};
|