Z-axis drift with Mid-360 on public outdoor dataset (RTX 2050 / 4GB VRAM)

[NITHISHKUMAR-A]

Describe the bug
When running glim on the official Mid-360 outdoor rosbag from Zenodo, the map builds
correctly for the first ~60–90 seconds, then experiences a sudden z-axis tilt/collapse
where the entire point cloud rotates and flattens. The factor graph develops many failed
(red) factors spreading in the wrong direction after the collapse.

IMPORTANT : with other bag file i record my self using livox mid 360 and test the glim with that bag file i am facing the z drift at the loop closer point
kindly check my config

The issue is reproducible on the official public dataset with the versatile config.

Screenshots

Environment (please complete the following information):

| glim version | 1.2.0 |
| ROS2 distribution | Humble |
| OS | Ubuntu 22.04 |
| GPU | NVIDIA RTX 2050 (4GB VRAM) |
| Sensor | Livox Mid-360 |
| Dataset | koide3 public outdoor dataset — https://zenodo.org/records/14841855 |
| Mode | Rosbag replay (ros2 bag play) |
| Config | config_versatile downloaded from https://koide3.github.io/glim/demo.html, with minor topic/sensor adjustments |

---

Additional context

Config Files (versatile config with sensor adjustments)

config_sensors.json (key section)

"imu_acc_noise": 0.01,
"imu_gyro_noise": 0.0001,
"imu_int_noise": 0.0001,
"imu_bias_noise": 1e-5,
"T_lidar_imu": [-0.011, -0.0234, 0.044, 0.0, 0.0, 0.0, 1.0],
"autoconf_perpoint_times": true,
"autoconf_prefer_frame_time": false,
"perpoint_relative_time": true,
"perpoint_time_scale": 1.0

config_odometry_gpu.json (key section)

"initialization_mode": "NAIVE",
"smoother_lag": 5.0,
"fix_imu_bias": false,
"voxel_resolution": 0.15,
"voxel_resolution_max": 0.5,
"voxelmap_levels": 2,
"full_connection_window_size": 2,
"keyframe_update_strategy": "OVERLAP",
"max_num_keyframes": 15,
"keyframe_max_overlap": 0.85,
"validate_imu": true

config_global_mapping_gpu.json (key section)

"enable_optimization": true,
"create_between_factors": false,
"registration_error_factor_type": "VGICP_GPU",
"randomsampling_rate": 0.3,
"submap_voxel_resolution": 0.15,
"submap_voxelmap_levels": 2,
"min_implicit_loop_overlap": 0.1,
"use_isam2_dogleg": false,
"isam2_relinearize_thresh": 0.0

[Pana1v]

@koide3 Hi!

I solved this problem very recently where I added a minimal GLIM extension to restrict this by adding appropriate factor graphs. Would you be open to including an implementation :D !?

[steph16cbr]

Same Z-axis collapse here with a Livox Mid-360, reproduced on our own underground (quarry) data — confirming this isn't dataset-specific.

Setup

• Mid-360 (built-in IMU), livox_ros_driver2, ROS 2 Jazzy, CPU-only (AMD, no CUDA), Docker image koide3/glim_ros2:jazzy (also PPA glim 1.2.0 / glim-ros 1.2.1 / gtsam-points 1.2.1 / gtsam-notbb 4.3.0).
• acc_scale: 9.80665 set (accel ≈ 9.81 at rest). Hand-carried, ~20–25° nose-down, tilt varies.
• Offline: glim_rosbag <bag> -p config_path:=... -p auto_quit:=true.

Symptom

• Single-level flat quarry, ~270×200 m, ~92-min there-and-back with one U-turn (~2/3 in).
• Map closes loops (≈80 matching factors after lowering min_implicit_loop_overlap to 0.1), but globally bows/arches vertically: world-Z extent ~160 m (impossible for one level). The orientation noticeably "tips" around the U-turn. Same flavor as your z-axis tilt/collapse.

Notes / things tried

• Confirmed it's not acc_scale (already 9.80665) and not a stale config (our config_*.json match the image defaults aside from CPU/topics/overlap).
• T_lidar_imu: started on GLIM's default template [0.006,-0.012,0.008,0,0,0,1]. The Mid-360 value with flipped sign [0.011,0.02329,-0.04412,...] made it far worse (submap tilts ~177°); the value here in Z-axis drift with Mid-360 on public outdoor dataset (RTX 2050 / 4GB VRAM) #300 [-0.011,-0.0234,0.044,0,0,0,1] behaves normally — so the sign matters.

Questions

1. @Pana1v — would you be willing to share your minimal GLIM extension (factor) that fixed the Z drift? That sounds like the actual cure.
2. @koide3 — is there a recommended way to bound vertical/pitch drift on long single-corridor Mid-360 scans (a gravity/planar constraint, IMU weighting, or global-mapping setting)? And is the Mid-360 T_lidar_imu purely translational, or is there a rotation between the Mid-360 IMU and LiDAR frames?

Happy to share a small (161 MB) and the full (~28 GB) bag. Thanks!

[Pana1v]

Sure, I am optimistic it will solve your problem I will be able to share one by tomorrow.

[Pana1v]

@steph16cbr Can you give this a shot 😄
@koide3 I would like to raise a PR for the same

#include <map>
#include <vector>
#include <cstdint>

#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_points/util/gtsam_migration.hpp>
#include <gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp>

#include <glim/odometry/callbacks.hpp>
#include <glim/util/logging.hpp>
#include <glim/util/extension_module.hpp>
#include <glim/odometry/estimation_frame.hpp>
#include <glim_ext/util/config_ext.hpp>

namespace glim {

// Constrains the pose's local Z axis to point along world Z (suppresses roll and pitch).
// Residual = [0,0,1] - R*[0,0,1]. Identical math to GravityAlignmentFactor but
// with a fixed upward=[0,0,1] applied from frame 0, bypassing gravity estimation.
class FlatGroundFactor : public gtsam::NoiseModelFactorN<gtsam::Pose3> {
public:
  using Base = gtsam::NoiseModelFactorN<gtsam::Pose3>;

  FlatGroundFactor() {}
  FlatGroundFactor(gtsam::Key key, const gtsam::SharedNoiseModel& nm) : Base(nm, key) {}

  gtsam::NonlinearFactor::shared_ptr clone() const override { return gtsam::make_shared<FlatGroundFactor>(*this); }

  gtsam::Vector evaluateError(const gtsam::Pose3& pose, gtsam_points::OptionalMatrixType H = gtsam_points::NoneValue) const override {
    gtsam::Matrix36 H_R_pose;
    const gtsam::Rot3 R = pose.rotation(H_R_pose);
    gtsam::Matrix33 H_up_R;
    const gtsam::Vector3 local_up = R.rotate(gtsam::Vector3::UnitZ(), H_up_R);
    if (H) {
      *H = -H_up_R * H_R_pose;
    }
    return gtsam::Vector3::UnitZ() - local_up;
  }
};

class FlatGroundOdomModule : public ExtensionModule {
public:
  FlatGroundOdomModule() : logger(create_module_logger("flatground")) {
    glim::Config config(glim::GlobalConfigExt::get_config_path("config_flat_ground_odom"));
    const double sigma = config.param<double>("flat_ground_odom", "roll_pitch_sigma", 1e-3);
    noise_model = gtsam::noiseModel::Isotropic::Sigma(3, sigma);
    logger->info("flat-ground odom constraint active (sigma={})", sigma);

    OdometryEstimationCallbacks::on_new_frame.add([this](const EstimationFrame::ConstPtr& frame) { pending_frame_ids.push_back(frame->id); });

    OdometryEstimationCallbacks::on_smoother_update.add(
      [this](gtsam_points::IncrementalFixedLagSmootherExtWithFallback&, gtsam::NonlinearFactorGraph& new_factors, gtsam::Values&, std::map<std::uint64_t, double>&) {
        for (auto id : pending_frame_ids) {
          new_factors.emplace_shared<FlatGroundFactor>(gtsam::symbol_shorthand::X(id), noise_model);
        }
        pending_frame_ids.clear();
      });
  }

private:
  gtsam::SharedNoiseModel noise_model;
  std::vector<std::uint64_t> pending_frame_ids;
  std::shared_ptr<spdlog::logger> logger;
};

}  // namespace glim

extern "C" glim::ExtensionModule* create_extension_module() {
  return new glim::FlatGroundOdomModule();
}