Deduplicate repeated loop-closure targets in GP

README.md

@@ -531,20 +531,19 @@ python -m vidmap.run_for_benchmark \
 <summary>[Results - click to expand]</summary>
 
 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 |
 
 </details>
 

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 <cstdint>
 #include <limits>
@@ -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<colmap::TrackElement> 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<colmap::image_t, colmap::point2D_t>;
 using ObservationKind = std::tuple<bool, bool, bool>;
 std::map<ObservationKind, ObservationSelection> PrepareObservations(
     const colmap::Reconstruction& reconstruction,
@@ -131,15 +133,25 @@ std::map<ObservationKind, ObservationSelection> PrepareObservations(
                          reconstruction.Image(regular[i].image_id).FrameId()) <=
                      *support_ranks.rbegin();
     }
+    colmap::FlatHashMap<Feature, size_t, colmap::PairHash> 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<std::pair<colmap::image_t, colmap::point2D_t>> regular_features;
+    std::set<Feature> 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<Feature, colmap::PairHash> 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<ObservationSelection*>& selections,
            const std::vector<std::shared_ptr<ceres::LossFunction>>& losses,
            const std::unordered_set<colmap::image_t>& images) {
-          using Feature = std::pair<colmap::image_t, colmap::point2D_t>;
           using Edge = std::pair<colmap::image_t, colmap::image_t>;
           struct EdgeScore {
             std::set<std::pair<Feature, Feature>> 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<Edge> order;

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}