Tilted 3D map with floam

[nugu_]

Hi,

I’m using an ouster os0-64-U and aim to 3D-map an unknown indoor environment with simultaneous localization. Therefore, I use rtabmap’s icp_odometry with floam and the robot_localization package.  Currently everything is working quite well, but the generated 3D-map is tilted:




Also, the odometry by icp_odmetry is off as the robot model lifts off by up to 60cm from the ground in rviz. This problem seems to have the same cause as the tilted map, as the maximum robot liftoff happens where the 3D map also lifts furthest off the ground. (far right in the first picture, far left in the second picture)

My launch file for mapping and localization:
launch_rtabmap_test.launch


My config file for icp_odometry
cfg_icp_odom.yaml

My config file for rtabmap:
cfg_rtabmap_mapping.yaml


I already looked at: this thread in the forum and the test_outser_launch files on github.
I assume the problem comes from not deskewing the point cloud of the ouster. I was not able to make the deskewing run reasonably, I always faced some transform or frame issues. In the launch file you can see my approach to deskewing commented. I couldn’t figure out which frames to use for the fixed and base frame of the imu_to_tf. I feel like there is something slightly off with my tf.


My tf_tree:
frames.png


Rosbag: Rosbag


I’m grateful for any type of help! Thank you!

[matlabbe]

Hi,

Thx for sharing the data. I tried multiple configs to make deskewing working with the provided imu, but it just seems that the  TF between imu to lidar is wrong (not sure why though, I saw that on other ousters too), so we cannot really use it for deskewing or aligning the map with gravity. I tried enabling (with the option in my example below) deskewing with the actual motion estimation from icp_odometry, but the resulting point clouds are pretty close to each. Probably because the robot is not moving that fast and the lidar frame rate is high (20 Hz) unlike the usual 10 Hz (see this video for more intense skewing).

Note that to avoid TF already recorded in the bag, I just ignored them all.

roslaunch test.launch
rosbag play --clock tf:=/not_used tf_static:=/not_used 2024-08-16-15-08-03.bag

The launch file I tested:

<!-- -->
<launch>
<param name="use_sim_time" value="true"/>
<arg name="scan_topic" value="/ouster/points"/>
<arg name="frame_id" value="os_sensor"/>
<arg name="deskewing" value="false"/>

<group ns="rtabmap">
<!-- Start Odometry Node  -->
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" value="$(arg frame_id)"/>
<param name="odom_frame_id" value="odom"/>
<param name="deskewing" value="$(arg deskewing)"/>
<param name="publish_tf" type="bool" value="true"/>
<rosparam command="load" file="$(find rtabmap_launch)/config/cfg_icp_odom.yaml"/>
</node>

<!-- Start Mapping Node -->
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" value="$(arg frame_id)"/>
<param name="map_frame_id" value="map"/>
<param name="subscribe_depth" value="false"/>
<param name="subscribe_rgb" value="false"/>
<param name="subscribe_scan_cloud" value="true"/>
<param name="approx_sync" value="true"/>
<rosparam command="load" file="$(find rtabmap_launch)/config/cfg_rtabmap_mapping.yaml"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
<remap from="odom" to="odom"/>
<remap from="grid_map" to="/map"/>
</node>

<node name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="false"/>
        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
      </node>

</group>
</launch>

odom config:

Icp/PointToPlane: true
Icp/Iterations: 10
Icp/VoxelSize: 0.2
Icp/DownsamplingStep: 1
Icp/Epsilon: 0.001
Icp/PointToPlaneK: 20
Icp/PointToPlaneRadius: 0
Icp/MaxTranslation: 2
Icp/MaxCorrespondenceDistance: 2
Icp/PM: true
Icp/PMOutlierRatio: 0.7
Icp/CorrespondenceRatio: 0.01
Icp/Force4DoF: true

Odom/GuessSmoothingDelay: 0.3
Odom/ScanKeyFrameThr: 0.6
Odom/Strategy: 0
OdomF2M/ScanSubtractRadius: 0.2
OdomF2M/ScanMaxSize: 15000

rtabmap config:

Rtabmap/DetectionRate: 1
RGBD/NeighborLinkRefining: false
RGBD/ProximityBySpace: true
RGBD/ProximityMaxGraphDepth: 0
RGBD/ProximityPathMaxNeighbors: 1
RGBD/LocalRadius: 2
RGBD/AngularUpdate: 0.05
RGBD/LinearUpdate: 0.05
Mem/NotLinkedNodesKept: false
Mem/STMSize: 30
Mem/LaserScanNormalK: 20
Reg/Strategy: 1
Optimizer/GravitySigma: 0.3
Optimizer/Strategy: 1

Grid/3D: true
Grid/CellSize: 0.05
Grid/ClusterRadius: 0.4
Grid/Sensor: 0
Grid/MaxObstacleHeight: 1.8
Grid/MaxGroundHeight: 0.3
Grid/NormalsSegmentation: true
Grid/RayTracing: true
GridGlobal/MinSize: 200

Icp/VoxelSize: 0.2
Icp/PointToPlaneK: 20
Icp/PointToPlaneRadius: 0
Icp/PointToPlane: true
Icp/Iterations: 1
Icp/Epsilon: 0.001
Icp/MaxTranslation: 3
Icp/MaxCorrespondenceDistance: 2
Icp/Strategy: 1
Icp/OutlierRatio: 0.7
Icp/CorrespondenceRatio: 0.2

With and without loop closures
:




Here some side views:






The small slope is caused by the lidar not starting exactly aligned with the gravity, and we don't have valid imu to re-align correctly. To do it automatically, you would need a better imu (maybe less noisy) with TF perfectly aligned with lidar frame. With CloudCompare, we can easily click three points to align the floor with xy plane though.

[nugu_]

Hi,

thank you for your response. I also assumed that the slope in the map comes from the lidar not being perfectly aligned with gravity in the beginning. I tried to calculate the starting position with the imu of the ouster, but as you said the imu is very noisy and poorly calibrated. You said the transform between the imu and the lidar frame seems off. I found this github issue. There they say that the pointcloud published by the ouster is in the os_sensor frame, not in the os_lidar frame. The os_sensor frame marks the bottom of the ouster, not the actual lidar that captures the point cloud. May this be an issue here?

When I use your parameters, the localization gets completely lost after some driving and the robot starts drifting across the map. Also, the slope in the map I experience is greater than just an initial orientation offset to gravity I believe. As mentioned, the robot lifts of the ground by more than 60cm at the highest point, when exploring the building.

Another thing I noticed is that when I start my mapping/ localization node and don’t move at all, the output of the icp_odometry is not at 0, 0, 0 or around these values, which it should. It always has an offset of around 4 cm in x direction and around 10cm in z direction. I assume this comes from errors in my urdf model, so the transform between the sensor frame and the base_footprint frame does not match the real-life properties of the robot. However, after remeasuring the robot and adjusting the urdf, I was not able to fully eliminate this initial offset. Is there any other reason you can think of why this happens?

Thank you for your help.

[matlabbe]

Hi,

In my test, the odometry will show only about 5 cm error in z between the beginning and the end of the trajectory. Also, I ignored /tf and /tf_static from the bag when replaying, and just estimate directly in os_sensor. I didn't read the whole issue, but it seems that lidar scans are projected in os_sensor instead os_lidar frame. That should not be a problem if the resulting xyz point cloud is in os_sensor frame. That could be a problem if the lidar scans are projected in os_lidar frame, but published point cloud has os_sensor frame_id instead. For the IMU frame, an experiment could be to stick another IMU on the lidar, and check if the values are aligned together.

If you tried exactly what I did with the rosbag and results are that different, maybe the rtabmap version is different. If you are using the ros binaries, that should be similar. If you built from source, make sure rtabmap library is built with libpointmatcher. Another thing that could be different is the computation power that may be lower than on computer I tested. If so, you can increase "Icp/VoxelSize" and "OdomF2M/ScanSubtractRadius" parameters. I think you can also remove "Icp/Force4DoF: true", probably forgot to remove it after testing with imu.

If you just started the mapping, icp_odometry should return values around (0,0,0) or exactly (0,0,0) for the first frame. An error in URDF would not show up there. Doublecheck that you don't have topics published from two different sources on same topic name. For example, I don't remember if the rosbag you sent had already some odometry topics in it, but if you are testing with a bag, make sure to remap the topics in the bag to some random name to avoid confusion with topics regenerated.

cheers,
Mathieu