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}