perturb_poses.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2026 Povl Filip Sonne-Frederiksen
//
// SPDX-License-Identifier: GPL-3.0-or-later
//
// Issue #338: seeded, deterministic drift synthesis on a seed trajectory.
//
// WHY THIS EXISTS. Issue #221 Tier 2 asks for a capture that BOTH drifts and
// has absolute ground truth, because only such a capture can adjudicate a pose
// refinement stage. The benchmark suite has neither half in one scan:
//
//   - the office / NewOffice captures drift heavily (~80% of the trajectory's
//     extent, docs/research/registration-improvements.md §9.5) but have no
//     ground-truth geometry, only the GT-FREE surface-consistency metrics; and
//   - the three ARKitScenes scans carry an absolute `3dod_mesh` GT but their
//     seeds are already accurate to 15.7-18.6 mm median with 2-9% edge
//     disagreement — they do not drift.
//
// On a scan that does not drift, the correct behaviour of every pose stage is
// to do nothing, so every solver-side experiment since #298 (#310, #311, #316,
// #337) has measured the benchmark rather than the solver. This module closes
// that gap the cheap way: take the ARKitScenes seeds, inject a realistic and
// exactly reproducible amount of drift, and keep the GT mesh — which lives in
// the world frame and is untouched — as the adjudicator. A pose stage that
// removes the synthetic drift now has somewhere to show it.
//
// WHAT "REALISTIC" MEANS HERE. Drift from a SLAM front-end is not a random
// displacement of each pose; it is the accumulation of small errors in the
// RELATIVE (odometry) chain, which is exactly why §8.3 found no outlier edge
// for a robust kernel to reject. So the model perturbs each consecutive
// relative pose and integrates:
//
//     P'[0]   = P[0]                                   (gauge, see below)
//     P'[i+1] = P'[i] * (P[i]^-1 * P[i+1]) * Exp(xi_i)
//
// with the per-step twist xi_i built from two components, both scaled by the
// step length so that drift grows with distance travelled rather than with
// frame count:
//
//   - a BIAS term, an Ornstein-Uhlenbeck process with a metric correlation
//     length. This is the systematic part — gyro bias, a scale error, a
//     miscalibrated depth unit — and it is what makes accumulated drift smooth
//     and curved rather than a jitter. Its rotational channel is the dominant
//     one in practice: a small constant yaw error integrates into a position
//     error that grows super-linearly with path length via the lever arm.
//   - a RANDOM-WALK term scaled by sqrt(step) so the relative errors compose
//     as Brownian motion, i.e. the classic "odometry error grows with the
//     square root of distance" behaviour.
//
// GAUGE. Frame 0 is left exactly on the seed. This is not cosmetic: the pose
// graph's only absolute anchor is its tight prior on frame 0 (§8.1), so a
// rigid displacement of the whole trajectory would be drift that no pose stage
// could possibly recover, and the benchmark would measure an impossibility
// rather than a capability. Anchoring frame 0 makes the injected error exactly
// the DEFORMATION the solver is asked to undo.
//
// DETERMINISM (STANDARDS §6). Everything is a pure function of (poses, seed,
// options). The noise sequence is drawn ONCE from a seeded std::mt19937 and
// then rescaled by the calibration search, so the amplitude solve cannot
// perturb the realisation it is calibrating.
 
#pragma once
 
#include <Eigen/Core>
 
#include <vector>
 
namespace reusex {
class ProjectDB;
}
 
namespace reusex::geometry {

struct PoseDriftOptions {
  unsigned seed = 42;

  double drift_scale = 1.0;

  double target_drift_ratio = 0.20;

  int min_frame_gap = 50;
 
  // --- Character of the drift (base gains, rescaled by the calibration) ----
  double rot_bias_gain = 0.020;
  double rot_walk_gain = 0.010;
  double trans_bias_gain = 0.020;
  double trans_walk_gain = 0.010;
  double bias_correlation_length = 5.0;
 
  // --- Calibration search --------------------------------------------------
  int calibration_iterations = 60;
  double calibration_tolerance = 1e-4;
};

struct PoseDriftResult {
  int frames = 0;                 
  double trajectory_extent = 0.0; 
  double path_length = 0.0;       
  double amplitude = 0.0;         
  bool calibrated = false;        

  double drift_ratio = 0.0;
  double median_pair_disagreement = 0.0; 
 
  // Camera-centre displacement between the seed and drifted trajectories.
  double median_position_error = 0.0; 
  double max_position_error = 0.0;    
  double final_position_error = 0.0;  
  double max_rotation_error = 0.0;    
};

PoseDriftResult perturb_trajectory(std::vector<Eigen::Matrix4d> &poses,
                                   const PoseDriftOptions &opt);

double median_pair_disagreement(const std::vector<Eigen::Matrix4d> &reference,
                                const std::vector<Eigen::Matrix4d> &other,
                                int min_frame_gap);

PoseDriftResult perturb_sensor_poses(ProjectDB &db, const PoseDriftOptions &opt,
                                     bool dry_run = false);
 
} // namespace reusex::geometry