Simplify triangulation API with config struct

This commit is contained in:
2026-03-12 00:08:56 +08:00
parent 7df34b18c3
commit c23f25f871
6 changed files with 158 additions and 198 deletions
+32 -108
View File
@@ -20,7 +20,6 @@ namespace
using PoseArray2D = using PoseArray2D =
nb::ndarray<nb::numpy, const float, nb::shape<-1, -1, -1, 3>, nb::c_contig>; nb::ndarray<nb::numpy, const float, nb::shape<-1, -1, -1, 3>, nb::c_contig>;
using CountArray = nb::ndarray<nb::numpy, const uint32_t, nb::shape<-1>, nb::c_contig>; using CountArray = nb::ndarray<nb::numpy, const uint32_t, nb::shape<-1>, nb::c_contig>;
using RoomArray = nb::ndarray<nb::numpy, const float, nb::shape<2, 3>, nb::c_contig>;
using PoseArray3DConst = using PoseArray3DConst =
nb::ndarray<nb::numpy, const float, nb::shape<-1, -1, 4>, nb::c_contig>; nb::ndarray<nb::numpy, const float, nb::shape<-1, -1, 4>, nb::c_contig>;
using PoseArray3D = nb::ndarray<nb::numpy, float, nb::shape<-1, -1, 4>, nb::c_contig>; using PoseArray3D = nb::ndarray<nb::numpy, float, nb::shape<-1, -1, 4>, nb::c_contig>;
@@ -59,19 +58,6 @@ PoseBatch3DView pose_batch3d_view_from_numpy(const PoseArray3DConst &poses_3d)
}; };
} }
std::array<std::array<float, 3>, 2> roomparams_from_numpy(const RoomArray &roomparams)
{
std::array<std::array<float, 3>, 2> result {};
for (size_t i = 0; i < 2; ++i)
{
for (size_t j = 0; j < 3; ++j)
{
result[i][j] = roomparams(i, j);
}
}
return result;
}
PoseArray3D pose_batch_to_numpy(PoseBatch3D batch) PoseArray3D pose_batch_to_numpy(PoseBatch3D batch)
{ {
auto *storage = new std::vector<float>(std::move(batch.data)); auto *storage = new std::vector<float>(std::move(batch.data));
@@ -172,6 +158,13 @@ NB_MODULE(_core, m)
.def_rw("min_match_score", &TriangulationOptions::min_match_score) .def_rw("min_match_score", &TriangulationOptions::min_match_score)
.def_rw("min_group_size", &TriangulationOptions::min_group_size); .def_rw("min_group_size", &TriangulationOptions::min_group_size);
nb::class_<TriangulationConfig>(m, "TriangulationConfig")
.def(nb::init<>())
.def_rw("cameras", &TriangulationConfig::cameras)
.def_rw("roomparams", &TriangulationConfig::roomparams)
.def_rw("joint_names", &TriangulationConfig::joint_names)
.def_rw("options", &TriangulationConfig::options);
nb::class_<PairCandidate>(m, "PairCandidate") nb::class_<PairCandidate>(m, "PairCandidate")
.def(nb::init<>()) .def(nb::init<>())
.def_rw("view1", &PairCandidate::view1) .def_rw("view1", &PairCandidate::view1)
@@ -268,148 +261,79 @@ NB_MODULE(_core, m)
"filter_pairs_with_previous_poses", "filter_pairs_with_previous_poses",
[](const PoseArray2D &poses_2d, [](const PoseArray2D &poses_2d,
const CountArray &person_counts, const CountArray &person_counts,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::vector<std::string> &joint_names, const PoseArray3DConst &previous_poses_3d)
const PoseArray3DConst &previous_poses_3d,
float min_match_score)
{ {
TriangulationOptions options;
options.min_match_score = min_match_score;
return filter_pairs_with_previous_poses( return filter_pairs_with_previous_poses(
pose_batch_view_from_numpy(poses_2d, person_counts), pose_batch_view_from_numpy(poses_2d, person_counts),
cameras, config,
joint_names, pose_batch3d_view_from_numpy(previous_poses_3d));
pose_batch3d_view_from_numpy(previous_poses_3d),
options);
}, },
"poses_2d"_a, "poses_2d"_a,
"person_counts"_a, "person_counts"_a,
"cameras"_a, "config"_a,
"joint_names"_a, "previous_poses_3d"_a);
"previous_poses_3d"_a,
"min_match_score"_a = 0.95f);
m.def( m.def(
"triangulate_debug", "triangulate_debug",
[](const PoseArray2D &poses_2d, [](const PoseArray2D &poses_2d,
const CountArray &person_counts, const CountArray &person_counts,
const std::vector<Camera> &cameras, const TriangulationConfig &config)
const RoomArray &roomparams,
const std::vector<std::string> &joint_names,
float min_match_score,
size_t min_group_size)
{ {
TriangulationOptions options; return triangulate_debug(pose_batch_view_from_numpy(poses_2d, person_counts), config);
options.min_match_score = min_match_score;
options.min_group_size = min_group_size;
return triangulate_debug(
pose_batch_view_from_numpy(poses_2d, person_counts),
cameras,
roomparams_from_numpy(roomparams),
joint_names,
nullptr,
options);
}, },
"poses_2d"_a, "poses_2d"_a,
"person_counts"_a, "person_counts"_a,
"cameras"_a, "config"_a);
"roomparams"_a,
"joint_names"_a,
"min_match_score"_a = 0.95f,
"min_group_size"_a = 1);
m.def( m.def(
"triangulate_debug", "triangulate_debug",
[](const PoseArray2D &poses_2d, [](const PoseArray2D &poses_2d,
const CountArray &person_counts, const CountArray &person_counts,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const RoomArray &roomparams, const PoseArray3DConst &previous_poses_3d)
const std::vector<std::string> &joint_names,
const PoseArray3DConst &previous_poses_3d,
float min_match_score,
size_t min_group_size)
{ {
const PoseBatch3DView previous_view = pose_batch3d_view_from_numpy(previous_poses_3d); const PoseBatch3DView previous_view = pose_batch3d_view_from_numpy(previous_poses_3d);
TriangulationOptions options;
options.min_match_score = min_match_score;
options.min_group_size = min_group_size;
return triangulate_debug( return triangulate_debug(
pose_batch_view_from_numpy(poses_2d, person_counts), pose_batch_view_from_numpy(poses_2d, person_counts),
cameras, config,
roomparams_from_numpy(roomparams), &previous_view);
joint_names,
&previous_view,
options);
}, },
"poses_2d"_a, "poses_2d"_a,
"person_counts"_a, "person_counts"_a,
"cameras"_a, "config"_a,
"roomparams"_a, "previous_poses_3d"_a);
"joint_names"_a,
"previous_poses_3d"_a,
"min_match_score"_a = 0.95f,
"min_group_size"_a = 1);
m.def( m.def(
"triangulate_poses", "triangulate_poses",
[](const PoseArray2D &poses_2d, [](const PoseArray2D &poses_2d,
const CountArray &person_counts, const CountArray &person_counts,
const std::vector<Camera> &cameras, const TriangulationConfig &config)
const RoomArray &roomparams,
const std::vector<std::string> &joint_names,
float min_match_score,
size_t min_group_size)
{ {
TriangulationOptions options; const PoseBatch3D poses_3d =
options.min_match_score = min_match_score; triangulate_poses(pose_batch_view_from_numpy(poses_2d, person_counts), config);
options.min_group_size = min_group_size;
const PoseBatch3D poses_3d = triangulate_poses(
pose_batch_view_from_numpy(poses_2d, person_counts),
cameras,
roomparams_from_numpy(roomparams),
joint_names,
nullptr,
options);
return pose_batch_to_numpy(poses_3d); return pose_batch_to_numpy(poses_3d);
}, },
"poses_2d"_a, "poses_2d"_a,
"person_counts"_a, "person_counts"_a,
"cameras"_a, "config"_a);
"roomparams"_a,
"joint_names"_a,
"min_match_score"_a = 0.95f,
"min_group_size"_a = 1);
m.def( m.def(
"triangulate_poses", "triangulate_poses",
[](const PoseArray2D &poses_2d, [](const PoseArray2D &poses_2d,
const CountArray &person_counts, const CountArray &person_counts,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const RoomArray &roomparams, const PoseArray3DConst &previous_poses_3d)
const std::vector<std::string> &joint_names,
const PoseArray3DConst &previous_poses_3d,
float min_match_score,
size_t min_group_size)
{ {
const PoseBatch3DView previous_view = pose_batch3d_view_from_numpy(previous_poses_3d); const PoseBatch3DView previous_view = pose_batch3d_view_from_numpy(previous_poses_3d);
TriangulationOptions options;
options.min_match_score = min_match_score;
options.min_group_size = min_group_size;
const PoseBatch3D poses_3d = triangulate_poses( const PoseBatch3D poses_3d = triangulate_poses(
pose_batch_view_from_numpy(poses_2d, person_counts), pose_batch_view_from_numpy(poses_2d, person_counts),
cameras, config,
roomparams_from_numpy(roomparams), &previous_view);
joint_names,
&previous_view,
options);
return pose_batch_to_numpy(poses_3d); return pose_batch_to_numpy(poses_3d);
}, },
"poses_2d"_a, "poses_2d"_a,
"person_counts"_a, "person_counts"_a,
"cameras"_a, "config"_a,
"roomparams"_a, "previous_poses_3d"_a);
"joint_names"_a,
"previous_poses_3d"_a,
"min_match_score"_a = 0.95f,
"min_group_size"_a = 1);
} }
+25 -32
View File
@@ -150,43 +150,44 @@ struct TriangulationOptions
size_t min_group_size = 1; size_t min_group_size = 1;
}; };
struct TriangulationConfig
{
std::vector<Camera> cameras;
std::array<std::array<float, 3>, 2> roomparams {{{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}};
std::vector<std::string> joint_names;
TriangulationOptions options;
};
// ================================================================================================= // =================================================================================================
std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d); std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d);
PreviousPoseFilterDebug filter_pairs_with_previous_poses( PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const PoseBatch2DView &poses_2d, const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::vector<std::string> &joint_names,
const PoseBatch3DView &previous_poses_3d, const PoseBatch3DView &previous_poses_3d,
const TriangulationOptions &options = {}); const TriangulationOptions *options_override = nullptr);
inline PreviousPoseFilterDebug filter_pairs_with_previous_poses( inline PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const PoseBatch2D &poses_2d, const PoseBatch2D &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::vector<std::string> &joint_names,
const PoseBatch3D &previous_poses_3d, const PoseBatch3D &previous_poses_3d,
const TriangulationOptions &options = {}) const TriangulationOptions *options_override = nullptr)
{ {
return filter_pairs_with_previous_poses( return filter_pairs_with_previous_poses(poses_2d.view(), config, previous_poses_3d.view(), options_override);
poses_2d.view(), cameras, joint_names, previous_poses_3d.view(), options);
} }
TriangulationTrace triangulate_debug( TriangulationTrace triangulate_debug(
const PoseBatch2DView &poses_2d, const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3DView *previous_poses_3d = nullptr, const PoseBatch3DView *previous_poses_3d = nullptr,
const TriangulationOptions &options = {}); const TriangulationOptions *options_override = nullptr);
inline TriangulationTrace triangulate_debug( inline TriangulationTrace triangulate_debug(
const PoseBatch2D &poses_2d, const PoseBatch2D &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3D *previous_poses_3d = nullptr, const PoseBatch3D *previous_poses_3d = nullptr,
const TriangulationOptions &options = {}) const TriangulationOptions *options_override = nullptr)
{ {
PoseBatch3DView previous_view_storage; PoseBatch3DView previous_view_storage;
const PoseBatch3DView *previous_view = nullptr; const PoseBatch3DView *previous_view = nullptr;
@@ -195,8 +196,7 @@ inline TriangulationTrace triangulate_debug(
previous_view_storage = previous_poses_3d->view(); previous_view_storage = previous_poses_3d->view();
previous_view = &previous_view_storage; previous_view = &previous_view_storage;
} }
return triangulate_debug( return triangulate_debug(poses_2d.view(), config, previous_view, options_override);
poses_2d.view(), cameras, roomparams, joint_names, previous_view, options);
} }
// ================================================================================================= // =================================================================================================
@@ -205,28 +205,22 @@ inline TriangulationTrace triangulate_debug(
* Calculate a triangulation using a padded pose tensor. * Calculate a triangulation using a padded pose tensor.
* *
* @param poses_2d Padded poses of shape [views, max_persons, joints, 3]. * @param poses_2d Padded poses of shape [views, max_persons, joints, 3].
* @param cameras List of cameras. * @param config Triangulation configuration (cameras, room parameters, joint names, options).
* @param roomparams Room parameters (room size, room center). * @param options_override Optional per-call options override. Defaults to config.options.
* @param joint_names List of 2D joint names.
* @param options Triangulation options.
* *
* @return Pose tensor of shape [persons, joints, 4]. * @return Pose tensor of shape [persons, joints, 4].
*/ */
PoseBatch3D triangulate_poses( PoseBatch3D triangulate_poses(
const PoseBatch2DView &poses_2d, const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3DView *previous_poses_3d = nullptr, const PoseBatch3DView *previous_poses_3d = nullptr,
const TriangulationOptions &options = {}); const TriangulationOptions *options_override = nullptr);
inline PoseBatch3D triangulate_poses( inline PoseBatch3D triangulate_poses(
const PoseBatch2D &poses_2d, const PoseBatch2D &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::string> &joint_names,
const PoseBatch3D *previous_poses_3d = nullptr, const PoseBatch3D *previous_poses_3d = nullptr,
const TriangulationOptions &options = {}) const TriangulationOptions *options_override = nullptr)
{ {
PoseBatch3DView previous_view_storage; PoseBatch3DView previous_view_storage;
const PoseBatch3DView *previous_view = nullptr; const PoseBatch3DView *previous_view = nullptr;
@@ -235,6 +229,5 @@ inline PoseBatch3D triangulate_poses(
previous_view_storage = previous_poses_3d->view(); previous_view_storage = previous_poses_3d->view();
previous_view = &previous_view_storage; previous_view = &previous_view_storage;
} }
return triangulate_poses( return triangulate_poses(poses_2d.view(), config, previous_view, options_override);
poses_2d.view(), cameras, roomparams, joint_names, previous_view, options);
} }
+15 -21
View File
@@ -2416,39 +2416,33 @@ std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d
PreviousPoseFilterDebug filter_pairs_with_previous_poses( PreviousPoseFilterDebug filter_pairs_with_previous_poses(
const PoseBatch2DView &poses_2d, const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
const std::vector<std::string> &joint_names,
const PoseBatch3DView &previous_poses_3d, const PoseBatch3DView &previous_poses_3d,
const TriangulationOptions &options) const TriangulationOptions *options_override)
{ {
return filter_pairs_with_previous_poses_impl(poses_2d, cameras, joint_names, previous_poses_3d, options); const TriangulationOptions &options =
options_override != nullptr ? *options_override : config.options;
return filter_pairs_with_previous_poses_impl(
poses_2d, config.cameras, config.joint_names, previous_poses_3d, options);
} }
TriangulationTrace triangulate_debug( TriangulationTrace triangulate_debug(
const PoseBatch2DView &poses_2d, const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
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) const TriangulationOptions *options_override)
{ {
return triangulate_debug_impl(poses_2d, cameras, roomparams, joint_names, previous_poses_3d, options); const TriangulationOptions &options =
options_override != nullptr ? *options_override : config.options;
return triangulate_debug_impl(
poses_2d, config.cameras, config.roomparams, config.joint_names, previous_poses_3d, options);
} }
PoseBatch3D triangulate_poses( PoseBatch3D triangulate_poses(
const PoseBatch2DView &poses_2d, const PoseBatch2DView &poses_2d,
const std::vector<Camera> &cameras, const TriangulationConfig &config,
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) const TriangulationOptions *options_override)
{ {
return triangulate_debug( return triangulate_debug(poses_2d, config, previous_poses_3d, options_override).final_poses;
poses_2d,
cameras,
roomparams,
joint_names,
previous_poses_3d,
options)
.final_poses;
} }
+22
View File
@@ -6,6 +6,7 @@ from typing import TYPE_CHECKING
from ._core import ( from ._core import (
Camera, Camera,
CameraModel, CameraModel,
TriangulationConfig,
TriangulationOptions, TriangulationOptions,
CoreProposalDebug, CoreProposalDebug,
FullProposalDebug, FullProposalDebug,
@@ -43,9 +44,29 @@ def pack_poses_2d(
return _pack_poses_2d(views, joint_count=joint_count) return _pack_poses_2d(views, joint_count=joint_count)
def make_triangulation_config(
cameras: "Sequence[CameraLike]",
roomparams: "npt.NDArray[np.generic] | Sequence[Sequence[float]]",
joint_names: "Sequence[str]",
*,
min_match_score: float = 0.95,
min_group_size: int = 1,
) -> TriangulationConfig:
from ._helpers import make_triangulation_config as _make_triangulation_config
return _make_triangulation_config(
cameras,
roomparams,
joint_names,
min_match_score=min_match_score,
min_group_size=min_group_size,
)
__all__ = [ __all__ = [
"Camera", "Camera",
"CameraModel", "CameraModel",
"TriangulationConfig",
"TriangulationOptions", "TriangulationOptions",
"CoreProposalDebug", "CoreProposalDebug",
"FullProposalDebug", "FullProposalDebug",
@@ -59,6 +80,7 @@ __all__ = [
"build_pair_candidates", "build_pair_candidates",
"convert_cameras", "convert_cameras",
"filter_pairs_with_previous_poses", "filter_pairs_with_previous_poses",
"make_triangulation_config",
"pack_poses_2d", "pack_poses_2d",
"triangulate_debug", "triangulate_debug",
"triangulate_poses", "triangulate_poses",
+27 -1
View File
@@ -6,10 +6,11 @@ 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 from ._core import Camera, CameraModel, TriangulationConfig, TriangulationOptions
Matrix3x3Like: TypeAlias = Sequence[Sequence[float]] Matrix3x3Like: TypeAlias = Sequence[Sequence[float]]
VectorLike: TypeAlias = Sequence[float] VectorLike: TypeAlias = Sequence[float]
RoomParamsLike: TypeAlias = npt.NDArray[np.generic] | Sequence[Sequence[float]]
PoseViewLike: TypeAlias = npt.NDArray[np.generic] | Sequence[Sequence[Sequence[float]]] | Sequence[Sequence[float]] PoseViewLike: TypeAlias = npt.NDArray[np.generic] | Sequence[Sequence[Sequence[float]]] | Sequence[Sequence[float]]
@@ -129,3 +130,28 @@ def pack_poses_2d(
packed[view_idx, :person_count, :, :] = array packed[view_idx, :person_count, :, :] = array
return packed, counts return packed, counts
def make_triangulation_config(
cameras: Sequence[CameraLike],
roomparams: RoomParamsLike,
joint_names: Sequence[str],
*,
min_match_score: float = 0.95,
min_group_size: int = 1,
) -> TriangulationConfig:
config = TriangulationConfig()
config.cameras = convert_cameras(cameras)
roomparams_array = np.asarray(roomparams, dtype=np.float32)
if roomparams_array.shape != (2, 3):
raise ValueError("roomparams must have shape [2, 3].")
config.roomparams = roomparams_array.tolist()
config.joint_names = [str(joint_name) for joint_name in joint_names]
options = TriangulationOptions()
options.min_match_score = float(min_match_score)
options.min_group_size = int(min_group_size)
config.options = options
return config
+37 -36
View File
@@ -39,8 +39,15 @@ def load_case(camera_path: str, pose_path: str):
pose_data = json.load(file) pose_data = json.load(file)
poses_2d, person_counts = rpt.pack_poses_2d(pose_data["2D"], joint_count=len(JOINT_NAMES)) poses_2d, person_counts = rpt.pack_poses_2d(pose_data["2D"], joint_count=len(JOINT_NAMES))
cameras = rpt.convert_cameras(camera_data["cameras"]) return poses_2d, person_counts, camera_data["cameras"]
return poses_2d, person_counts, cameras
def make_config(cameras, roomparams) -> rpt.TriangulationConfig:
return rpt.make_triangulation_config(
cameras,
np.asarray(roomparams, dtype=np.float32),
JOINT_NAMES,
)
def test_camera_structure_repr(): def test_camera_structure_repr():
@@ -69,14 +76,8 @@ def test_camera_structure_repr():
) )
def test_triangulate_samples(camera_path: str, pose_path: str, roomparams): def test_triangulate_samples(camera_path: str, pose_path: str, roomparams):
poses_2d, person_counts, cameras = load_case(camera_path, pose_path) poses_2d, person_counts, cameras = load_case(camera_path, pose_path)
poses_3d = rpt.triangulate_poses( config = make_config(cameras, roomparams)
poses_2d, poses_3d = rpt.triangulate_poses(poses_2d, person_counts, config)
person_counts,
cameras,
np.asarray(roomparams, dtype=np.float32),
JOINT_NAMES,
min_match_score=0.95,
)
assert isinstance(poses_3d, np.ndarray) assert isinstance(poses_3d, np.ndarray)
assert poses_3d.dtype == np.float32 assert poses_3d.dtype == np.float32
@@ -88,14 +89,10 @@ def test_triangulate_samples(camera_path: str, pose_path: str, roomparams):
def test_triangulate_repeatability(): def test_triangulate_repeatability():
poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json") poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
roomparams = np.asarray([[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]], dtype=np.float32) config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
first = rpt.triangulate_poses( first = rpt.triangulate_poses(poses_2d, person_counts, config)
poses_2d, person_counts, cameras, roomparams, JOINT_NAMES, min_match_score=0.95 second = rpt.triangulate_poses(poses_2d, person_counts, config)
)
second = rpt.triangulate_poses(
poses_2d, person_counts, cameras, roomparams, JOINT_NAMES, min_match_score=0.95
)
np.testing.assert_allclose(first, second, rtol=1e-5, atol=1e-5) np.testing.assert_allclose(first, second, rtol=1e-5, atol=1e-5)
@@ -119,28 +116,21 @@ def test_build_pair_candidates_exposes_cartesian_view_pairs():
def test_triangulate_accepts_empty_previous_poses(): def test_triangulate_accepts_empty_previous_poses():
poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json") poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
roomparams = np.asarray([[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]], dtype=np.float32) config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
empty_previous = np.zeros((0, len(JOINT_NAMES), 4), dtype=np.float32) empty_previous = np.zeros((0, len(JOINT_NAMES), 4), dtype=np.float32)
baseline = rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES) baseline = rpt.triangulate_poses(poses_2d, person_counts, config)
with_previous = rpt.triangulate_poses( with_previous = rpt.triangulate_poses(poses_2d, person_counts, config, empty_previous)
poses_2d,
person_counts,
cameras,
roomparams,
JOINT_NAMES,
empty_previous,
)
np.testing.assert_allclose(with_previous, baseline, rtol=1e-5, atol=1e-5) np.testing.assert_allclose(with_previous, baseline, rtol=1e-5, atol=1e-5)
def test_triangulate_debug_matches_final_output(): def test_triangulate_debug_matches_final_output():
poses_2d, person_counts, cameras = load_case("data/h1/sample.json", "tests/poses_h1.json") poses_2d, person_counts, cameras = load_case("data/h1/sample.json", "tests/poses_h1.json")
roomparams = np.asarray([[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]], dtype=np.float32) config = make_config(cameras, [[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]])
final_poses = rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES) final_poses = rpt.triangulate_poses(poses_2d, person_counts, config)
trace = rpt.triangulate_debug(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES) trace = rpt.triangulate_debug(poses_2d, person_counts, config)
np.testing.assert_allclose(trace.final_poses, final_poses, rtol=1e-5, atol=1e-5) np.testing.assert_allclose(trace.final_poses, final_poses, rtol=1e-5, atol=1e-5)
assert len(trace.pairs) >= len(trace.core_proposals) assert len(trace.pairs) >= len(trace.core_proposals)
@@ -152,14 +142,13 @@ def test_triangulate_debug_matches_final_output():
def test_filter_pairs_with_previous_poses_returns_debug_matches(): def test_filter_pairs_with_previous_poses_returns_debug_matches():
poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json") poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
roomparams = np.asarray([[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]], dtype=np.float32) config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
previous_poses = rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES) previous_poses = rpt.triangulate_poses(poses_2d, person_counts, config)
debug = rpt.filter_pairs_with_previous_poses( debug = rpt.filter_pairs_with_previous_poses(
poses_2d, poses_2d,
person_counts, person_counts,
cameras, config,
JOINT_NAMES,
previous_poses, previous_poses,
) )
@@ -171,12 +160,12 @@ def test_filter_pairs_with_previous_poses_returns_debug_matches():
def test_triangulate_does_not_mutate_inputs(): def test_triangulate_does_not_mutate_inputs():
poses_2d, person_counts, cameras = load_case("data/h1/sample.json", "tests/poses_h1.json") poses_2d, person_counts, cameras = load_case("data/h1/sample.json", "tests/poses_h1.json")
roomparams = np.asarray([[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]], dtype=np.float32) config = make_config(cameras, [[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]])
poses_before = poses_2d.copy() poses_before = poses_2d.copy()
counts_before = person_counts.copy() counts_before = person_counts.copy()
rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES) rpt.triangulate_poses(poses_2d, person_counts, config)
np.testing.assert_array_equal(poses_2d, poses_before) np.testing.assert_array_equal(poses_2d, poses_before)
np.testing.assert_array_equal(person_counts, counts_before) np.testing.assert_array_equal(person_counts, counts_before)
@@ -219,3 +208,15 @@ def test_pack_poses_2d_rejects_inconsistent_joint_count():
def test_pack_poses_2d_rejects_invalid_last_dimension(): def test_pack_poses_2d_rejects_invalid_last_dimension():
with pytest.raises(ValueError, match="shape"): with pytest.raises(ValueError, match="shape"):
rpt.pack_poses_2d([np.zeros((1, 2, 2), dtype=np.float32)]) rpt.pack_poses_2d([np.zeros((1, 2, 2), dtype=np.float32)])
def test_make_triangulation_config_builds_bound_struct():
_, _, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
assert isinstance(config, rpt.TriangulationConfig)
assert len(config.cameras) > 0
np.testing.assert_allclose(config.roomparams, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
assert config.joint_names == JOINT_NAMES
assert config.options.min_match_score == pytest.approx(0.95)
assert config.options.min_group_size == 1