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
bindings/rpt_module.cpp
+60 -9
View File
@@ -139,15 +139,54 @@ NB_MODULE(_core, m)
.value("FISHEYE", CameraModel::Fisheye); .value("FISHEYE", CameraModel::Fisheye);
nb::class_<Camera>(m, "Camera") nb::class_<Camera>(m, "Camera")
.def(nb::init<>()) .def_prop_ro("name", [](const Camera &camera)
.def_rw("name", &Camera::name) {
.def_rw("K", &Camera::K) return camera.name;
.def_rw("DC", &Camera::DC) })
.def_rw("R", &Camera::R) .def_prop_ro("K", [](const Camera &camera)
.def_rw("T", &Camera::T) {
.def_rw("width", &Camera::width) return camera.K;
.def_rw("height", &Camera::height) })
.def_rw("model", &Camera::model) .def_prop_ro("DC", [](const Camera &camera)
{
return camera.DC;
})
.def_prop_ro("R", [](const Camera &camera)
{
return camera.R;
})
.def_prop_ro("T", [](const Camera &camera)
{
return camera.T;
})
.def_prop_ro("width", [](const Camera &camera)
{
return camera.width;
})
.def_prop_ro("height", [](const Camera &camera)
{
return camera.height;
})
.def_prop_ro("model", [](const Camera &camera)
{
return camera.model;
})
.def_prop_ro("invR", [](const Camera &camera)
{
return camera.invR;
})
.def_prop_ro("center", [](const Camera &camera)
{
return camera.center;
})
.def_prop_ro("newK", [](const Camera &camera)
{
return camera.newK;
})
.def_prop_ro("invK", [](const Camera &camera)
{
return camera.invK;
})
.def("__repr__", [](const Camera &camera) .def("__repr__", [](const Camera &camera)
{ {
return camera.to_string(); return camera.to_string();
@@ -248,6 +287,18 @@ NB_MODULE(_core, m)
return pose_batch_to_numpy_copy(trace.final_poses); return pose_batch_to_numpy_copy(trace.final_poses);
}, nb::rv_policy::move); }, nb::rv_policy::move);
m.def(
"make_camera",
&make_camera,
"name"_a,
"K"_a,
"DC"_a,
"R"_a,
"T"_a,
"width"_a,
"height"_a,
"model"_a);
m.def( m.def(
"build_pair_candidates", "build_pair_candidates",
[](const PoseArray2D &poses_2d, const CountArray &person_counts) [](const PoseArray2D &poses_2d, const CountArray &person_counts)
rpt/cached_camera.hpp
-19
View File
@@ -1,19 +0,0 @@
#pragma once
#include <array>
#include "camera.hpp"
struct CachedCamera
{
const Camera cam;
const std::array<std::array<float, 3>, 3> invR;
const std::array<float, 3> center;
const std::array<std::array<float, 3>, 3> newK;
const std::array<std::array<float, 3>, 3> invK;
};
CachedCamera cache_camera(const Camera &camera);
void undistort_point_pinhole(std::array<float, 3> &point, const std::array<float, 5> &distortion);
void undistort_point_fisheye(std::array<float, 3> &point, const std::array<float, 5> &distortion);
rpt/camera.cpp
+29 -11
View File
@@ -1,12 +1,12 @@
#include <array> #include <array>
#include <cmath> #include <cmath>
#include <iomanip> #include <iomanip>
#include <stdexcept>
#include <sstream> #include <sstream>
#include <stdexcept>
#include <utility>
#include <vector> #include <vector>
#include <cmath>
#include "cached_camera.hpp" #include "camera.hpp"
namespace namespace
{ {
@@ -156,28 +156,46 @@ std::ostream &operator<<(std::ostream &out, const Camera &cam)
// ================================================================================================= // =================================================================================================
// ================================================================================================= // =================================================================================================
CachedCamera cache_camera(const Camera &cam) Camera make_camera(
std::string name,
std::array<std::array<float, 3>, 3> K,
std::array<float, 5> DC,
std::array<std::array<float, 3>, 3> R,
std::array<std::array<float, 1>, 3> T,
int width,
int height,
CameraModel model)
{ {
const std::array<std::array<float, 3>, 3> invR = transpose3x3(cam.R); Camera cam {
.name = std::move(name),
.K = K,
.DC = DC,
.R = R,
.T = T,
.width = width,
.height = height,
.model = model,
};
cam.invR = transpose3x3(cam.R);
// Camera center: // Camera center:
// C = -(Rᵀ * t) = -(Rᵀ * (R * (T * -1))) = -(Rᵀ * (R * -T)) = -(Rᵀ * -R * T) = -(-T) = T // C = -(Rᵀ * t) = -(Rᵀ * (R * (T * -1))) = -(Rᵀ * (R * -T)) = -(Rᵀ * -R * T) = -(-T) = T
const std::array<float, 3> center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]}; cam.center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]};
// Undistort camera matrix // Undistort camera matrix
// As with the undistortion, the own implementation avoids some overhead compared to OpenCV // As with the undistortion, the own implementation avoids some overhead compared to OpenCV
std::array<std::array<float, 3>, 3> newK;
if (cam.model == CameraModel::Fisheye) if (cam.model == CameraModel::Fisheye)
{ {
newK = calc_optimal_camera_matrix_fisheye(cam, 1.0f, {cam.width, cam.height}); cam.newK = calc_optimal_camera_matrix_fisheye(cam, 1.0f, {cam.width, cam.height});
} }
else else
{ {
newK = calc_optimal_camera_matrix_pinhole(cam, 1.0f, {cam.width, cam.height}); cam.newK = calc_optimal_camera_matrix_pinhole(cam, 1.0f, {cam.width, cam.height});
} }
const std::array<std::array<float, 3>, 3> invK = invert3x3(newK); cam.invK = invert3x3(cam.newK);
return CachedCamera {cam, invR, center, newK, invK}; return cam;
} }
// ================================================================================================= // =================================================================================================
rpt/camera.hpp
+19 -2
View File
@@ -25,10 +25,27 @@ struct Camera
std::array<float, 5> DC = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; std::array<float, 5> DC = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
std::array<std::array<float, 3>, 3> R; std::array<std::array<float, 3>, 3> R;
std::array<std::array<float, 1>, 3> T; std::array<std::array<float, 1>, 3> T;
int width; int width = 0;
int height; int height = 0;
CameraModel model = CameraModel::Pinhole; CameraModel model = CameraModel::Pinhole;
std::array<std::array<float, 3>, 3> invR {};
std::array<float, 3> center {};
std::array<std::array<float, 3>, 3> newK {};
std::array<std::array<float, 3>, 3> invK {};
friend std::ostream &operator<<(std::ostream &out, Camera const &camera); friend std::ostream &operator<<(std::ostream &out, Camera const &camera);
std::string to_string() const; std::string to_string() const;
}; };
Camera make_camera(
std::string name,
std::array<std::array<float, 3>, 3> K,
std::array<float, 5> DC,
std::array<std::array<float, 3>, 3> R,
std::array<std::array<float, 1>, 3> T,
int width,
int height,
CameraModel model);
void undistort_point_pinhole(std::array<float, 3> &point, const std::array<float, 5> &distortion);
void undistort_point_fisheye(std::array<float, 3> &point, const std::array<float, 5> &distortion);
rpt/triangulator.cpp
+44 -111
View File
@@ -11,7 +11,6 @@
#include <unordered_map> #include <unordered_map>
#include "interface.hpp" #include "interface.hpp"
#include "cached_camera.hpp"
namespace 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; 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( 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; std::array<float, 3> point = raw_point;
const float ifx_old = 1.0f / icam.cam.K[0][0]; const float ifx_old = 1.0f / icam.K[0][0];
const float ify_old = 1.0f / icam.cam.K[1][1]; const float ify_old = 1.0f / icam.K[1][1];
const float cx_old = icam.cam.K[0][2]; const float cx_old = icam.K[0][2];
const float cy_old = icam.cam.K[1][2]; const float cy_old = icam.K[1][2];
const float fx_new = icam.newK[0][0]; const float fx_new = icam.newK[0][0];
const float fy_new = icam.newK[1][1]; const float fy_new = icam.newK[1][1];
const float cx_new = icam.newK[0][2]; 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[0] = (point[0] - cx_old) * ifx_old;
point[1] = (point[1] - cy_old) * ify_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 else
{ {
undistort_point_pinhole(point, icam.cam.DC); undistort_point_pinhole(point, icam.DC);
} }
point[0] = (point[0] * fx_new) + cx_new; point[0] = (point[0] * fx_new) + cx_new;
point[1] = (point[1] * fy_new) + cy_new; point[1] = (point[1] * fy_new) + cy_new;
const float mask_offset = (icam.cam.width + icam.cam.height) / 20.0f; const float mask_offset = (icam.width + icam.height) / 20.0f;
if (point[0] < -mask_offset || point[0] >= icam.cam.width + mask_offset || if (point[0] < -mask_offset || point[0] >= icam.width + mask_offset ||
point[1] < -mask_offset || point[1] >= icam.cam.height + mask_offset) point[1] < -mask_offset || point[1] >= icam.height + mask_offset)
{ {
point = {0.0f, 0.0f, 0.0f}; point = {0.0f, 0.0f, 0.0f};
} }
@@ -127,7 +67,7 @@ class PackedPoseStore2D
public: public:
PackedPoseStore2D( PackedPoseStore2D(
const PoseBatch2DView &source, const PoseBatch2DView &source,
const std::vector<CachedCamera> &internal_cameras) const std::vector<Camera> &internal_cameras)
: person_counts(source.num_views), : person_counts(source.num_views),
view_offsets(source.num_views), view_offsets(source.num_views),
num_views(source.num_views), num_views(source.num_views),
@@ -225,17 +165,17 @@ private:
struct PreparedInputs struct PreparedInputs
{ {
std::vector<CachedCamera> internal_cameras; const std::vector<Camera> *internal_cameras = nullptr;
std::vector<size_t> core_joint_idx; std::vector<size_t> core_joint_idx;
std::vector<std::array<size_t, 2>> core_limbs_idx; std::vector<std::array<size_t, 2>> core_limbs_idx;
PackedPoseStore2D packed_poses; PackedPoseStore2D packed_poses;
PreparedInputs( PreparedInputs(
std::vector<CachedCamera> cameras_in, const std::vector<Camera> &cameras_in,
std::vector<size_t> core_joint_idx_in, std::vector<size_t> core_joint_idx_in,
std::vector<std::array<size_t, 2>> core_limbs_idx_in, std::vector<std::array<size_t, 2>> core_limbs_idx_in,
PackedPoseStore2D packed_poses_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_joint_idx(std::move(core_joint_idx_in)),
core_limbs_idx(std::move(core_limbs_idx_in)), core_limbs_idx(std::move(core_limbs_idx_in)),
packed_poses(std::move(packed_poses_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); std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses);
PreviousProjectionCache project_previous_poses( PreviousProjectionCache project_previous_poses(
const PoseBatch3DView &previous_poses_3d, 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); const std::vector<size_t> &core_joint_idx);
PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl( PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
const PackedPoseStore2D &packed_poses, 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<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs, const std::vector<PairCandidate> &pairs,
const TriangulationOptions &options, const TriangulationOptions &options,
@@ -292,10 +232,10 @@ float calc_pose_score(
const Pose2D &pose, const Pose2D &pose,
const Pose2D &reference_pose, const Pose2D &reference_pose,
const std::vector<float> &dists, const std::vector<float> &dists,
const CachedCamera &icam); const Camera &icam);
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses( std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d, const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam, const Camera &icam,
bool calc_dists); bool calc_dists);
std::vector<float> score_projection( std::vector<float> score_projection(
const Pose2D &pose, const Pose2D &pose,
@@ -306,8 +246,8 @@ std::vector<float> score_projection(
std::pair<Pose3D, float> triangulate_and_score( std::pair<Pose3D, float> triangulate_and_score(
const Pose2D &pose1, const Pose2D &pose1,
const Pose2D &pose2, const Pose2D &pose2,
const CachedCamera &cam1, const Camera &cam1,
const CachedCamera &cam2, const Camera &cam2,
const std::array<std::array<float, 3>, 2> &roomparams, const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx); const std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<GroupedPose> calc_grouping( 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; std::vector<size_t> core_joint_idx;
core_joint_idx.reserve(kCoreJoints.size()); core_joint_idx.reserve(kCoreJoints.size());
for (const auto &joint : kCoreJoints) for (const auto &joint : kCoreJoints)
@@ -405,9 +338,9 @@ PreparedInputs prepare_inputs(
static_cast<size_t>(std::distance(kCoreJoints.begin(), it2))}); static_cast<size_t>(std::distance(kCoreJoints.begin(), it2))});
} }
PackedPoseStore2D packed_poses(poses_2d, internal_cameras); PackedPoseStore2D packed_poses(poses_2d, cameras);
return PreparedInputs( return PreparedInputs(
std::move(internal_cameras), cameras,
std::move(core_joint_idx), std::move(core_joint_idx),
std::move(core_limbs_idx), std::move(core_limbs_idx),
std::move(packed_poses)); std::move(packed_poses));
@@ -441,7 +374,7 @@ std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseSto
PreviousProjectionCache project_previous_poses( PreviousProjectionCache project_previous_poses(
const PoseBatch3DView &previous_poses_3d, 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) const std::vector<size_t> &core_joint_idx)
{ {
PreviousProjectionCache cache; PreviousProjectionCache cache;
@@ -476,7 +409,7 @@ float calc_pose_score(
const Pose2D &pose, const Pose2D &pose,
const Pose2D &reference_pose, const Pose2D &reference_pose,
const std::vector<float> &dists, const std::vector<float> &dists,
const CachedCamera &icam) const Camera &icam)
{ {
const float min_score = 0.1f; const float min_score = 0.1f;
std::vector<bool> mask(pose.size(), false); std::vector<bool> mask(pose.size(), false);
@@ -494,7 +427,7 @@ float calc_pose_score(
return 0.0f; 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); 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); const size_t drop_k = static_cast<size_t>(pose.size() * 0.2f);
if (drop_k > 0) if (drop_k > 0)
@@ -512,7 +445,7 @@ float calc_pose_score(
PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl( PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
const PackedPoseStore2D &packed_poses, 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<size_t> &core_joint_idx,
const std::vector<PairCandidate> &pairs, const std::vector<PairCandidate> &pairs,
const TriangulationOptions &options, const TriangulationOptions &options,
@@ -643,7 +576,7 @@ TriangulationTrace triangulate_debug_impl(
{ {
trace.previous_filter = filter_pairs_with_previous_poses_impl( trace.previous_filter = filter_pairs_with_previous_poses_impl(
prepared.packed_poses, prepared.packed_poses,
prepared.internal_cameras, *prepared.internal_cameras,
prepared.core_joint_idx, prepared.core_joint_idx,
trace.pairs, trace.pairs,
options, options,
@@ -663,8 +596,8 @@ TriangulationTrace triangulate_debug_impl(
for (size_t i = 0; i < active_pairs.size(); ++i) for (size_t i = 0; i < active_pairs.size(); ++i)
{ {
const PairCandidate &pair = active_pairs[i]; const PairCandidate &pair = active_pairs[i];
const CachedCamera &cam1 = prepared.internal_cameras[static_cast<size_t>(pair.view1)]; const Camera &cam1 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view1)];
const CachedCamera &cam2 = prepared.internal_cameras[static_cast<size_t>(pair.view2)]; const Camera &cam2 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view2)];
const Pose2D pose1_core = prepared.packed_poses.pose( const Pose2D pose1_core = prepared.packed_poses.pose(
static_cast<size_t>(pair.view1), static_cast<size_t>(pair.person1), prepared.core_joint_idx); static_cast<size_t>(pair.view1), static_cast<size_t>(pair.person1), prepared.core_joint_idx);
const Pose2D pose2_core = prepared.packed_poses.pose( 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.first,
kept_pose_pairs[i].second.second, kept_pose_pairs[i].second.second,
}; };
const CachedCamera &cam1 = prepared.internal_cameras[static_cast<size_t>(pair.view1)]; const Camera &cam1 = (*prepared.internal_cameras)[static_cast<size_t>(pair.view1)];
const CachedCamera &cam2 = prepared.internal_cameras[static_cast<size_t>(pair.view2)]; 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 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)); 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)]); poses.push_back(full_pose_buffer[static_cast<size_t>(index)]);
source_core_indices.push_back(kept_core_indices[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.group_proposal_indices.push_back(std::move(source_core_indices));
trace.merge.merged_poses.push_back(final_poses_3d[i]); 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( return filter_pairs_with_previous_poses_impl(
prepared.packed_poses, prepared.packed_poses,
prepared.internal_cameras, *prepared.internal_cameras,
prepared.core_joint_idx, prepared.core_joint_idx,
pairs, pairs,
options, options,
@@ -982,7 +915,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses( std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
const std::vector<Pose3D> &poses_3d, const std::vector<Pose3D> &poses_3d,
const CachedCamera &icam, const Camera &icam,
bool calc_dists) bool calc_dists)
{ {
if (poses_3d.empty()) 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> &K = icam.newK;
const std::array<std::array<float, 3>, 3> &R = icam.invR; 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) 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; 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; u = 0.0f;
v = 0.0f; v = 0.0f;
@@ -1155,19 +1088,19 @@ std::array<float, 3> mat_mul_vec(
return res; 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 // Compute normalized ray direction from the point
std::array<float, 3> uv1 = {pt[0], pt[1], 1.0}; 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); auto ray_dir = normalize(d);
return ray_dir; return ray_dir;
} }
std::array<float, 4> triangulate_midpoint( std::array<float, 4> triangulate_midpoint(
const CachedCamera &icam1, const Camera &icam1,
const CachedCamera &icam2, const Camera &icam2,
const std::array<float, 2> &pt1, const std::array<float, 2> &pt1,
const std::array<float, 2> &pt2) 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( 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>> &pose1,
const std::vector<std::array<float, 3>> &pose2, const std::vector<std::array<float, 3>> &pose2,
const CachedCamera &cam1, const Camera &cam1,
const CachedCamera &cam2, const Camera &cam2,
const std::array<std::array<float, 3>, 2> &roomparams, const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx) 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]; auto dists2 = dists2s[0];
// Calculate scores for each view // 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> score1 = score_projection(pose1, repro1, dists1, mask, iscale);
std::vector<float> score2 = score_projection(pose2, repro2, dists2, mask, iscale); std::vector<float> score2 = score_projection(pose2, repro2, dists2, mask, iscale);
src/rpt/__init__.py
+2
View File
@@ -19,6 +19,7 @@ from ._core import (
TriangulationTrace, TriangulationTrace,
build_pair_candidates, build_pair_candidates,
filter_pairs_with_previous_poses, filter_pairs_with_previous_poses,
make_camera,
triangulate_debug, triangulate_debug,
triangulate_poses, triangulate_poses,
) )
@@ -80,6 +81,7 @@ __all__ = [
"build_pair_candidates", "build_pair_candidates",
"convert_cameras", "convert_cameras",
"filter_pairs_with_previous_poses", "filter_pairs_with_previous_poses",
"make_camera",
"make_triangulation_config", "make_triangulation_config",
"pack_poses_2d", "pack_poses_2d",
"triangulate_debug", "triangulate_debug",
src/rpt/_helpers.py
+13 -11
View File
@@ -6,7 +6,7 @@ from typing import Literal, TypeAlias, TypedDict
import numpy as np import numpy as np
import numpy.typing as npt import numpy.typing as npt
from ._core import Camera, CameraModel, TriangulationConfig, TriangulationOptions from ._core import Camera, CameraModel, TriangulationConfig, TriangulationOptions, make_camera
Matrix3x3Like: TypeAlias = Sequence[Sequence[float]] Matrix3x3Like: TypeAlias = Sequence[Sequence[float]]
VectorLike: TypeAlias = Sequence[float] VectorLike: TypeAlias = Sequence[float]
@@ -64,17 +64,19 @@ def convert_cameras(cameras: Sequence[CameraLike]) -> list[Camera]:
converted.append(cam) converted.append(cam)
continue continue
camera = Camera()
camera.name = str(cam["name"])
camera.K = cam["K"]
camera_model = _coerce_camera_model(cam.get("model", cam.get("type", "pinhole"))) camera_model = _coerce_camera_model(cam.get("model", cam.get("type", "pinhole")))
camera.DC = _coerce_distortion(cam["DC"], camera_model) converted.append(
camera.R = cam["R"] make_camera(
camera.T = cam["T"] str(cam["name"]),
camera.width = int(cam["width"]) cam["K"],
camera.height = int(cam["height"]) _coerce_distortion(cam["DC"], camera_model),
camera.model = camera_model cam["R"],
converted.append(camera) cam["T"],
int(cam["width"]),
int(cam["height"]),
camera_model,
)
)
return converted return converted
tests/test_interface.py
+10 -9
View File
@@ -51,15 +51,16 @@ def make_config(cameras, roomparams) -> rpt.TriangulationConfig:
def test_camera_structure_repr(): def test_camera_structure_repr():
camera = rpt.Camera() camera = rpt.make_camera(
camera.name = "Camera 1" "Camera 1",
camera.K = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] [[1, 0, 0], [0, 1, 0], [0, 0, 1]],
camera.DC = [0, 0, 0, 0, 0] [0, 0, 0, 0, 0],
camera.R = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] [[1, 0, 0], [0, 1, 0], [0, 0, 1]],
camera.T = [[1], [2], [3]] [[1], [2], [3]],
camera.width = 640 640,
camera.height = 480 480,
camera.model = rpt.CameraModel.PINHOLE rpt.CameraModel.PINHOLE,
)
rendered = repr(camera) rendered = repr(camera)
assert "Camera 1" in rendered assert "Camera 1" in rendered