LoopClosure.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2026 Povl Filip Sonne-Frederiksen
//
// SPDX-License-Identifier: GPL-3.0-or-later
//
// P2 of issue #225: wide-baseline loop-closure edges for the plane-landmark
// pose graph.
//
// The plane-landmark back-end (PlaneGraphOptimizer) fixes *local* global
// consistency where frames co-observe the same walls, but it cannot constrain
// two temporally distant views of the same place that share no plane landmark
// chain — the "basin problem" from docs/research/registration-improvements.md.
// This module supplies the missing wide-baseline constraints: it proposes
// loop-candidate frame pairs, matches them with image features, lifts the
// matches to metric 3D using the stored per-frame depth, and estimates a robust
// metric relative pose (RANSAC 3D-3D). Each accepted pair becomes a
// gtsam::BetweenFactor<Pose3> loop edge fed into the SAME GncOptimizer graph,
// so a wrong edge is down-weighted rather than corrupting the solution.
//
// Front-end design (research doc §Q2, commercial-safe path): the matcher is
// abstracted behind FrameMatcher so the default OpenCV ORB detector+matcher
// (BSD, no model weights, verifiable offline) can be swapped for a learned
// matcher (EfficientLoFTR / LightGlue+ALIKED via the existing TensorRT backend)
// without touching the graph wiring. The bundled superpoint.pt is
// non-commercial and deliberately NOT used here.
//
// This header carries no GTSAM include (STANDARDS.md §2); the factor lives in
// PlaneGraphOptimizer.cpp.
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <array>
#include <random>
#include <string>
#include <vector>
 
namespace reusex {
class ProjectDB;
}
 
namespace reusex::geometry {

struct LoopEdge {
  int i = -1; 
  int j = -1; 
  Eigen::Matrix4d T_ij = Eigen::Matrix4d::Identity();
  double sigma_rot = 0.05;   
  double sigma_trans = 0.05; 
  int inliers = 0;           
};

enum class LoopProposal {
  automatic,
  spatial,
  appearance,
  exhaustive
};

struct LoopClosureOptions {
  bool enable =
      false; 

  LoopProposal proposal = LoopProposal::automatic;
  int exhaustive_budget = 80000;
 
  // --- Candidate proposal (common) -----------------------------------------
  int min_frame_gap = 50;
  int max_candidates_per_frame = 5;
 
  // --- Spatial proposal only -----------------------------------------------
  float max_candidate_distance = 2.5f; 
  float max_view_angle = 45.0f; 
 
  // --- Appearance proposal only (bag-of-words over ORB) --------------------
  int vocab_size = 400;            
  int vocab_sample_per_frame = 80; 
  int vocab_iterations = 8;        
 
  // --- Feature matching + robust relative pose -----------------------------
  // Verification defaults are tuned to actually surface wide-baseline loops on
  // iPad-LiDAR scans: 1500 features / 0.05 m were too strict and found nothing;
  // 3000 features + a 0.10 m 3D-3D threshold (depth is noisy at range) recover
  // strong loops (measured: a start/end pair with ~90 RANSAC inliers).
  int max_features = 3000;    
  float ratio_test = 0.85f;   
  int min_match_inliers = 60; 
  float ransac_inlier_dist = 0.10f; 
  int ransac_iterations = 300;      
  float min_depth = 0.3f;           
  float max_depth = 5.0f;           
  float max_seed_disagreement = 0.0f;
  float min_seed_disagreement = 0.10f;
  float min_seed_disagreement_fraction = 0.00555f;
 
  // --- Edge noise ----------------------------------------------------------
  float base_sigma_trans = 0.10f; 
  float base_sigma_rot = 0.05f;   
  float min_sigma_trans = 0.04f;  
  float min_sigma_rot = 0.02f;    
 
  // --- Pairwise Consistency Maximization (false-positive rejection) --------
  bool pcm = true;
  float pcm_trans_threshold = 0.30f; 
  float pcm_rot_threshold = 0.15f; 
  int pcm_max_edges = 3000;
 
  unsigned seed = 42; 
};

double trajectory_extent(const std::vector<Eigen::Matrix4d> &poses);

double seed_disagreement_gate(double extent, double fraction, double floor_m);

struct LoopClosureResult {
  int candidates = 0;    
  int edges = 0;         
  int total_inliers = 0; 
};

std::vector<LoopEdge>
detect_loop_edges(ProjectDB &db, const std::vector<int> &node_ids,
                  const std::vector<Eigen::Matrix4d> &seed_poses,
                  const LoopClosureOptions &options,
                  LoopClosureResult *out_result = nullptr);

std::vector<LoopEdge> load_loop_edges(const std::string &path,
                                      const std::vector<int> &node_ids,
                                      LoopClosureResult *out_result = nullptr);

std::vector<LoopEdge>
filter_consistent_loop_edges(std::vector<LoopEdge> edges,
                             const std::vector<Eigen::Matrix4d> &seed,
                             const LoopClosureOptions &options);

namespace detail {

std::vector<int> ransac_rigid(const Eigen::Matrix3Xd &src,
                              const Eigen::Matrix3Xd &dst,
                              const LoopClosureOptions &opt, std::mt19937 &rng,
                              Eigen::Matrix4d &best_T);

std::vector<LoopEdge> pcm_filter(std::vector<LoopEdge> edges,
                                 const std::vector<Eigen::Matrix4d> &seed,
                                 const LoopClosureOptions &opt);
 
} // namespace detail
 
} // namespace reusex::geometry