Added Relative pose graph ImuPreintegration

[avnishnp]

This PR introduces a Relative Pose Graph variant to the existing IMU Preintegration module in the super_odometry stack. The original implementation was based on an Absolute Pose Graph formulation, where all pose nodes are anchored in the global frame. In contrast, this new module constructs the pose graph using relative pose constraints, which can provide benefits in scenarios with unobservable global frame (e.g., loopless trajectories, poor global priors, or unknown drift).

• Key Contributions:

1. New Backend: Introduced a new relative pose graph formulation in imuPreintegration_relative, preserving the core logic of preintegration while adjusting for relative constraints.

2. Factor Graph Updates:
   Replaced absolute pose priors with relative pose BetweenFactors between consecutive poses.
   Local pose graph is periodically reset and re-anchored for drift management.

3. World Pose Reconstruction: Maintained world pose tracking outside the graph via cumulative composition, enabling consistent odometry output in the global frame.

Evaluated both absolute and relative backends using Velodyne sensors on the Lauren Caverns SubtMRS dataset available on Superodometry website. Attached the trajectory image obtained between both of them.

Absolute Pose::

Relative Pose::

[avnishnp]

Here is the trajectory plots obtained on the Laurel caverns dataset with the ATE and RPE errors with the relative pose graph

Time range is as follows::
GT range: 1681498134.611 → 1681499088.897
EST range: 1681498137.569 → 1681499088.964
⏱ Time offset applied: -2.957684 seconds

Since there is some time offset present, i had to apply SVD to align both trajectories resulting in some error while calculating ATE
ATE RMSE: 6.0083 m
ATE Max: 10.3421 m

RPE RMSE: 0.5003 m
RPE Max: 3.4587 m
Attached below is the RPE error plot with time:

[shibowing]

Hi @avnishnp, very nice updates! Could you compare the ATE and RPE with the relative pose graph and absolute pose graph settings? Thank you very much!

[avnishnp]

@shibowing thankyou for reviewing. Updated results below

Here are the plots with absolute pose graphs (original) in IMU preintegration

Time range is as follows:
GT range: 1681498134.611 → 1681499088.897
EST range: 1681498137.054 → 1681499088.994
⏱ Time offset applied: -2.442676 seconds

ATE RMSE: 5.2234 m
ATE Max: 8.9856 m

RPE RMSE: 0.4233 m
RPE Max: 2.5770 m

below is the RPE plot::

[avnishnp]

Code update::
Added append_pose_to_csv function in the file src/SuperOdom/script/save_benchmark_result.py :: This helper function was added to log individual pose data from PoseStamped messages into a CSV file for direct comparison with the GT CSV file. It extracts the timestamp, position (x, y, z), and orientation (quaternion x, y, z, w) from each pose and appends it to a CSV file (e.g., imu_poses.csv).

[avnishnp]

If we remove the gtsam::imuBias::ConstantBias from the file imuPreintegration_relative.cpp, the result degrades, we get the following ATE plot :

ATE RMSE: 7.0183 m
ATE Max: 11.6869 m

RPE plot is as follows:

RPE RMSE: 1.4577 m
RPE Max: 5.1429 m

It is the bias evolution constraint , also used in LIO-SAM, i.e. the estimated IMU bias (accelerometer + gyroscope) at two successive keyframes in the optimization graph, which is assumed to be slowly time-varying and modeled using a soft constraint via a BetweenFactor.

IMU bias (accel + gyro) drifts slowly over time due to thermal and mechanical effects. So this ensures IMU preintegration uses correct bias, leading to better pose estimation.

[shibowing]

Thanks for clarification! It makes sense for me. How about the adding the marginalization factor as we discussed?

[avnishnp]

I will be working on the marginalization factor now. Before that, I wanted to show you some important logic and parameter change, which I made to reduce error.

Code change::
A. checkMotionThresholds logic change
In the file src/SuperOdom/super_odometry/src/LidarProcess/LidarSlam.cpp , in the checkMotionThresholds function, there is a line acceptResult=True, i have commented that out.

Uncommenting or reintroducing acceptResult = true at the end of the method would override the outcome of the earlier motion threshold checks, effectively negating the logic that:

1. Reverts pose updates on excessive motion (velocity_failure_threshold)
2. Ignores updates on negligible motion (below translation < 0.02 and rotation < 0.005)
   This would cause the system to incorrectly accept invalid localization updates, even after explicitly flagging them as unreliable.

B. Increased Ceres solver iterations:

options.max_num_iterations = 8; // was 4
➤ Allows the optimizer more room to converge, particularly helpful in low-feature or noisy environments.

C.Raised neighbor count for planar features:

LocalizationPlaneDistanceNbrNeighbors = 9; // was 5
➤ Improves PCA stability for surface normal estimation, reducing spurious matches and increasing robustness of planar constraints.

ATE plot is as follows:

GT range: 1681498134.611 → 1681499088.897
EST range: 1681498136.839 → 1681499088.964
⏱ Time offset applied: -2.227732 seconds

ATE RMSE: 1.9899 m
ATE Max: 3.5521 m

RPE plot is as follows:

RPE RMSE: 0.3795 m
RPE Max: 2.6893 m

[avnishnp]

Summary
This PR refactors the IMU preintegration node to introduce bounded-complexity smoothing via optional GTSAM fixed-lag smoothers and an intelligent marginalization/reset strategy for the traditional ISAM2 backend. It also adds detailed performance monitoring and diagnostics to help tune and verify long-term runtime behavior.

Files changed: imuPreintegration_relative.cpp, analysis.py , CMakeLists.txt

Motivation
The original relative-pose graph implementation suffered from:

1. Unbounded Graph Growth
   Memory usage and solver time grow without bound as new keyframes accumulate.
2. No Information Management
   Historical states are simply kept alive, even once they no longer contribute to current estimates.
3. Poor Long-Term Performance Visibility
   Lacked systematic counters, timing logs, and memory reports for optimization and graph updates.

Key Changes
A. Fixed-Lag Smoothing (Primary Solution)
Configurable via use_fixed_lag, fixed_lag_size, and use_batch_fixed_lag ROS parameters.

1. Batch Fixed-Lag

• Full relinearization of all factors within the lag window at each update.

• Exact marginalization, at the cost of higher per-update work.

2. Incremental Fixed-Lag

• Leverages IncrementalFixedLagSmoother (ISAM2-based) for incremental updates and marginalization.

• Keeps per-update costs low; marginalization information continues to be incorporated via linearization.

3. Marginalization Workflow

• Gather Factors & Values for the new key at the window boundary.

• Assign Timestamps to state variables.

• Invoke Smoother Update → under-the-hood Schur complement marginalizes old variables.

• Extract Estimates back into prevPose_, prevVel_, and prevBias_.

• World-Pose Tracking remains continuous.

B. Smart Reset + Traditional ISAM2 (Fallback)
When fixed-lag is disabled but enable_marginalization is enabled:

1. Dynamic Window Monitoring

• Tracks oldest/newest key timestamps and counts.

• Resets or “smart-resets” when window exceeds marginalization_window_size

2. Smart-Reset Steps

• Extract current pose, velocity, and bias estimates.

• Reanchor graph at identity pose (X(0)) with those estimates as priors.

• Transfer remaining information into the new minimal graph.

• Continue state integration seamlessly.

C. Performance Monitoring & Diagnostics

1. Realtime Counters & Timers

• last_optimization_time_ms, avg_optimization_time_ms, max_optimization_time_ms

• current_graph_size, max_graph_size

• Periodic Reports every N keys or seconds:

• Logs formatted tables with graph stats, timing, memory usage.

• Warnings if optimization time exceeds a threshold (e.g. 50 ms).

D. Configuration Parameters

Parameter	Default	Description
imu_preintegration_node.use_fixed_lag	true	Enable fixed-lag smoother
imu_preintegration_node.fixed_lag_size	10.0	Window length (seconds) for fixed-lag smoothing
imu_preintegration_node.use_batch_fixed_lag	false	Use batch vs. incremental fixed-lag smoother
imu_preintegration_node.enable_marginalization	false	Enable smart-reset loop when fixed-lag disabled
imu_preintegration_node.marginalization_window_size	10.0	Sliding window length (seconds) for traditional reset

E. Results

Below is the attached plots between Traditional iSAM2 backend, with no smart resets VS Fixed-Lag Smoother

Using a ~ 15 minute recording I compared:

1. Graph Size Over Time

• Traditional ISAM2 grows linearly until a hard reset (spikes up to ~2 k nodes).

• Fixed-Lag stays around ~250 nodes , no resets required.

2. Optimization Performance

• Traditional shows frequent spikes well above 50 ms (peaks ~85 ms).

• Fixed-Lag remains under 20 ms consistently, never exceeding the 50 ms threshold.

3. Memory Usage

• Traditional climbs steadily to ~270 MB.

• Fixed-Lag plateaus around 100 MB once the window fills.

4. Overall Efficiency Gains

Metric	Improvement (Fixed-Lag vs Traditional)
Graph-Size Reduction	148.3 %
Optimization Speed Gain	116.5 %
Memory Reduction	128.0 %
Runtime Efficiency	53.2 %
Peak-Opt Time Reduction	553.3 %

• Graph-Size Reduction: the percent by which the maximum number of nodes in the factor graph is smaller under fixed-lag smoothing than with traditional ISAM2.

• Optimization Speed Gain: the percent faster average solver (optimization) time per update when using fixed-lag smoothing.

• Memory Reduction: the percent decrease in average RAM usage when running fixed-lag smoothing versus traditional ISAM2.

• Runtime Efficiency: the percent less total wall-clock time required to process the entire dataset with fixed-lag smoothing.

• Peak-Opt Time Reduction: the percent reduction in the single slowest (peak) optimization step time under fixed-lag smoothing.

E. ATE and RPE plots

1. Traditional iSAM2 backend (no resets)

=== 3D Absolute Trajectory Error (ATE) ===
ATE RMSE: 2.2120 m
ATE Max: 12.8578 m

=== Relative Pose Error (RPE, delta=1) ===
RPE RMSE: 0.8011 m
RPE Max: 11.7772 m

2. Fixed Lag Smoother

=== 3D Absolute Trajectory Error (ATE) ===
ATE RMSE: 1.2894 m
ATE Max: 2.3632 m

=== Relative Pose Error (RPE, delta=1) ===
RPE RMSE: 0.2445 m
RPE Max: 1.4017 m