PlaneGraphOptimizer.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2026 Povl Filip Sonne-Frederiksen
//
// SPDX-License-Identifier: GPL-3.0-or-later
//
// Plane-landmark pose-graph back-end (P1 of issue #225).
//
// ReUseX's first owned global pose-optimization stage. Where Joint Pairwise
// Registration (JointPairwiseRegistration) polishes poses locally via
// frame-to-frame point-to-plane residuals — and provably saturates because
// pairwise energy cannot express global consistency — this optimizer changes
// the objective to *shared global plane landmarks*:
//
//   1. detect the dominant planes each frame sees (from its surfels),
//   2. associate those observations across frames into persistent
//      gtsam::OrientedPlane3 landmarks,
//   3. build a factor graph (per-frame Pose3 + odometry BetweenFactors from the
//      seed poses + one OrientedPlane3Factor per observation + a soft prior on
//      the first pose for gauge), and
//   4. solve it with GncOptimizer (Graduated Non-Convexity) wrapping
//      Levenberg-Marquardt, so a handful of bad plane associations are
//      down-weighted rather than corrupting the global solution.
//
// The surfel extraction and the pose write-back (removal of the constant local
// optical->sensor transform) mirror the JPR path exactly, so the two stages are
// interchangeable in the pipeline.
//
// This header is deliberately free of any GTSAM include (STANDARDS.md §2): the
// factor graph lives entirely in the .cpp.
 
#pragma once
 
#include "reusex/segmentation/Surfel.hpp"
#include "reusex/segmentation/surfel_extraction.hpp"
#include "reusex/slam/LoopClosure.hpp"
#include "reusex/slam/PanoramaLoopEdges.hpp"
 
#include <string>
#include <vector>
 
namespace reusex {
class ProjectDB;
}
 
namespace reusex::geometry {

enum class PlaneNoiseModel {
  uniform,
  inlier_count,
  fit_geometry,
};

enum class OdometryNoiseModel {
  fixed,
  motion,
};

struct OdometryEdgeMotion {
  double translation = 0.0; 
  double rotation = 0.0;    
};

struct OdometrySigmaScale {
  double rotation = 1.0;
  double translation = 1.0;
};

std::vector<OdometrySigmaScale>
odometry_motion_scales(const std::vector<OdometryEdgeMotion> &motion,
                       double min_scale, double max_scale);

struct PlaneFitQuality {
  int inliers = 0;           
  double residual_rms = 0.0; 
  double extent_minor = 0.0;
};

struct PlaneFitSigmas {
  double normal = 0.0;   
  double distance = 0.0; 
};

PlaneFitSigmas plane_fit_sigmas(const PlaneFitQuality &q);

struct PlaneGraphOptions {
  // --- Per-frame plane detection (sequential RANSAC on frame surfels) -------
  // "Mid-density" defaults: measured to beat the old conservative values
  // (4 / 200 / obs 5) on both office flatness and honka laser GT once
  // per-observation plane weighting (plane_noise_model) is on.
  int max_planes_per_frame = 6;     
  int min_plane_inliers = 120;      
  float ransac_distance = 0.02f;    
  float ransac_normal_angle = 20.f; 
  int ransac_iterations = 200;      
 
  // --- Cross-frame association into landmarks -------------------------------
  float assoc_normal_angle = 10.f; 
  float assoc_distance = 0.10f;    
  int min_landmark_observations =
      4; 
  float assoc_overlap_margin = 0.30f;
  float min_landmark_spread_ratio = 0.05f;
 
  // --- Alternating association/optimization rounds --------------------------
  int assoc_rounds = 2;
  float assoc_round_tol = 0.02f; 
 
  // --- Factor graph noise (std-devs) ----------------------------------------
  // The seed trajectory (RTABMap SLAM) is already good, so odometry is trusted
  // tightly and the plane factors act as a gentle global regularizer: this is
  // what keeps corrections at cm scale and beats the no-op baseline instead of
  // degrading it (the #225 P1 defaults over-trusted the fragile plane factors).
  float odometry_sigma_rot = 0.005f;  
  float odometry_sigma_trans = 0.01f; 
  float underconstrained_odom_scale = 0.25f;
  OdometryNoiseModel odometry_noise_model = OdometryNoiseModel::fixed;
  float odometry_weight_min = 0.5f; 
  float odometry_weight_max = 3.0f; 
  bool odometry_robust = false;
  float odometry_gnc_inlier_cost = 8.41f;
  float plane_sigma_normal = 0.24f;   
  float plane_sigma_distance = 0.19f; 
  PlaneNoiseModel plane_noise_model = PlaneNoiseModel::inlier_count;
  float plane_weight_min = 0.5f; 
  float plane_weight_max = 3.0f; 
  float plane_sigma_scale = 1.0f;
  bool use_plane_factors = true;
  float prior_sigma_rot = 0.001f; 
  float prior_sigma_trans =
      0.001f; 
 
  // --- Solver ---------------------------------------------------------------
  bool use_gnc = true; 
  float gnc_inlier_cost = 5.67f;
  int max_iterations = 100; 
  unsigned seed = 42;       

  SurfelExtractionParams surfel{.sampling_factor = 6};

  LoopClosureOptions loop_closure;
  PanoramaLoopOptions panorama_loops;
  bool loop_edges_trusted = false;
  float loop_trust_inlier_cost = 200.0f;
  std::string loop_edges_file;
  double loop_edges_min_seed_disagreement = 0.0;
  double loop_edges_min_seed_disagreement_fraction = 0.0278;
};

struct PlaneGraphResult {
  bool converged = false;      
  int frames = 0;              
  int planes_detected = 0;     
  int landmarks = 0;           
  int plane_factors = 0;       
  int iterations = 0;          
  double initial_error = 0.0;  
  double final_error = 0.0;    
  double max_pose_shift = 0.0; 
  int rounds = 0; 
  int landmarks_rejected_overlap = 0; 
  int landmarks_rejected_degenerate =
      0;                           
  int underconstrained_frames = 0; 
  int loop_edges = 0;              
  int panorama_loop_edges = 0;
  int plane_noise_clamped_low = 0;
  int plane_noise_clamped_high = 0;
  double median_fit_sigma_normal = 0.0;   
  double median_fit_sigma_distance = 0.0; 
};

class PlaneGraphOptimizer {
    public:
  explicit PlaneGraphOptimizer(PlaneGraphOptions options);

  PlaneGraphResult optimize(std::vector<FrameSurfels> &frames,
                            const std::vector<LoopEdge> &loop_edges = {}) const;
 
    private:
  PlaneGraphOptions options_;
};

PlaneGraphResult optimize_sensor_poses(ProjectDB &db,
                                       const PlaneGraphOptions &options,
                                       bool dry_run = false);
 
} // namespace reusex::geometry