Indeterminant Linear System Exception

[rvxfahim]

I have setup turtlebot3 with velodyne lidar in gazebo and I can view the pointcloud in rviz2, and also checked the topics being published where imu_raw is also present. When i start scanmatcher I get the following error:

[scanmatcher_node-1] [INFO] [1718576201.142020542] [scan_matcher]: create a first map
[imu_preintegration-3] terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
[imu_preintegration-3]   what():  
[imu_preintegration-3] Indeterminant linear system detected while working near variable
[imu_preintegration-3] 8646911284551352324 (Symbol: x4).
[imu_preintegration-3] 
[imu_preintegration-3] Thrown when a linear system is ill-posed.  The most common cause for this
[imu_preintegration-3] error is having underconstrained variables.  Mathematically, the system is
[imu_preintegration-3] underdetermined.  See the GTSAM Doxygen documentation at
[imu_preintegration-3] http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
[imu_preintegration-3] more information.
[ERROR] [imu_preintegration-3]: process has died [pid 26718, exit code -6, cmd '/home/rvxfahim/ros2_ws/install/scanmatcher/lib/scanmatcher/imu_preintegration --ros-args --params-file /home/rvxfahim/ros2_ws/install/scanmatcher/share/scanmatcher/param/lio.yaml -r /odometry:=/odom'].
[graph_based_slam_node-5] ----------------------------
[graph_based_slam_node-5] searching Loop, num_submaps:2
[graph_based_slam_node-5] ----------------------------
[graph_based_slam_node-5] searching Loop, num_submaps:3
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[scanmatcher_node-1] [INFO] [1718576208.570184806] [rclcpp]: signal_handler(signum=2)
[static_transform_publisher-2] [INFO] [1718576208.570192306] [rclcpp]: signal_handler(signum=2)
[image_projection-4] [INFO] [1718576208.570188036] [rclcpp]: signal_handler(signum=2)
[graph_based_slam_node-5] [INFO] [1718576208.570228275] [rclcpp]: signal_handler(signum=2)
[INFO] [static_transform_publisher-2]: process has finished cleanly [pid 26716]
[INFO] [image_projection-4]: process has finished cleanly [pid 26720]
[INFO] [graph_based_slam_node-5]: process has finished cleanly [pid 26722]
[INFO] [scanmatcher_node-1]: process has finished cleanly [pid 26714]

[rsasaki0109]

This is quite a difficult problem, so I will ask various questions.

• I would like a screenshot of rviz at the moment of the error.
• Can you show me the yaml?
• Have you checked if the orientation of the IMU is correct?
• What is the frequency of the IMU?

[rvxfahim]

• I would like a screenshot of rviz at the moment of the error.

the configuration from the repo was loaded, if i configure rviz2 myself using the pointcloud topics then the scale of the pointcloud seems ok, but the configuration from your repo shows the points_raw after zooming absurdly high enough. shell A: loads turtlebot3 in gazebo shell B: rviz2 shell C: scanmatcher, crashes immediately.

• Can you show me the yaml?

/**:
    ros__parameters:
      # LiDAR setting
      pointCloudTopic: "points_raw"
      sensor: velodyne    # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
      N_SCAN: 32          # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
      Horizon_SCAN: 2187  # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
      # IMU setting
      imuTopic: "imu_raw"
      imuAccNoise: 3.9939570888238808e-03
      imuGyrNoise: 1.5636343949698187e-03
      imuAccBiasN: 6.4356659353532566e-05
      imuGyrBiasN: 3.5640318696367613e-05
      imuGravity: 9.80511
      extrinsicTrans: [0.0, 0.0, 0.0]
      extrinsicRot: [-1.0, 0.0, 0.0,
                  0.0, 1.0, 0.0,
                  0.0, 0.0, -1.0]
      extrinsicRPY: [0.0,  1.0, 0.0,
                 -1.0, 0.0, 0.0,
                  0.0, 0.0, 1.0]

scan_matcher:
    ros__parameters:
      global_frame_id: "map"
      robot_frame_id: "base_link"
      registration_method: "NDT"
      ndt_resolution: 2.0
      ndt_num_threads: 0
      trans_for_mapupdate: 1.5
      vg_size_for_input: 0.1
      vg_size_for_map: 0.1
      use_min_max_filter: false
      map_publish_period: 100.0
      num_targeted_cloud: 20
      set_initial_pose: true
      initial_pose_x: 0.0
      initial_pose_y: 0.0
      initial_pose_z: 0.0
      initial_pose_qx: 0.0
      initial_pose_qy: 0.0
      initial_pose_qz: 0.0
      initial_pose_qw: 1.0
      use_imu: true
      use_odom: false
      debug_flag: false

graph_based_slam:
    ros__parameters:
      registration_method: "GICP"
      ndt_resolution: 5.0
      ndt_num_threads: 1
      voxel_leaf_size: 0.1
      loop_detection_period: 3000
      threshold_loop_closure_score: 1.0
      distance_loop_closure: 25.0
      range_of_searching_loop_closure: 20.0
      search_submap_num: 4
      num_adjacent_pose_cnstraints: 5
      use_save_map_in_loop: false

• Have you checked if the orientation of the IMU is correct?

i only checked if the IMU messages are published, not sure about the orientation, in that case should i pan/tilt the robot inside Gazebo to check what are the outputs? and then what to correct afterwards ?

• What is the frequency of the IMU?

500, I set it from the model sdf file and checked it using ros2 topic hz

[rsasaki0109]

It's small and I can't quite make it out, but what is happening with the trajectory on the rviz screen? Is it not appearing? Or is there a discontinuous trajectory being displayed?

If the IMU orientation is not correct, please either change the orientation of the IMU in Gazebo or modify the extrinsicRot parameter in the yaml file of this package. For debugging, refer to the following: Supplementary note: (This package operates with a 6-axis IMU.) https://github.com/rsasaki0109/li_slam_ros2/blob/develop/scanmatcher/src/image_projection.cpp#L196-L211 https://github.com/TixiaoShan/LIO-SAM?tab=readme-ov-file#prepare-imu-data

The IMU frequency seems to be sufficiently high, so that should not be an issue.

[rvxfahim]

It's small and I can't quite make it out, but what is happening with the trajectory on the rviz screen? Is it not appearing? Or is there a discontinuous trajectory being displayed?

I have fixed the Indeterminant Linear System Exception by publishing odometry to topic /odometry as it was originally publishing to /odom directly.

but now I have the warning like this and the rviz2 trajectory is moving although my robot is not moving:

[imu_preintegration-3] [WARN] [1719339191.485009932] [imu_preintegration]: Large bias, reset IMU-preintegration! [imu_preintegration-3] [WARN] [1719339192.081607345] [imu_preintegration]: Large velocity, reset IMU-preintegration!

[rvxfahim]

I fixed everything, thanks for pointing out the possible solutions. i had to fix the pose of gazebo imu and also the extrinsicROT and extrinsicRPY along with the noise params.