LIO-SAM unstability for S3E dataset

[khalilzrk]

Sudden Position Jumps in Odometry Output During Mapping (ROS2 / S3E Dataset)

Environment

• ROS2 Jazzy (Ubuntu 24.04)
• LIO-SAM (latest ros2 branch)
• Dataset: S3E Campus Road 1 — Scout Mini UGV (stereo + LiDAR + IMU)
• IMU: internal UGV IMU, 200 Hz
• LiDAR: 3D spinning LiDAR, 10 Hz
• CPU-only inference, no GPU involvement in SLAM

---

Description

When running LIO-SAM on the S3E Campus Road 1 sequence, the estimated trajectory exhibits sudden discontinuous position jumps — instantaneous large displacements in the published /lio_sam/mapping/odometry topic that do not correspond to any real motion in the sequence.

These jumps are distinct from gradual drift: the pose is stable for extended periods (tens of seconds), then snaps by several meters in a single publish cycle, before continuing to track normally.

---

Observed Behavior

• Odometry publishes normally at expected frequency (~10 Hz, tied to LiDAR scan rate)
• At unpredictable intervals, a single frame produces a position delta inconsistent with IMU-predicted motion (e.g., >1.5 m in a single step at low vehicle speed)
• The jump is not corrected in subsequent frames — the trajectory continues from the new (incorrect) pose
• The covariance published in the odometry message does not inflate to reflect the jump
• Behavior persists regardless of mappingSurfLeafSize, surroundingKeyframeSize, or loop closure settings

---

Suspected Cause

The jumps appear correlated with moments of degenerate scan-matching — featureless or repetitive LiDAR environments (long straight corridors, open plazas) where the point cloud registration becomes under-constrained. In these regions, the optimization likely converges to a local minimum far from the IMU-predicted prior, and the result is accepted without sufficient rejection logic.

Additionally, when two independent nodes subscribe to /odometry/imu_incremental (e.g., in multi-node launch configurations), a race condition can cause the IMU preintegration buffer to desynchronize, weakening the IMU prior that should constrain scan matching. This was observed when running a duplicate imuPreintegration node inadvertently — the /odometry/imu_incremental topic intermittently dropped, removing the IMU constraint entirely at critical moments.

---

Steps to Reproduce

1. Launch LIO-SAM with the S3E Campus Road 1 rosbag (Alpha/UGV namespace)
2. Play the bag at 1× speed
3. Monitor /lio_sam/mapping/odometry via ros2 topic echo or rviz2
4. Observe the trajectory around t ≈ 80–120 s where the environment becomes geometrically repetitive
5. Sudden jumps of 1–3 m appear with no corresponding IMU spike

---

Expected Behavior

Odometry should degrade gracefully in degenerate scan-matching conditions — either by:

• Increasing pose covariance to signal low confidence, or
• Falling back to IMU-only propagation when the point cloud registration residual exceeds a threshold

A jump of >N cm/frame (tunable) at a given speed should trigger a degeneracy flag rather than being silently accepted.

---

Suggested Fix / Feature Request

1. Degeneracy detection: expose the eigenvalue-based degeneracy flag (already partially computed internally in mapOptimization.cpp) as a published topic (e.g., /lio_sam/mapping/degenerate) so downstream nodes can react
2. Jump rejection: add a configurable maxPositionJumpMeters parameter; if the scan-matching result exceeds this threshold relative to the IMU prior, reject the result and propagate IMU only for that frame
3. Covariance inflation: inflate the output odometry covariance when degeneracy is detected, making the signal meaningful for fusion pipelines

---

Additional Context

This was encountered in the context of a multi-modal SLAM fusion pipeline (LIO-SAM + ORB-SLAM3 + attention-based transformer fusion) where undetected LIO-SAM jumps corrupt the cross-modal consistency metric used by the fusion layer. A downstream health monitor had to be implemented to catch these jumps heuristically, which ideally should not be necessary.

Any guidance on whether jump rejection is planned, or if there is an existing parameter to tune this behavior, would be appreciated.

[DJJ-12]

i also encountered this problem, which occurs because LMOptimization produce erroneous result ; but algorithm fails to detect it ; additionally，when there are too few corner / surface feature point, they should not be filtered ; for example :

   void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
{
    // fuse the map
    laserCloudCornerFromMap->clear();
    laserCloudSurfFromMap->clear(); 
    for (int i = 0; i < (int)cloudToExtract->size(); ++i)
    {
        if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
            continue;                                                                                                                                                                                                                                                                            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
        if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) 
        {
            // transformed cloud available
            *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
            *laserCloudSurfFromMap   += laserCloudMapContainer[thisKeyInd].second;
        } else {
            // transformed cloud not available
            pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
            pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
            *laserCloudCornerFromMap += laserCloudCornerTemp;
            *laserCloudSurfFromMap   += laserCloudSurfTemp;
            laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
        }
        
    }

    // Downsample the surrounding corner key frames (or map)
    downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
    downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
    laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
   
    if(laserCloudCornerFromMapDSNum < 500)
    {
        pcl::copyPointCloud(*laserCloudCornerFromMap, *laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
    }
   
    // Downsample the surrounding surf key frames (or map)
    downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
    downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
    laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
    
    if(laserCloudSurfFromMapDSNum < 500)
    {
        pcl::copyPointCloud(*laserCloudSurfFromMap, *laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();

    }
 

    // clear map cache if too large
    if (laserCloudMapContainer.size() > 1000)
        laserCloudMapContainer.clear();
}

however ,this is only a mirror improvement ,it does not truly solve the problem if the environment degeneracy is too severe, it just slightly reduces impact , i think a better approach is to detect the degenerated frames-specifically， when the LMoptimization produces erroneous result , and skip them , as long as the LiDAR frequency is high enough , skipping a few frames will not have much of an impact ;