crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Simplify triangulation core and remove integrations

Browse Source
This commit is contained in:
crosstyan
2026-03-11 23:55:04 +08:00
parent 103aeb5887
commit 7df34b18c3
20 changed files with 222 additions and 1969 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rpt/interface.hpp
+18 -15
View File
@@ -144,6 +144,14 @@ struct TriangulationTrace
// =================================================================================================
struct TriangulationOptions
{
float min_match_score = 0.95f;
size_t min_group_size = 1;
};
// =================================================================================================
std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d);
PreviousPoseFilterDebug filter_pairs_with_previous_poses(
@@ -151,17 +159,17 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names,
const PoseBatch3DView &previous_poses_3d,
float min_match_score = 0.95f);
const TriangulationOptions &options = {});
inline PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const PoseBatch2D &poses_2d,
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names,
const PoseBatch3D &previous_poses_3d,
float min_match_score = 0.95f)
const TriangulationOptions &options = {})
{
return filter_pairs_with_previous_poses(
poses_2d.view(), cameras, joint_names, previous_poses_3d.view(), min_match_score);
poses_2d.view(), cameras, joint_names, previous_poses_3d.view(), options);
}
TriangulationTrace triangulate_debug(
@@ -170,8 +178,7 @@ TriangulationTrace triangulate_debug(
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3DView *previous_poses_3d = nullptr,
float min_match_score = 0.95f,
size_t min_group_size = 1);
const TriangulationOptions &options = {});
inline TriangulationTrace triangulate_debug(
const PoseBatch2D &poses_2d,
@@ -179,8 +186,7 @@ inline TriangulationTrace triangulate_debug(
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3D *previous_poses_3d = nullptr,
float min_match_score = 0.95f,
size_t min_group_size = 1)
const TriangulationOptions &options = {})
{
PoseBatch3DView previous_view_storage;
const PoseBatch3DView *previous_view = nullptr;
@@ -190,7 +196,7 @@ inline TriangulationTrace triangulate_debug(
previous_view = &previous_view_storage;
}
return triangulate_debug(
poses_2d.view(), cameras, roomparams, joint_names, previous_view, min_match_score, min_group_size);
poses_2d.view(), cameras, roomparams, joint_names, previous_view, options);
}
// =================================================================================================
@@ -202,8 +208,7 @@ inline TriangulationTrace triangulate_debug(
* @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.
* @param options Triangulation options.
*
* @return Pose tensor of shape [persons, joints, 4].
*/
@@ -213,8 +218,7 @@ PoseBatch3D triangulate_poses(
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3DView *previous_poses_3d = nullptr,
float min_match_score = 0.95f,
size_t min_group_size = 1);
const TriangulationOptions &options = {});
inline PoseBatch3D triangulate_poses(
const PoseBatch2D &poses_2d,
@@ -222,8 +226,7 @@ inline PoseBatch3D triangulate_poses(
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3D *previous_poses_3d = nullptr,
float min_match_score = 0.95f,
size_t min_group_size = 1)
const TriangulationOptions &options = {})
{
PoseBatch3DView previous_view_storage;
const PoseBatch3DView *previous_view = nullptr;
@@ -233,5 +236,5 @@ inline PoseBatch3D triangulate_poses(
previous_view = &previous_view_storage;
}
return triangulate_poses(
poses_2d.view(), cameras, roomparams, joint_names, previous_view, min_match_score, min_group_size);
poses_2d.view(), cameras, roomparams, joint_names, previous_view, options);
}
rpt/tracker.hpp
-325
View File
@@ -1,325 +0,0 @@
#pragma once
#include <array>
#include <string>
#include <vector>
#include <algorithm>
#include <limits>
#include <cmath>
#include <iostream>
// =================================================================================================
struct Track
{
std::vector<std::vector<std::array<float, 4>>> core_poses;
std::vector<std::vector<std::array<float, 4>>> full_poses;
std::vector<double> timestamps;
size_t id;
};
// =================================================================================================
class PoseTracker
{
public:
PoseTracker(float max_movement_speed, float max_distance);
std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> track_poses(
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
const std::vector<std::string> &joint_names,
const double timestamp);
void reset();
private:
float max_distance;
float max_movement_speed;
size_t history_size = 3;
std::vector<double> timestamps;
std::vector<Track> pose_tracks;
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::tuple<int, float> match_to_track(const std::vector<std::array<float, 4>> &core_pose_3d);
std::vector<std::array<float, 4>> refine_pose(const Track &track);
};
// =================================================================================================
// =================================================================================================
PoseTracker::PoseTracker(float max_movement_speed, float max_distance)
{
this->max_movement_speed = max_movement_speed;
this->max_distance = max_distance;
}
// =================================================================================================
void PoseTracker::reset()
{
pose_tracks.clear();
timestamps.clear();
}
// =================================================================================================
std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> PoseTracker::track_poses(
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
const std::vector<std::string> &joint_names,
const double timestamp)
{
// Extract core joints
std::vector<size_t> core_joint_idx;
for (const auto &joint : core_joints)
{
auto it = std::find(joint_names.begin(), joint_names.end(), joint);
core_joint_idx.push_back(std::distance(joint_names.begin(), it));
}
std::vector<std::vector<std::array<float, 4>>> core_poses;
core_poses.resize(poses_3d.size());
for (size_t i = 0; i < poses_3d.size(); ++i)
{
core_poses[i].resize(core_joint_idx.size());
for (size_t j = 0; j < core_joint_idx.size(); ++j)
{
for (size_t k = 0; k < 4; ++k)
{
core_poses[i][j][k] = poses_3d[i][core_joint_idx[j]][k];
}
}
}
// Match core poses to tracks
std::vector<std::tuple<size_t, int, float>> matches;
for (size_t i = 0; i < core_poses.size(); ++i)
{
auto [track_idx, distance_sq] = match_to_track(core_poses[i]);
matches.emplace_back(i, track_idx, distance_sq);
}
std::sort(matches.begin(), matches.end(),
[](const auto &a, const auto &b)
{ return std::get<2>(a) < std::get<2>(b); });
// If track is matched multiple times, only add the best and create new tracks for the rest
std::vector<bool> used(pose_tracks.size(), false);
for (size_t i = 0; i < matches.size(); ++i)
{
auto [pose_idx, track_idx, distance_sq] = matches[i];
if (track_idx != -1 && !used[track_idx])
{
used[track_idx] = true;
}
else
{
std::get<1>(matches[i]) = -1;
}
}
// Update tracks
for (size_t i = 0; i < matches.size(); ++i)
{
auto [pose_idx, track_idx, distance_sq] = matches[i];
if (track_idx == -1)
{
// Create a new track
Track new_track;
new_track.core_poses.push_back(core_poses[pose_idx]);
new_track.full_poses.push_back(poses_3d[pose_idx]);
new_track.timestamps.push_back(timestamp);
new_track.id = pose_tracks.size();
pose_tracks.push_back(new_track);
}
else
{
// Update existing track
auto &track = pose_tracks[track_idx];
track.core_poses.push_back(core_poses[pose_idx]);
track.full_poses.push_back(poses_3d[pose_idx]);
track.timestamps.push_back(timestamp);
}
}
// Remove old track entries
timestamps.push_back(timestamp);
if (timestamps.size() > history_size)
{
timestamps.erase(timestamps.begin());
}
double max_age = timestamps.front();
for (size_t i = 0; i < pose_tracks.size();)
{
auto &track = pose_tracks[i];
for (size_t j = 0; j < track.timestamps.size();)
{
double ts = track.timestamps[j];
if (ts < max_age)
{
track.core_poses.erase(track.core_poses.begin() + j);
track.full_poses.erase(track.full_poses.begin() + j);
track.timestamps.erase(track.timestamps.begin() + j);
}
else
{
j++;
}
}
if (track.timestamps.size() == 0)
{
pose_tracks.erase(pose_tracks.begin() + i);
}
else
{
++i;
}
}
// Refine poses
std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> tracked_poses;
for (size_t i = 0; i < pose_tracks.size(); ++i)
{
auto &track = pose_tracks[i];
// Create a refined pose for current tracks, or old tracks with a bit history,
// to avoid continuing tracks of flickering persons
if (track.core_poses.size() >= std::ceil(history_size / 2.0) ||
track.timestamps.back() == timestamps.back())
{
std::vector<std::array<float, 4>> refined_pose = refine_pose(track);
tracked_poses.emplace_back(track.id, refined_pose);
}
}
return tracked_poses;
}
// =================================================================================================
std::tuple<int, float> PoseTracker::match_to_track(
const std::vector<std::array<float, 4>> &core_pose_3d)
{
int best_track = -1;
float best_distance_sq = max_distance * max_distance;
for (size_t i = 0; i < pose_tracks.size(); ++i)
{
const auto &track = pose_tracks[i];
if (track.core_poses.size() == 0)
{
continue;
}
// Calculate distance to the last pose in the track
const auto &last_pose = track.core_poses.back();
float distance_sq = 0.0;
for (size_t j = 0; j < core_pose_3d.size(); ++j)
{
float dx = core_pose_3d[j][0] - last_pose[j][0];
float dy = core_pose_3d[j][1] - last_pose[j][1];
float dz = core_pose_3d[j][2] - last_pose[j][2];
distance_sq += dx * dx + dy * dy + dz * dz;
}
distance_sq /= core_pose_3d.size();
if (distance_sq < best_distance_sq)
{
best_distance_sq = distance_sq;
best_track = static_cast<int>(i);
}
}
return {best_track, best_distance_sq};
}
// =================================================================================================
std::vector<std::array<float, 4>> PoseTracker::refine_pose(const Track &track)
{
// Restrict maximum movement by physical constraints, by clipping the pose to the maximum
// movement distance from one of the track's history poses
//
// While advanced sensor filtering techniques, like using a Kalman-Filter, might improve the
// average accuracy of the pose, they introduce update delays on fast movement changes. For
// example, if a person stands still for a while and then suddenly moves, the first few frames
// will likely be treated as outliers, since the filter will not be able to adapt fast enough.
// This behaviour is not desired in a real-time critical applications, where the pose needs to
// be updated to the real physical position of the person as fast as possible. Therefore, only
// the movement speed is limited here.
if (track.core_poses.size() < 2)
{
return track.full_poses.back();
}
// Calculate maximum possible movement distance from each history pose
std::vector<float> max_movement_dists_sq;
max_movement_dists_sq.resize(track.core_poses.size() - 1);
double last_timestamp = track.timestamps.back();
for (size_t i = 0; i < track.core_poses.size() - 1; ++i)
{
double ts = track.timestamps[i];
float max_movement = max_movement_speed * (last_timestamp - ts);
max_movement_dists_sq[i] = max_movement * max_movement;
}
// Clip joint if required
std::vector<std::array<float, 4>> refined_pose = track.full_poses.back();
for (size_t i = 0; i < refined_pose.size(); ++i)
{
float min_dist_sq = std::numeric_limits<float>::infinity();
size_t closest_idx = 0;
bool clip = true;
for (size_t j = 0; j < max_movement_dists_sq.size(); ++j)
{
float dx = refined_pose[i][0] - track.full_poses[j][i][0];
float dy = refined_pose[i][1] - track.full_poses[j][i][1];
float dz = refined_pose[i][2] - track.full_poses[j][i][2];
float dist_sq = dx * dx + dy * dy + dz * dz;
if (dist_sq < min_dist_sq)
{
min_dist_sq = dist_sq;
closest_idx = j;
}
if (dist_sq <= max_movement_dists_sq[j])
{
clip = false;
break;
}
}
if (clip)
{
float dist_sq = min_dist_sq;
float scale = max_movement_dists_sq[closest_idx] / dist_sq;
float dx = refined_pose[i][0] - track.full_poses[closest_idx][i][0];
float dy = refined_pose[i][1] - track.full_poses[closest_idx][i][1];
float dz = refined_pose[i][2] - track.full_poses[closest_idx][i][2];
refined_pose[i][0] = track.full_poses[closest_idx][i][0] + dx * scale;
refined_pose[i][1] = track.full_poses[closest_idx][i][1] + dy * scale;
refined_pose[i][2] = track.full_poses[closest_idx][i][2] + dz * scale;
// Set confidence to a low value if the joint is clipped
refined_pose[i][3] = 0.1;
}
}
return refined_pose;
}
rpt/triangulator.cpp
+178 -208
View File
@@ -7,6 +7,7 @@
#include <numeric>
#include <set>
#include <stdexcept>
#include <string_view>
#include <unordered_map>
#include "interface.hpp"
@@ -222,158 +223,126 @@ private:
std::vector<float> data;
};
class TriangulatorInternal
struct PreparedInputs
{
public:
TriangulatorInternal(float min_match_score, size_t min_group_size)
: min_match_score(min_match_score), min_group_size(min_group_size)
std::vector<CachedCamera> internal_cameras;
std::vector<size_t> core_joint_idx;
std::vector<std::array<size_t, 2>> core_limbs_idx;
PackedPoseStore2D packed_poses;
PreparedInputs(
std::vector<CachedCamera> cameras_in,
std::vector<size_t> core_joint_idx_in,
std::vector<std::array<size_t, 2>> core_limbs_idx_in,
PackedPoseStore2D packed_poses_in)
: internal_cameras(std::move(cameras_in)),
core_joint_idx(std::move(core_joint_idx_in)),
core_limbs_idx(std::move(core_limbs_idx_in)),
packed_poses(std::move(packed_poses_in))
{
}
TriangulationTrace triangulate_debug(
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,
const PoseBatch3DView *previous_poses_3d);
PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names,
const PoseBatch3DView &previous_poses_3d);
private:
struct PreparedInputs
{
std::vector<CachedCamera> internal_cameras;
std::vector<size_t> core_joint_idx;
std::vector<std::array<size_t, 2>> core_limbs_idx;
PackedPoseStore2D packed_poses;
PreparedInputs(
std::vector<CachedCamera> cameras_in,
std::vector<size_t> core_joint_idx_in,
std::vector<std::array<size_t, 2>> core_limbs_idx_in,
PackedPoseStore2D packed_poses_in)
: internal_cameras(std::move(cameras_in)),
core_joint_idx(std::move(core_joint_idx_in)),
core_limbs_idx(std::move(core_limbs_idx_in)),
packed_poses(std::move(packed_poses_in))
{
}
};
struct PreviousProjectionCache
{
std::vector<std::vector<Pose2D>> projected_poses;
std::vector<std::vector<std::vector<float>>> projected_dists;
std::vector<Pose3D> core_poses;
};
PreparedInputs prepare_inputs(
const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names);
std::vector<PairCandidate> build_pair_candidates(const PackedPoseStore2D &packed_poses) const;
PreviousProjectionCache project_previous_poses(
const PoseBatch3DView &previous_poses_3d,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<size_t> &core_joint_idx);
PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const PackedPoseStore2D &packed_poses,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs,
const PoseBatch3DView &previous_poses_3d);
float calc_pose_score(
const Pose2D &pose,
const Pose2D &reference_pose,
const std::vector<float> &dists,
const CachedCamera &icam);
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam,
bool calc_dists);
std::vector<float> score_projection(
const Pose2D &pose,
const Pose2D &repro,
const std::vector<float> &dists,
const std::vector<bool> &mask,
float iscale);
std::pair<Pose3D, float> triangulate_and_score(
const Pose2D &pose1,
const Pose2D &pose2,
const CachedCamera &cam1,
const CachedCamera &cam2,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<GroupedPose> calc_grouping(
const std::vector<PosePair> &all_pairs,
const std::vector<std::pair<Pose3D, float>> &all_scored_poses,
float min_score);
Pose3D merge_group(
const std::vector<Pose3D> &poses_3d,
float min_score);
void add_extra_joints(
std::vector<Pose3D> &poses,
const std::vector<std::string> &joint_names);
void filter_poses(
std::vector<Pose3D> &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<Pose3D> &poses,
const std::vector<std::string> &joint_names);
void replace_far_joints(
std::vector<Pose3D> &poses,
const std::vector<std::string> &joint_names);
float min_match_score;
size_t min_group_size;
size_t num_cams = 0;
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"},
};
};
TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
struct PreviousProjectionCache
{
std::vector<std::vector<Pose2D>> projected_poses;
std::vector<std::vector<std::vector<float>>> projected_dists;
std::vector<Pose3D> core_poses;
};
constexpr std::array<std::string_view, 12> kCoreJoints = {
"shoulder_left",
"shoulder_right",
"hip_left",
"hip_right",
"elbow_left",
"elbow_right",
"knee_left",
"knee_right",
"wrist_left",
"wrist_right",
"ankle_left",
"ankle_right",
};
constexpr std::array<std::pair<std::string_view, std::string_view>, 8> kCoreLimbs = {{
{"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<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses);
PreviousProjectionCache project_previous_poses(
const PoseBatch3DView &previous_poses_3d,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<size_t> &core_joint_idx);
PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
const PackedPoseStore2D &packed_poses,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs,
const TriangulationOptions &options,
const PoseBatch3DView &previous_poses_3d);
float calc_pose_score(
const Pose2D &pose,
const Pose2D &reference_pose,
const std::vector<float> &dists,
const CachedCamera &icam);
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam,
bool calc_dists);
std::vector<float> score_projection(
const Pose2D &pose,
const Pose2D &repro,
const std::vector<float> &dists,
const std::vector<bool> &mask,
float iscale);
std::pair<Pose3D, float> triangulate_and_score(
const Pose2D &pose1,
const Pose2D &pose2,
const CachedCamera &cam1,
const CachedCamera &cam2,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<GroupedPose> calc_grouping(
const std::vector<PosePair> &all_pairs,
const std::vector<std::pair<Pose3D, float>> &all_scored_poses,
float min_score);
Pose3D merge_group(
const std::vector<Pose3D> &poses_3d,
float min_score,
size_t num_cams);
void add_extra_joints(
std::vector<Pose3D> &poses,
const std::vector<std::string> &joint_names);
void filter_poses(
std::vector<Pose3D> &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<Pose3D> &poses,
const std::vector<std::string> &joint_names,
float min_match_score);
void replace_far_joints(
std::vector<Pose3D> &poses,
const std::vector<std::string> &joint_names,
float min_match_score);
TriangulationTrace triangulate_debug_impl(
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,
const PoseBatch3DView *previous_poses_3d,
const TriangulationOptions &options);
PreparedInputs prepare_inputs(
const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names)
@@ -402,16 +371,14 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
{
throw std::invalid_argument("poses_2d data must not be null.");
}
for (const auto &joint : core_joints)
for (const auto &joint : kCoreJoints)
{
if (std::find(joint_names.begin(), joint_names.end(), joint) == joint_names.end())
{
throw std::invalid_argument("Core joint '" + joint + "' not found in joint names.");
throw std::invalid_argument("Core joint '" + std::string(joint) + "' not found in joint names.");
}
}
num_cams = cameras.size();
std::vector<CachedCamera> internal_cameras;
internal_cameras.reserve(cameras.size());
for (const auto &camera : cameras)
@@ -420,22 +387,22 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
}
std::vector<size_t> core_joint_idx;
core_joint_idx.reserve(core_joints.size());
for (const auto &joint : core_joints)
core_joint_idx.reserve(kCoreJoints.size());
for (const auto &joint : kCoreJoints)
{
auto it = std::find(joint_names.begin(), joint_names.end(), joint);
core_joint_idx.push_back(static_cast<size_t>(std::distance(joint_names.begin(), it)));
}
std::vector<std::array<size_t, 2>> core_limbs_idx;
core_limbs_idx.reserve(core_limbs.size());
for (const auto &limb : core_limbs)
core_limbs_idx.reserve(kCoreLimbs.size());
for (const auto &limb : kCoreLimbs)
{
auto it1 = std::find(core_joints.begin(), core_joints.end(), limb.first);
auto it2 = std::find(core_joints.begin(), core_joints.end(), limb.second);
auto it1 = std::find(kCoreJoints.begin(), kCoreJoints.end(), limb.first);
auto it2 = std::find(kCoreJoints.begin(), kCoreJoints.end(), limb.second);
core_limbs_idx.push_back(
{static_cast<size_t>(std::distance(core_joints.begin(), it1)),
static_cast<size_t>(std::distance(core_joints.begin(), it2))});
{static_cast<size_t>(std::distance(kCoreJoints.begin(), it1)),
static_cast<size_t>(std::distance(kCoreJoints.begin(), it2))});
}
PackedPoseStore2D packed_poses(poses_2d, internal_cameras);
@@ -446,7 +413,7 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
std::move(packed_poses));
}
std::vector<PairCandidate> TriangulatorInternal::build_pair_candidates(const PackedPoseStore2D &packed_poses) const
std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses)
{
std::vector<PairCandidate> pairs;
for (size_t view1 = 0; view1 < packed_poses.num_views; ++view1)
@@ -472,7 +439,7 @@ std::vector<PairCandidate> TriangulatorInternal::build_pair_candidates(const Pac
return pairs;
}
TriangulatorInternal::PreviousProjectionCache TriangulatorInternal::project_previous_poses(
PreviousProjectionCache project_previous_poses(
const PoseBatch3DView &previous_poses_3d,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<size_t> &core_joint_idx)
@@ -505,7 +472,7 @@ TriangulatorInternal::PreviousProjectionCache TriangulatorInternal::project_prev
return cache;
}
float TriangulatorInternal::calc_pose_score(
float calc_pose_score(
const Pose2D &pose,
const Pose2D &reference_pose,
const std::vector<float> &dists,
@@ -543,11 +510,12 @@ float TriangulatorInternal::calc_pose_score(
return total / static_cast<float>(scores.size() - drop_k);
}
PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
const PackedPoseStore2D &packed_poses,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs,
const TriangulationOptions &options,
const PoseBatch3DView &previous_poses_3d)
{
PreviousPoseFilterDebug debug;
@@ -568,7 +536,7 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
const PreviousProjectionCache projected_previous =
project_previous_poses(previous_poses_3d, internal_cameras, core_joint_idx);
const float threshold = min_match_score;
const float threshold = options.min_match_score;
for (size_t pair_index = 0; pair_index < pairs.size(); ++pair_index)
{
@@ -643,12 +611,13 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
return debug;
}
TriangulationTrace TriangulatorInternal::triangulate_debug(
TriangulationTrace triangulate_debug_impl(
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,
const PoseBatch3DView *previous_poses_3d)
const PoseBatch3DView *previous_poses_3d,
const TriangulationOptions &options)
{
TriangulationTrace trace;
trace.final_poses.num_joints = joint_names.size();
@@ -660,7 +629,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
}
PreparedInputs prepared = prepare_inputs(poses_2d, cameras, joint_names);
trace.pairs = build_pair_candidates(prepared.packed_poses);
trace.pairs = build_pair_candidates_from_packed(prepared.packed_poses);
if (trace.pairs.empty())
{
return trace;
@@ -672,11 +641,12 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
if (previous_poses_3d != nullptr)
{
trace.previous_filter = filter_pairs_with_previous_poses(
trace.previous_filter = filter_pairs_with_previous_poses_impl(
prepared.packed_poses,
prepared.internal_cameras,
prepared.core_joint_idx,
trace.pairs,
options,
*previous_poses_3d);
active_pair_indices = trace.previous_filter.kept_pair_indices;
active_pairs = trace.previous_filter.kept_pairs;
@@ -708,7 +678,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
proposal.pair = pair;
proposal.pose_3d = pose3d;
proposal.score = score;
proposal.kept = score >= min_match_score;
proposal.kept = score >= options.min_match_score;
proposal.drop_reason = proposal.kept ? "kept" : "below_min_match_score";
trace.core_proposals.push_back(proposal);
@@ -729,7 +699,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
return trace;
}
auto groups = calc_grouping(kept_pose_pairs, kept_scored_poses, min_match_score);
auto groups = calc_grouping(kept_pose_pairs, kept_scored_poses, options.min_match_score);
trace.grouping.initial_groups.reserve(groups.size());
for (const auto &group : groups)
{
@@ -745,7 +715,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
for (size_t i = groups.size(); i > 0; --i)
{
if (std::get<2>(groups[i - 1]).size() < min_group_size)
if (std::get<2>(groups[i - 1]).size() < options.min_group_size)
{
groups.erase(groups.begin() + static_cast<std::ptrdiff_t>(i - 1));
}
@@ -842,7 +812,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
for (size_t i = groups.size(); i > 0; --i)
{
if (std::get<2>(groups[i - 1]).size() < min_group_size)
if (std::get<2>(groups[i - 1]).size() < options.min_group_size)
{
groups.erase(groups.begin() + static_cast<std::ptrdiff_t>(i - 1));
}
@@ -923,15 +893,15 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
poses.push_back(full_pose_buffer[static_cast<size_t>(index)]);
source_core_indices.push_back(kept_core_indices[static_cast<size_t>(index)]);
}
final_poses_3d[i] = merge_group(poses, min_match_score);
final_poses_3d[i] = merge_group(poses, options.min_match_score, prepared.internal_cameras.size());
trace.merge.group_proposal_indices.push_back(std::move(source_core_indices));
trace.merge.merged_poses.push_back(final_poses_3d[i]);
}
add_extra_joints(final_poses_3d, joint_names);
filter_poses(final_poses_3d, roomparams, prepared.core_joint_idx, prepared.core_limbs_idx);
add_missing_joints(final_poses_3d, joint_names);
replace_far_joints(final_poses_3d, joint_names);
add_missing_joints(final_poses_3d, joint_names, options.min_match_score);
replace_far_joints(final_poses_3d, joint_names, options.min_match_score);
size_t valid_persons = 0;
for (const auto &pose : final_poses_3d)
@@ -939,9 +909,9 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
const bool is_valid = std::any_of(
pose.begin(),
pose.end(),
[this](const std::array<float, 4> &joint)
[&options](const std::array<float, 4> &joint)
{
return joint[3] > min_match_score;
return joint[3] > options.min_match_score;
});
if (is_valid)
{
@@ -958,9 +928,9 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
const bool is_valid = std::any_of(
pose.begin(),
pose.end(),
[this](const std::array<float, 4> &joint)
[&options](const std::array<float, 4> &joint)
{
return joint[3] > min_match_score;
return joint[3] > options.min_match_score;
});
if (!is_valid)
{
@@ -980,11 +950,12 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
return trace;
}
PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names,
const PoseBatch3DView &previous_poses_3d)
const PoseBatch3DView &previous_poses_3d,
const TriangulationOptions &options)
{
if (previous_poses_3d.num_persons > 0 && previous_poses_3d.num_joints != joint_names.size())
{
@@ -992,7 +963,7 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
}
PreparedInputs prepared = prepare_inputs(poses_2d, cameras, joint_names);
const std::vector<PairCandidate> pairs = build_pair_candidates(prepared.packed_poses);
const std::vector<PairCandidate> pairs = build_pair_candidates_from_packed(prepared.packed_poses);
if (pairs.empty())
{
PreviousPoseFilterDebug empty;
@@ -1000,15 +971,16 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
return empty;
}
return filter_pairs_with_previous_poses(
return filter_pairs_with_previous_poses_impl(
prepared.packed_poses,
prepared.internal_cameras,
prepared.core_joint_idx,
pairs,
options,
previous_poses_3d);
}
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> TriangulatorInternal::project_poses(
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam,
bool calc_dists)
@@ -1082,7 +1054,7 @@ std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> TriangulatorInt
return std::make_tuple(poses_2d, all_dists);
}
std::vector<float> TriangulatorInternal::score_projection(
std::vector<float> score_projection(
const Pose2D &pose,
const Pose2D &repro,
const std::vector<float> &dists,
@@ -1231,7 +1203,7 @@ std::array<float, 4> triangulate_midpoint(
// =================================================================================================
std::pair<std::vector<std::array<float, 4>>, float> TriangulatorInternal::triangulate_and_score(
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 CachedCamera &cam1,
@@ -1424,7 +1396,7 @@ std::pair<std::vector<std::array<float, 4>>, float> TriangulatorInternal::triang
// =================================================================================================
std::vector<std::tuple<std::array<float, 3>, std::vector<std::array<float, 4>>, std::vector<int>>>
TriangulatorInternal::calc_grouping(
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)
@@ -1692,9 +1664,10 @@ TriangulatorInternal::calc_grouping(
// =================================================================================================
std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
std::vector<std::array<float, 4>> merge_group(
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
float min_score)
float min_score,
size_t num_cams)
{
size_t num_poses = poses_3d.size();
size_t num_joints = poses_3d[0].size();
@@ -1794,7 +1767,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
}
// Select the best-k proposals for each joint that are closest to the initial pose
int keep_best = std::max((int)std::ceil(this->num_cams / 2.0), (int)std::ceil(num_poses / 3.0));
int keep_best = std::max((int)std::ceil(num_cams / 2.0), (int)std::ceil(num_poses / 3.0));
std::vector<std::vector<bool>> best_k_mask(num_poses, std::vector<bool>(num_joints, false));
for (size_t j = 0; j < num_joints; ++j)
{
@@ -1869,7 +1842,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
// =================================================================================================
void TriangulatorInternal::add_extra_joints(
void add_extra_joints(
std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names)
{
@@ -1900,7 +1873,7 @@ void TriangulatorInternal::add_extra_joints(
// =================================================================================================
void TriangulatorInternal::filter_poses(
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,
@@ -2091,9 +2064,10 @@ void TriangulatorInternal::filter_poses(
// =================================================================================================
void TriangulatorInternal::add_missing_joints(
void add_missing_joints(
std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names)
const std::vector<std::string> &joint_names,
float min_match_score)
{
// Map joint names to their indices for quick lookup
std::unordered_map<std::string, size_t> joint_name_to_idx;
@@ -2229,9 +2203,10 @@ void TriangulatorInternal::add_missing_joints(
// =================================================================================================
void TriangulatorInternal::replace_far_joints(
void replace_far_joints(
std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names)
const std::vector<std::string> &joint_names,
float min_match_score)
{
for (size_t i = 0; i < poses.size(); ++i)
{
@@ -2248,7 +2223,7 @@ void TriangulatorInternal::replace_far_joints(
{
const std::string &jname = joint_names[j];
float offset2 = (1.0 - min_match_score) * 2;
float min_score = this->min_match_score - offset2;
float min_score = min_match_score - offset2;
if (pose[j][3] > min_score)
{
@@ -2444,10 +2419,9 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const std::vector<Camera> &cameras,
const std::vector<std::string> &joint_names,
const PoseBatch3DView &previous_poses_3d,
float min_match_score)
const TriangulationOptions &options)
{
TriangulatorInternal triangulator(min_match_score, 1);
return triangulator.filter_pairs_with_previous_poses(poses_2d, cameras, joint_names, previous_poses_3d);
return filter_pairs_with_previous_poses_impl(poses_2d, cameras, joint_names, previous_poses_3d, options);
}
TriangulationTrace triangulate_debug(
@@ -2456,11 +2430,9 @@ TriangulationTrace triangulate_debug(
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3DView *previous_poses_3d,
float min_match_score,
size_t min_group_size)
const TriangulationOptions &options)
{
TriangulatorInternal triangulator(min_match_score, min_group_size);
return triangulator.triangulate_debug(poses_2d, cameras, roomparams, joint_names, previous_poses_3d);
return triangulate_debug_impl(poses_2d, cameras, roomparams, joint_names, previous_poses_3d, options);
}
PoseBatch3D triangulate_poses(
@@ -2469,8 +2441,7 @@ PoseBatch3D triangulate_poses(
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3DView *previous_poses_3d,
float min_match_score,
size_t min_group_size)
const TriangulationOptions &options)
{
return triangulate_debug(
poses_2d,
@@ -2478,7 +2449,6 @@ PoseBatch3D triangulate_poses(
roomparams,
joint_names,
previous_poses_3d,
min_match_score,
min_group_size)
options)
.final_poses;
}