search

engcang / SLAM-application

LeGO-LOAM, LIO-SAM, LVI-SAM, FAST-LIO2, Faster-LIO, VoxelMap, R3LIVE, Point-LIO, KISS-ICP, DLO, DLIO, Ada-LIO, PV-LIO, SLAMesh, ImMesh, FAST-LIO-MULTI, M-LOAM, LOCUS, SLICT, MA-LIO application and comparison on Gazebo and real-world datasets. Installation and config files are provided.
BSD 3-Clause "New" or "Revised" License
744 stars 105 forks source link

LIO-SAM not working with bag files captured in gazebo #1

Closed esaumar closed 3 years ago

esaumar commented 3 years ago

Hi @engcang! Nice integration, your videos look pretty good. I have been trying to replay what you did with LIO-SAM repo but using another robot. I am using a differential drive robot model (https://github.com/sanuann/DifferentialDriveRobot) where I attached a Velodyne VLP-16 (https://github.com/lmark1/velodyne_simulator) and a IMU (libgazebo_ros_imu.so).

I can't get a good reconstruction. Have you seen this behavior? I have found that it could be related to the extrinsic parameters in params.yaml but I believe I'm setting the values correctly.

lio-sam-velodyne

I attached the Velodyne into the robot chassis using the file myrobot.xacro from https://github.com/sanuann/DifferentialDriveRobot

<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
  <VLP-16 parent="chassis" name="velodyne" topic="/velodyne_points" hz="10" samples="440">
    <origin xyz="0 0 0.0" rpy="0 0 0" />
  </VLP-16>

lio-sam-velodyne-3

I attached the IMU into the velodyne body frame and defined an update rate of 500Hz in the file myrobot.xacro from https://github.com/sanuann/DifferentialDriveRobot

<gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>velodyne</bodyName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0</gaussianNoise>
      <updateRate>500.0</updateRate>
    </plugin>
  </gazebo>

Thus, all sensors are in the same body frame, velodyne. That is why I used this Extrinsics

# Extrinsics (lidar -> IMU)
  extrinsicTrans:  [0.000, 0.000, 0.0]
  extrinsicRot: [1, 0, 0,
                  0, 1, 0,
                  0, 0, 1]
  extrinsicRPY: [1,  0, 0,
                 0, 1, 0,
                 0, 0, 1]

but can't get a good reconstruction. I also tested moving the VLP-16 0.4m above the chassis and modified the Extrinsics to [0.000, 0.000, -0.438] (gotten from rosrun tf tf_echo velodyne chassis) but I have the same behavior. lio-sam-velodyne-4

Any suggestion will be appreciated. Thanks!

esaumar commented 3 years ago

Btw, I tried to use your script lidar_repair.py for the trouble shooting with CPU ray in the Velodyne Gazebo plugin. I guess the output is the topic with name velody but I don't get any point cloud, not even the red ones I showed in the first comment.

engcang commented 3 years ago

@esaumar Hi. first thank you for your interest in my repo. Looking into your questions, I doubt that could because of IMU sensor. I used libgazebo_ros_imu_sensor rather than libgazebo_ros_imu and the detailed difference can be found at: http://gazebosim.org/tutorials?tut=ros_gzplugins According to the original author of LIO-SAM (https://github.com/TixiaoShan/LIO-SAM#prepare-imu-data), you can check that the IMU must include 9-axis values, and I guess it also has to include gravity factor.

Otherwise, exptrinsic parameters seem ok.

esaumar commented 3 years ago

Hi again @engcang ! Thanks for your quick response. Finally I got consistent reconstructions with LIO-SAM! You were right, it seemed that it was an issue with the IMU. The gravity should be included in the inertial measurements. I changed the plugin from libgazebo_ros_imu to libgazebo_ros_imu_sensor as suggested and now it works correctly.

image (25) image (26)

By using libgazebo_ros_imu_sensor now I can see the gravity on the Z-axis acceleration. Bad IMU

orientation: 
  x: 2.27188461314e-06
  y: -4.9678469459e-07
  z: 0.99810920732
  w: -0.0614655208814
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.000769452875622
  y: 0.00324902769181
  z: 4.03222099903e-05
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.227426137416
  y: 0.0242159376913
  z: 4.67897961339
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Good IMU

orientation: 
  x: 1.55563938577e-14
  y: -6.53325214776e-15
  z: -2.5826076197e-15
  w: 1.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -6.18309291924e-13
  y: 2.5967517029e-13
  z: 6.44077756609e-21
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 1.2805174418e-13
  y: 3.04905318523e-13
  z: 9.8
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Thanks for your support!

narutojxl commented 3 years ago

Hi @esaumar, i meet similar problem with https://github.com/TurtleZhong/LVIO-SAM gazebo, I don't know whether i missing something or mistake something, Could you please give some suggestions about this problem. Thanks in advance!

extrinsicTrans: [0.109, 0.000, -0.764] extrinsicRot: [0, 0, 1, 0, -1, 0, 1, 0, 0] extrinsicRPY: [0, 0, 1, 0, -1, 0, 1, 0, 0]

Thanks for your help and time sincerely！

narutojxl commented 3 years ago

The first problem about imu axis not aligned with binded frame axis problem is caused by world file, when using with cmu indoor.world. The second problem, should lio-sam Horizon_SCAN param about vlp-16 be consistent with vlp-16 xacro samples param? I see the author's lio-sam Horizon_SCAN param still is 1800.

BTW: when use with @engcang's script lidar_repair.py, lio-sam's imageProjection node will crash, could you please give a little explaination why should we interchange point cloud's height - width with gpu="false" in vlp-16 xacro? Thanks a lot!