Make triangulation a zero-copy pure function
This commit is contained in:
+8
-70
@@ -1,7 +1,7 @@
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
#include "triangulator.hpp"
|
||||
#include "interface.hpp"
|
||||
|
||||
// =================================================================================================
|
||||
@@ -33,36 +33,19 @@ float &PoseBatch2D::at(size_t view, size_t person, size_t joint, size_t coord)
|
||||
return data[pose2d_offset(view, person, joint, coord, max_persons, num_joints)];
|
||||
}
|
||||
|
||||
const float &PoseBatch2DView::at(size_t view, size_t person, size_t joint, size_t coord) const
|
||||
{
|
||||
return data[pose2d_offset(view, person, joint, coord, max_persons, num_joints)];
|
||||
}
|
||||
|
||||
const float &PoseBatch2D::at(size_t view, size_t person, size_t joint, size_t coord) const
|
||||
{
|
||||
return data[pose2d_offset(view, person, joint, coord, max_persons, num_joints)];
|
||||
}
|
||||
|
||||
RaggedPoses2D PoseBatch2D::to_nested() const
|
||||
PoseBatch2DView PoseBatch2D::view() const
|
||||
{
|
||||
if (person_counts.size() != num_views)
|
||||
{
|
||||
throw std::invalid_argument("PoseBatch2D person_counts size must match num_views.");
|
||||
}
|
||||
|
||||
RaggedPoses2D poses_2d(num_views);
|
||||
for (size_t view = 0; view < num_views; ++view)
|
||||
{
|
||||
const size_t num_persons = person_counts[view];
|
||||
poses_2d[view].resize(num_persons);
|
||||
for (size_t person = 0; person < num_persons; ++person)
|
||||
{
|
||||
poses_2d[view][person].resize(num_joints);
|
||||
for (size_t joint = 0; joint < num_joints; ++joint)
|
||||
{
|
||||
for (size_t coord = 0; coord < 3; ++coord)
|
||||
{
|
||||
poses_2d[view][person][joint][coord] = at(view, person, joint, coord);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return poses_2d;
|
||||
return PoseBatch2DView {data.data(), person_counts.data(), num_views, max_persons, num_joints};
|
||||
}
|
||||
|
||||
PoseBatch2D PoseBatch2D::from_nested(const RaggedPoses2D &poses_2d)
|
||||
@@ -169,48 +152,3 @@ PoseBatch3D PoseBatch3D::from_nested(const NestedPoses3D &poses_3d)
|
||||
}
|
||||
return batch;
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
Triangulator::Triangulator(float min_match_score, size_t min_group_size)
|
||||
: triangulator(std::make_unique<TriangulatorInternal>(min_match_score, min_group_size))
|
||||
{
|
||||
}
|
||||
|
||||
Triangulator::~Triangulator() = default;
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
PoseBatch3D Triangulator::triangulate_poses(
|
||||
const PoseBatch2D &poses_2d,
|
||||
const std::vector<Camera> &cameras,
|
||||
const std::array<std::array<float, 3>, 2> &roomparams,
|
||||
const std::vector<std::string> &joint_names)
|
||||
{
|
||||
return PoseBatch3D::from_nested(
|
||||
triangulator->triangulate_poses(poses_2d.to_nested(), cameras, roomparams, joint_names));
|
||||
}
|
||||
|
||||
NestedPoses3D Triangulator::triangulate_poses(
|
||||
const RaggedPoses2D &poses_2d,
|
||||
const std::vector<Camera> &cameras,
|
||||
const std::array<std::array<float, 3>, 2> &roomparams,
|
||||
const std::vector<std::string> &joint_names)
|
||||
{
|
||||
return triangulator->triangulate_poses(poses_2d, cameras, roomparams, joint_names);
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
void Triangulator::reset()
|
||||
{
|
||||
triangulator->reset();
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
void Triangulator::print_stats()
|
||||
{
|
||||
triangulator->print_stats();
|
||||
}
|
||||
|
||||
+42
-53
@@ -1,7 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -14,6 +14,17 @@ using NestedPoses3D = std::vector<std::vector<std::array<float, 4>>>;
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
struct PoseBatch2DView
|
||||
{
|
||||
const float *data = nullptr;
|
||||
const uint32_t *person_counts = nullptr;
|
||||
size_t num_views = 0;
|
||||
size_t max_persons = 0;
|
||||
size_t num_joints = 0;
|
||||
|
||||
const float &at(size_t view, size_t person, size_t joint, size_t coord) const;
|
||||
};
|
||||
|
||||
struct PoseBatch2D
|
||||
{
|
||||
std::vector<float> data;
|
||||
@@ -24,8 +35,8 @@ struct PoseBatch2D
|
||||
|
||||
float &at(size_t view, size_t person, size_t joint, size_t coord);
|
||||
const float &at(size_t view, size_t person, size_t joint, size_t coord) const;
|
||||
PoseBatch2DView view() const;
|
||||
|
||||
RaggedPoses2D to_nested() const;
|
||||
static PoseBatch2D from_nested(const RaggedPoses2D &poses_2d);
|
||||
};
|
||||
|
||||
@@ -44,56 +55,34 @@ struct PoseBatch3D
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
class TriangulatorInternal;
|
||||
/**
|
||||
* Calculate a triangulation using a padded pose tensor.
|
||||
*
|
||||
* @param poses_2d Padded poses of shape [views, max_persons, joints, 3].
|
||||
* @param cameras List of cameras.
|
||||
* @param roomparams Room parameters (room size, room center).
|
||||
* @param joint_names List of 2D joint names.
|
||||
* @param min_match_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.
|
||||
*
|
||||
* @return Pose tensor of shape [persons, joints, 4].
|
||||
*/
|
||||
PoseBatch3D triangulate_poses(
|
||||
const PoseBatch2DView &poses_2d,
|
||||
const std::vector<Camera> &cameras,
|
||||
const std::array<std::array<float, 3>, 2> &roomparams,
|
||||
const std::vector<std::string> &joint_names,
|
||||
float min_match_score = 0.95f,
|
||||
size_t min_group_size = 1);
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
class Triangulator
|
||||
inline PoseBatch3D triangulate_poses(
|
||||
const PoseBatch2D &poses_2d,
|
||||
const std::vector<Camera> &cameras,
|
||||
const std::array<std::array<float, 3>, 2> &roomparams,
|
||||
const std::vector<std::string> &joint_names,
|
||||
float min_match_score = 0.95f,
|
||||
size_t min_group_size = 1)
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Triangulator to predict poses from multiple views.
|
||||
*
|
||||
* @param min_match_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_match_score = 0.95,
|
||||
size_t min_group_size = 1);
|
||||
|
||||
~Triangulator();
|
||||
|
||||
/**
|
||||
* Calculate a triangulation using a padded pose tensor.
|
||||
*
|
||||
* @param poses_2d Padded poses of shape [views, max_persons, joints, 3].
|
||||
* @param cameras List of cameras.
|
||||
* @param roomparams Room parameters (room size, room center).
|
||||
* @param joint_names List of 2D joint names.
|
||||
*
|
||||
* @return Pose tensor of shape [persons, joints, 4].
|
||||
*/
|
||||
PoseBatch3D triangulate_poses(
|
||||
const PoseBatch2D &poses_2d,
|
||||
const std::vector<Camera> &cameras,
|
||||
const std::array<std::array<float, 3>, 2> &roomparams,
|
||||
const std::vector<std::string> &joint_names);
|
||||
|
||||
/**
|
||||
* Compatibility overload for callers that still produce ragged nested vectors.
|
||||
*/
|
||||
NestedPoses3D triangulate_poses(
|
||||
const RaggedPoses2D &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:
|
||||
std::unique_ptr<TriangulatorInternal> triangulator;
|
||||
};
|
||||
return triangulate_poses(
|
||||
poses_2d.view(), cameras, roomparams, joint_names, min_match_score, min_group_size);
|
||||
}
|
||||
|
||||
+466
-631
File diff suppressed because it is too large
Load Diff
@@ -1,130 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "camera.hpp"
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
class TriangulatorInternal
|
||||
{
|
||||
public:
|
||||
TriangulatorInternal(float min_match_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_match_score;
|
||||
float min_group_size;
|
||||
size_t num_cams;
|
||||
|
||||
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<std::vector<std::array<float, 4>>> last_poses_3d;
|
||||
|
||||
void undistort_poses(
|
||||
std::vector<std::vector<std::array<float, 3>>> &poses_2d, CameraInternal &icam);
|
||||
|
||||
std::tuple<std::vector<std::vector<std::array<float, 3>>>, std::vector<std::vector<float>>>
|
||||
project_poses(
|
||||
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
|
||||
const CameraInternal &icam,
|
||||
bool calc_dists);
|
||||
|
||||
float calc_pose_score(
|
||||
const std::vector<std::array<float, 3>> &pose1,
|
||||
const std::vector<std::array<float, 3>> &pose2,
|
||||
const std::vector<float> &dist1,
|
||||
const CameraInternal &icam);
|
||||
|
||||
std::vector<float> score_projection(
|
||||
const std::vector<std::array<float, 3>> &pose,
|
||||
const std::vector<std::array<float, 3>> &repro,
|
||||
const std::vector<float> &dists,
|
||||
const std::vector<bool> &mask,
|
||||
float iscale);
|
||||
|
||||
std::pair<std::vector<std::array<float, 4>>, float> triangulate_and_score(
|
||||
const std::vector<std::array<float, 3>> &pose1,
|
||||
const std::vector<std::array<float, 3>> &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<
|
||||
std::array<float, 3>, std::vector<std::array<float, 4>>, 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<std::vector<std::array<float, 4>>, float>> &all_scored_poses,
|
||||
float min_score);
|
||||
|
||||
std::vector<std::array<float, 4>> merge_group(
|
||||
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
|
||||
float min_score);
|
||||
|
||||
void add_extra_joints(
|
||||
std::vector<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names);
|
||||
|
||||
void filter_poses(
|
||||
std::vector<std::vector<std::array<float, 4>>> &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<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names);
|
||||
|
||||
void replace_far_joints(
|
||||
std::vector<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names);
|
||||
|
||||
// Statistics
|
||||
double num_calls = 0;
|
||||
double total_time = 0;
|
||||
double init_time = 0;
|
||||
double undistort_time = 0;
|
||||
double project_time = 0;
|
||||
double match_time = 0;
|
||||
double pairs_time = 0;
|
||||
double pair_scoring_time = 0;
|
||||
double grouping_time = 0;
|
||||
double full_time = 0;
|
||||
double merge_time = 0;
|
||||
double post_time = 0;
|
||||
double convert_time = 0;
|
||||
};
|
||||
Reference in New Issue
Block a user