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

Unify camera model into immutable factory-built type

Browse Source
This commit is contained in:
crosstyan
2026-03-12 00:24:02 +08:00
parent c23f25f871
commit 31c4690121
8 changed files with 177 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rpt/triangulator.cpp
+44 -111
View File
@@ -11,7 +11,6 @@
#include <unordered_map>
#include "interface.hpp"
#include "cached_camera.hpp"
namespace
{
@@ -25,73 +24,14 @@ size_t packed_pose_offset(size_t pose, size_t joint, size_t coord, size_t num_jo
return (((pose * num_joints) + joint) * 3) + coord;
}
[[maybe_unused]] static void print_2d_poses(const Pose2D &poses)
{
std::cout << "Poses: (" << poses.size() << ", 3)[" << std::endl;
for (const auto &pose : poses)
{
std::cout << " [";
for (size_t i = 0; i < 3; ++i)
{
std::cout << std::fixed << std::setprecision(3) << pose[i];
if (i < 2)
{
std::cout << ", ";
}
}
std::cout << "]" << std::endl;
}
std::cout << "]" << std::endl;
}
[[maybe_unused]] static void print_3d_poses(const Pose3D &poses)
{
std::cout << "Poses: (" << poses.size() << ", 4)[" << std::endl;
for (const auto &pose : poses)
{
std::cout << " [";
for (size_t i = 0; i < 4; ++i)
{
std::cout << std::fixed << std::setprecision(3) << pose[i];
if (i < 3)
{
std::cout << ", ";
}
}
std::cout << "]" << std::endl;
}
std::cout << "]" << std::endl;
}
[[maybe_unused]] static void print_allpairs(const std::vector<PosePair> &all_pairs)
{
std::cout << "All pairs: [" << std::endl;
for (const auto &pair : all_pairs)
{
const auto &tuple_part = pair.first;
const auto &pair_part = pair.second;
std::cout << "("
<< std::get<0>(tuple_part) << ", "
<< std::get<1>(tuple_part) << ", "
<< std::get<2>(tuple_part) << ", "
<< std::get<3>(tuple_part) << ")";
std::cout << ", ("
<< pair_part.first << ", "
<< pair_part.second << ")"
<< std::endl;
}
std::cout << "]" << std::endl;
}
std::array<float, 3> undistort_point(
const std::array<float, 3> &raw_point, const CachedCamera &icam)
const std::array<float, 3> &raw_point, const Camera &icam)
{
std::array<float, 3> point = raw_point;
const float ifx_old = 1.0f / icam.cam.K[0][0];
const float ify_old = 1.0f / icam.cam.K[1][1];
const float cx_old = icam.cam.K[0][2];
const float cy_old = icam.cam.K[1][2];
const float ifx_old = 1.0f / icam.K[0][0];
const float ify_old = 1.0f / icam.K[1][1];
const float cx_old = icam.K[0][2];
const float cy_old = icam.K[1][2];
const float fx_new = icam.newK[0][0];
const float fy_new = icam.newK[1][1];
const float cx_new = icam.newK[0][2];
@@ -100,21 +40,21 @@ std::array<float, 3> undistort_point(
point[0] = (point[0] - cx_old) * ifx_old;
point[1] = (point[1] - cy_old) * ify_old;
if (icam.cam.model == CameraModel::Fisheye)
if (icam.model == CameraModel::Fisheye)
{
undistort_point_fisheye(point, icam.cam.DC);
undistort_point_fisheye(point, icam.DC);
}
else
{
undistort_point_pinhole(point, icam.cam.DC);
undistort_point_pinhole(point, icam.DC);
}
point[0] = (point[0] * fx_new) + cx_new;
point[1] = (point[1] * fy_new) + cy_new;
const float mask_offset = (icam.cam.width + icam.cam.height) / 20.0f;
if (point[0] < -mask_offset || point[0] >= icam.cam.width + mask_offset ||
point[1] < -mask_offset || point[1] >= icam.cam.height + mask_offset)
const float mask_offset = (icam.width + icam.height) / 20.0f;
if (point[0] < -mask_offset || point[0] >= icam.width + mask_offset ||
point[1] < -mask_offset || point[1] >= icam.height + mask_offset)
{
point = {0.0f, 0.0f, 0.0f};
}
@@ -127,7 +67,7 @@ class PackedPoseStore2D
public:
PackedPoseStore2D(
const PoseBatch2DView &source,
const std::vector<CachedCamera> &internal_cameras)
const std::vector<Camera> &internal_cameras)
: person_counts(source.num_views),
view_offsets(source.num_views),
num_views(source.num_views),
@@ -225,17 +165,17 @@ private:
struct PreparedInputs
{
std::vector<CachedCamera> internal_cameras;
const std::vector<Camera> *internal_cameras = nullptr;
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,
const std::vector<Camera> &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)),
: internal_cameras(&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))
@@ -279,11 +219,11 @@ constexpr std::array<std::pair<std::string_view, std::string_view>, 8> kCoreLimb
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<Camera> &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<Camera> &internal_cameras,
const std::vector<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs,
const TriangulationOptions &options,
@@ -292,10 +232,10 @@ float calc_pose_score(
const Pose2D &pose,
const Pose2D &reference_pose,
const std::vector<float> &dists,
const CachedCamera &icam);
const Camera &icam);
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam,
const Camera &icam,
bool calc_dists);
std::vector<float> score_projection(
const Pose2D &pose,
@@ -306,8 +246,8 @@ std::vector<float> score_projection(
std::pair<Pose3D, float> triangulate_and_score(
const Pose2D &pose1,
const Pose2D &pose2,
const CachedCamera &cam1,
const CachedCamera &cam2,
const Camera &cam1,
const Camera &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(
@@ -379,13 +319,6 @@ PreparedInputs prepare_inputs(
}
}
std::vector<CachedCamera> internal_cameras;
internal_cameras.reserve(cameras.size());
for (const auto &camera : cameras)
{
internal_cameras.push_back(cache_camera(camera));
}
std::vector<size_t> core_joint_idx;
core_joint_idx.reserve(kCoreJoints.size());
for (const auto &joint : kCoreJoints)
@@ -405,9 +338,9 @@ PreparedInputs prepare_inputs(
static_cast<size_t>(std::distance(kCoreJoints.begin(), it2))});
}
PackedPoseStore2D packed_poses(poses_2d, internal_cameras);
PackedPoseStore2D packed_poses(poses_2d, cameras);
return PreparedInputs(
std::move(internal_cameras),
cameras,
std::move(core_joint_idx),
std::move(core_limbs_idx),
std::move(packed_poses));
@@ -441,7 +374,7 @@ std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseSto
PreviousProjectionCache project_previous_poses(
const PoseBatch3DView &previous_poses_3d,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<Camera> &internal_cameras,
const std::vector<size_t> &core_joint_idx)
{
PreviousProjectionCache cache;
@@ -476,7 +409,7 @@ float calc_pose_score(
const Pose2D &pose,
const Pose2D &reference_pose,
const std::vector<float> &dists,
const CachedCamera &icam)
const Camera &icam)
{
const float min_score = 0.1f;
std::vector<bool> mask(pose.size(), false);
@@ -494,7 +427,7 @@ float calc_pose_score(
return 0.0f;
}
const float iscale = (icam.cam.width + icam.cam.height) * 0.5f;
const float iscale = (icam.width + icam.height) * 0.5f;
std::vector<float> scores = score_projection(pose, reference_pose, dists, mask, iscale);
const size_t drop_k = static_cast<size_t>(pose.size() * 0.2f);
if (drop_k > 0)
@@ -512,7 +445,7 @@ float calc_pose_score(
PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
const PackedPoseStore2D &packed_poses,
const std::vector<CachedCamera> &internal_cameras,
const std::vector<Camera> &internal_cameras,
const std::vector<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs,
const TriangulationOptions &options,
@@ -643,7 +576,7 @@ TriangulationTrace triangulate_debug_impl(
{
trace.previous_filter = filter_pairs_with_previous_poses_impl(
prepared.packed_poses,
prepared.internal_cameras,
*prepared.internal_cameras,
prepared.core_joint_idx,
trace.pairs,
options,
@@ -663,8 +596,8 @@ TriangulationTrace triangulate_debug_impl(
for (size_t i = 0; i < active_pairs.size(); ++i)
{
const PairCandidate &pair = active_pairs[i];
const CachedCamera &cam1 = prepared.internal_cameras[static_cast<size_t>(pair.view1)];
const CachedCamera &cam2 = prepared.internal_cameras[static_cast<size_t>(pair.view2)];
const Camera &cam1 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view1)];
const Camera &cam2 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view2)];
const Pose2D pose1_core = prepared.packed_poses.pose(
static_cast<size_t>(pair.view1), static_cast<size_t>(pair.person1), prepared.core_joint_idx);
const Pose2D pose2_core = prepared.packed_poses.pose(
@@ -849,8 +782,8 @@ TriangulationTrace triangulate_debug_impl(
kept_pose_pairs[i].second.first,
kept_pose_pairs[i].second.second,
};
const CachedCamera &cam1 = prepared.internal_cameras[static_cast<size_t>(pair.view1)];
const CachedCamera &cam2 = prepared.internal_cameras[static_cast<size_t>(pair.view2)];
const Camera &cam1 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view1)];
const Camera &cam2 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view2)];
Pose2D pose1 = prepared.packed_poses.pose(static_cast<size_t>(pair.view1), static_cast<size_t>(pair.person1));
Pose2D pose2 = prepared.packed_poses.pose(static_cast<size_t>(pair.view2), static_cast<size_t>(pair.person2));
@@ -893,7 +826,7 @@ TriangulationTrace triangulate_debug_impl(
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, options.min_match_score, prepared.internal_cameras.size());
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]);
}
@@ -973,7 +906,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
return filter_pairs_with_previous_poses_impl(
prepared.packed_poses,
prepared.internal_cameras,
*prepared.internal_cameras,
prepared.core_joint_idx,
pairs,
options,
@@ -982,7 +915,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam,
const Camera &icam,
bool calc_dists)
{
if (poses_3d.empty())
@@ -998,7 +931,7 @@ std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::array<std::array<float, 3>, 3> &K = icam.newK;
const std::array<std::array<float, 3>, 3> &R = icam.invR;
const std::array<std::array<float, 1>, 3> &T = icam.cam.T;
const std::array<std::array<float, 1>, 3> &T = icam.T;
for (size_t i = 0; i < num_persons; ++i)
{
@@ -1037,7 +970,7 @@ std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
v = (K[1][0] * x_cam + K[1][1] * y_cam + K[1][2] * z_cam) / z_cam;
}
if (u < 0.0f || u >= icam.cam.width || v < 0.0f || v >= icam.cam.height)
if (u < 0.0f || u >= icam.width || v < 0.0f || v >= icam.height)
{
u = 0.0f;
v = 0.0f;
@@ -1155,19 +1088,19 @@ std::array<float, 3> mat_mul_vec(
return res;
}
std::array<float, 3> calc_ray_dir(const CachedCamera &icam, const std::array<float, 2> &pt)
std::array<float, 3> calc_ray_dir(const Camera &icam, const std::array<float, 2> &pt)
{
// Compute normalized ray direction from the point
std::array<float, 3> uv1 = {pt[0], pt[1], 1.0};
auto d = mat_mul_vec(icam.cam.R, mat_mul_vec(icam.invK, uv1));
auto d = mat_mul_vec(icam.R, mat_mul_vec(icam.invK, uv1));
auto ray_dir = normalize(d);
return ray_dir;
}
std::array<float, 4> triangulate_midpoint(
const CachedCamera &icam1,
const CachedCamera &icam2,
const Camera &icam1,
const Camera &icam2,
const std::array<float, 2> &pt1,
const std::array<float, 2> &pt2)
{
@@ -1206,8 +1139,8 @@ std::array<float, 4> triangulate_midpoint(
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,
const CachedCamera &cam2,
const Camera &cam1,
const Camera &cam2,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx)
{
@@ -1307,7 +1240,7 @@ std::pair<std::vector<std::array<float, 4>>, float> triangulate_and_score(
auto dists2 = dists2s[0];
// Calculate scores for each view
float iscale = (cam1.cam.width + cam1.cam.height) / 2.0;
float iscale = (cam1.width + cam1.height) / 2.0f;
std::vector<float> score1 = score_projection(pose1, repro1, dists1, mask, iscale);
std::vector<float> score2 = score_projection(pose2, repro2, dists2, mask, iscale);