From a62123fe2afdc7250d91df66b6ff1681768b9a05 Mon Sep 17 00:00:00 2001 From: Zador-Pataki Date: Wed, 4 Nov 2026 02:50:38 +0100 Subject: [PATCH] Deduplicate repeated loop-closure targets in GP --- README.md | 11 ++-- .../bindings/global_positioning_problem.cc | 57 ++++++++++++------- .../tests/python/test_global_positioning.py | 47 +++++++++++++++ 3 files changed, 88 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index 010af8c..fd865fd 100644 --- a/README.md +++ b/README.md @@ -531,20 +531,19 @@ python -m vidmap.run_for_benchmark \ [Results - click to expand] WATE-AUC with late principal point refinement, at 5% error thresholds (%, higher -is better). Errors are pooled within each location, then eligible locations are -averaged equally: +is better). Results are averaged equally across eligible sequences: | Dataset | 10 m @ 0.5 m | 25 m @ 1.25 m | 50 m @ 2.5 m | 100 m @ 5 m | Full @ 5% | | :--- | ---: | ---: | ---: | ---: | ---: | -| LaMAR (sample-300, 50 sequences) | 92.26 | 92.66 | 91.16 | 90.77 | 90.23 | -| CroCoDL (64 sequences) | 95.10 | 96.55 | 97.22 | 97.56 | 97.18 | +| LaMAR (sample-300, 50 sequences) | 92.43 | 93.08 | 91.79 | 91.03 | 90.41 | +| CroCoDL (64 sequences) | 95.72 | 97.27 | 97.39 | 96.56 | 98.21 | Pose AUC, averaged equally across sequences (%, higher is better): | Dataset | 5 cm | 10 cm | 50 cm | 1 m | 10 m | | :--- | ---: | ---: | ---: | ---: | ---: | -| EuRoC (11 sequences) | 28.86 | 60.02 | 91.96 | 95.98 | 99.60 | -| ETH3D-SLAM (55 sequences) | 78.48 | 85.74 | 94.39 | 96.61 | 99.52 | +| EuRoC (11 sequences) | 28.44 | 59.74 | 91.91 | 95.95 | 99.60 | +| ETH3D-SLAM (55 sequences) | 80.32 | 87.85 | 95.86 | 97.35 | 99.28 | diff --git a/extensions/colmap/src/bindings/global_positioning_problem.cc b/extensions/colmap/src/bindings/global_positioning_problem.cc index 341b3d2..f413730 100644 --- a/extensions/colmap/src/bindings/global_positioning_problem.cc +++ b/extensions/colmap/src/bindings/global_positioning_problem.cc @@ -3,6 +3,7 @@ #include "colmap/estimators/global_positioning.h" #include "colmap/scene/camera.h" #include "colmap/scene/reconstruction.h" +#include "colmap/util/hash_containers.h" #include #include @@ -63,7 +64,7 @@ class DeadZoneHuberLoss final : public ceres::LossFunction { struct Observation { colmap::point3D_t point_id; colmap::TrackElement element; - colmap::TrackElement anchor; + std::vector anchors; bool loop; bool support; double* center = nullptr; @@ -98,6 +99,7 @@ bool MaskValue(const vidmap::VectorXb& mask, colmap::point2D_t index) { return index < mask.size() && mask(index); } +using Feature = std::pair; using ObservationKind = std::tuple; std::map PrepareObservations( const colmap::Reconstruction& reconstruction, @@ -131,15 +133,25 @@ std::map PrepareObservations( reconstruction.Image(regular[i].image_id).FrameId()) <= *support_ranks.rbegin(); } + colmap::FlatHashMap loop_targets; auto append = [&](const colmap::TrackElement& element, const colmap::TrackElement& anchor, bool loop, bool early) { const auto& image = reconstruction.Image(element.image_id); - selected[{loop, image.CameraPtr()->HasPriorFocalLength(), early}] - .observations.push_back({point_id, element, anchor, loop, early}); + auto& observations = + selected[{loop, image.CameraPtr()->HasPriorFocalLength(), early}] + .observations; + size_t index = observations.size(); + if (loop) { + const Feature feature{element.image_id, element.point2D_idx}; + index = loop_targets.emplace(feature, index).first->second; + } + if (index == observations.size()) + observations.push_back({point_id, element, {}, loop, early}); + if (loop) observations[index].anchors.push_back(anchor); }; - std::set> regular_features; + std::set regular_features; for (size_t row = 0; row < regular.size(); ++row) { if (!regular_features .emplace(regular[row].image_id, regular[row].point2D_idx) @@ -327,11 +339,13 @@ PYBIND11_MODULE(global_positioning, m) { for (const auto& element : point.track.Elements()) ++counts[element.image_id]; const auto extra = sidecars.tracks.find(id); - if (extra != sidecars.tracks.end()) - for (Eigen::Index row = 0; - row < extra->second.loop_closure_observations.rows(); - ++row) - ++counts[extra->second.loop_closure_observations(row, 0)]; + if (extra == sidecars.tracks.end()) continue; + colmap::FlatHashSet targets; + const auto& elements = extra->second.loop_closure_observations; + for (Eigen::Index row = 0; row < elements.rows(); ++row) { + if (targets.emplace(elements(row, 0), elements(row, 1)).second) + ++counts[elements(row, 0)]; + } } return counts; }); @@ -551,7 +565,6 @@ PYBIND11_MODULE(global_positioning, m) { [](const std::vector& selections, const std::vector>& losses, const std::unordered_set& images) { - using Feature = std::pair; using Edge = std::pair; struct EdgeScore { std::set> matches; @@ -561,16 +574,6 @@ PYBIND11_MODULE(global_positioning, m) { for (size_t group = 0; group < selections.size(); ++group) { for (const auto& obs : selections[group]->observations) { if (!obs.loop) continue; - if (obs.anchor.image_id == colmap::kInvalidImageId) - throw std::invalid_argument( - "Playback requires exact loop-closure anchors"); - const auto anchor_image = obs.anchor.image_id; - if (!images.count(obs.ImageId()) || !images.count(anchor_image)) - continue; - const Feature a{obs.ImageId(), obs.Index()}, - b{anchor_image, obs.anchor.point2D_idx}; - auto& edge = edges[std::minmax(obs.ImageId(), anchor_image)]; - edge.matches.insert(std::minmax(a, b)); const double* parameters[] = {obs.point, obs.center, obs.scale}; Eigen::Vector3d residual; // Evaluate the cost directly: Problem::Evaluate is unavailable in @@ -580,7 +583,19 @@ PYBIND11_MODULE(global_positioning, m) { losses.at(group)->Evaluate(residual.squaredNorm(), rho); const double score = std::max(rho[1], 0.0) * std::sqrt(std::max(rho[0], 0.0)); - if (std::isfinite(score)) edge.score += score; + for (const auto& anchor : obs.anchors) { + if (anchor.image_id == colmap::kInvalidImageId) + throw std::invalid_argument( + "Playback requires exact loop-closure anchors"); + if (!images.count(obs.ImageId()) || + !images.count(anchor.image_id)) + continue; + const Feature a{obs.ImageId(), obs.Index()}, + b{anchor.image_id, anchor.point2D_idx}; + auto& edge = edges[std::minmax(obs.ImageId(), anchor.image_id)]; + edge.matches.insert(std::minmax(a, b)); + if (std::isfinite(score)) edge.score += score; + } } } std::vector order; diff --git a/extensions/colmap/tests/python/test_global_positioning.py b/extensions/colmap/tests/python/test_global_positioning.py index dc9bfb2..0bec44e 100644 --- a/extensions/colmap/tests/python/test_global_positioning.py +++ b/extensions/colmap/tests/python/test_global_positioning.py @@ -374,3 +374,50 @@ def test_bundle_adjustment_preserves_camera_without_observations(): assert result.success assert result.diagnostics.num_intrinsics_prior_residuals == 1 np.testing.assert_array_equal(problem.reconstruction.camera(2).params, camera_params) + + +def test_lc_target_deduplication_preserves_objective_and_playback(): + def solve(repeats): + state = _playback_problem() + data = state.sidecars.track(1) + data.loop_closure_observations = np.asarray([[3, 0]] * repeats, dtype=np.uint32) + data.loop_closure_anchors = np.asarray([[1, 0], [2, 0], [1, 0]][:repeats], dtype=np.uint32) + state.reconstruction.point3D(1).xyz += [0.2, 0.1, 1.0] + for image_id in state.image_order: + image = state.image_data(image_id) + image.depth_values = np.asarray([5.0]) + image.depth_stddevs = np.asarray([0.1]) + image.depth_validity = np.asarray([1], dtype=np.uint8) + options = _gp_options() + options = replace(options, common=replace(options.common, use_metric_depth_constraint=True)) + captures = [] + result = _run_gp(options, state, captures.append) + assert result.success + assert len(state.sidecars.track(1).loop_closure_observations) == repeats + return result.diagnostics, captures[0] + + unique, _ = solve(1) + deduplicated, playback = solve(3) + assert deduplicated.num_metric_depth_residuals == 3 + assert deduplicated.initial_cost == pytest.approx(unique.initial_cost, abs=1e-11) + assert deduplicated.final_cost == pytest.approx(unique.final_cost, abs=1e-11) + np.testing.assert_array_equal(playback["lc_pairs"], [[1, 3], [2, 3]]) + np.testing.assert_array_equal(playback["lc_support_count"], [1, 1]) + + +def test_lc_target_deduplication_is_per_track_and_exact_feature(): + state = _playback_problem() + for image_id in state.image_order: + state.image(image_id).points2D.append(pycolmap.Point2D([320.0, 340.0])) + second = state.reconstruction.add_point3D( + [0.0, 0.0, 6.0], pycolmap.Track([pycolmap.TrackElement(1, 1), pycolmap.TrackElement(2, 1)]) + ) + for point_id in (1, second): + data = native.TrackData() + data.loop_closure_observations = np.asarray([[3, 0], [3, 0], [3, 1], [1, 0]], dtype=np.uint32) + state.sidecars.set_track(point_id, data) + options = _gp_options() + prepared = gp.prepare_observations(state.reconstruction, state.sidecars, options) + counts = gp.gp_costs.observation_counts(list(prepared.values())) + assert counts[:3] == (4, 6, 2) + assert gp.gp_costs.track_observation_counts(state.reconstruction, state.sidecars) == {1: 4, 2: 2, 3: 4}