Cartographer_ros: Help required in tuning cartographer 3D: Drift and loop closure

[shannonwarren]

Thank you for your effort in providing and maintaining this SLAM framework.

I need help in tuning cartographer 3D for an outdoor application. I'm using a single 16 lines 360 LIDAR from robosense and an MTI-30 IMU to perform SLAM. I used a 4 wheeled trolley to collect the data with the LIDAR mounted at around 0.89m(z) above the IMU(base of the trolley: half a feet above the ground).

I am in the initial phase of testing and currently looking at different SLAM methods for our application. Currently I use a single LIDAR and currently the way its mounted it doesn't provide entire(360) horizon information(will change this for future iterations). As mentioned the key for cartographer to work out of the box is to have two LIDARs mounted on either side spanning the entire horizon however would one LIDAR with an IMU and probably NavSatFix data suffice?( Our application would involve a max speed of 15 miles per hour). Any suggestions?

I used the configuration provided in backpack_3D demo with just the frame_ids and number of point clouds modified.

My issues:

• I noticed a lot of drift in z axis (around 5 - 10m), my initial guess was its due to the IMU since using rqt_plot showed that there were a lot of jumps in linear acceleration however faking IMU data doesn't help too.
• In the data set provided I completed a loop however cartographer doesn't seem to correct for loop closure(probably due to the massive drift in z)

1. My rosbag_validate is provided here
   https://gist.github.com/shannonwarren/38bb8366deadd9b1f14487585e9f25a0

2. I have attached my launch and config and Urdf file.

config file: https://gist.github.com/shannonwarren/8c89a8fd483f8fdcb8ef525722fa0a18
launch file:https://gist.github.com/shannonwarren/e944b3d212a497a5934282ce6c74e131
urdf file: https://gist.github.com/shannonwarren/ca213ca2e111ba978844f09b3f000a85

1. Link to the bag file provided here:
   https://drive.google.com/file/d/1R3Q9aeQWRtsqaWRmgWSanRpozQtBjDE6/view?usp=sharing

2. Few images from my initial tests

Thanks again.

[shannonwarren]

An update, I got loop closure to take care of the drift in x-y and z by setting the fast_correlative_scan matching linear search window in x-y and z to about 30m and 15m respectively and the map seems fairly good. However local SLAM still drifts a lot and it takes a loop to solve for the large drift. Any suggestions on avoiding Local SLAM drift for large data sets?

[gaschler]

Currently I use a single LIDAR and currently the way its mounted it doesn't provide entire(360) horizon information(will change this for future iterations).

That is fine. As long as you see some features all the time. Seeing just flat ground would not be enough.

As mentioned the key for cartographer to work out of the box is to have two LIDARs mounted on either side spanning the entire horizon however would one LIDAR with an IMU and probably NavSatFix data suffice?

Usually, we recommend to tune for 2D first.
However, judging from the images, your IMU to lidar rotation is not completely off, so you almost have the 3D system running.
NavSatFix will help outdoors but I definitely would tune without it first.

My rosbag_validate is provided here https://gist.github.com/shannonwarren/38bb8366deadd9b1f14487585e9f25a0

This is not rosbag_validate, please run again.

Few images from my initial tests

Looks fine except for you have no loop closure.

What are your constraint score histograms like?
Do you already have a single loop closure constraint?
Please run rviz, disable "INFRA" constraints and show only "INTER"=loop closure constraints.

You can post the rviz screenshot, may help more that the x ray images.

Also, in case loop closure is too slow because it searches to much, re-run the bag file with the offline node, that will wait for background work to finish.

Where is your IMU in the urdf?

setting the fast_correlative_scan matching linear search window in x-y and z to about 30m and 15m respectively and the map seems fairly good

Good. But make sure the search is also coarser otherwise it will take more CPU. fast_correlative_scan matching is only an initial step so it is fine to be coarse.

Local SLAM drift for large data sets?

In order of likeliness:
In most cases, get your IMU to lidar rotation correct (in the urdf). The side xray view tells me your extrinsic error is at the order of ~5deg currently.
Make sure your submaps are neither too short or too long.
Make sure all your timestamps are fine.
Tune local SLAM cost weights.

Hope this helps

[shannonwarren]

Thanks Gaschler.

You're right about the IMU to LIDAR rotation. I modified it in the order of ~3-5 degrees(pitch) and Local SLAM performs much better(drift in z is much less overtime). Do you have any suggestion on extrinsic calibrations I could use for LIDAR and IMU since I'm currently using a testing cart and there's no CAD available?

Few observations on cartographer performance for my data set, correct me if I'm wrong :

As mentioned I changed the URDF to accommodate for the pitch offset( which works better ) as well as POSE_GRAPH.optimization_problem.acceleration and rotation weight to a lower value of 500 and 100 respectively. I experimented with this weight from a value of 0 to 1000. My observation is that for my data set and IMU where there is a slight slope and its very jumpy setting the value to 0 doesn't help however if the area was a flat surface setting it to zero would work(Correct if wrong)?

[shannonwarren]

At the moment mapping does well in real time if there is a closed loop (one in my case), Local SLAM drifts less after altering the extrinsic and changing the IMU acceleration and rotation weights.

My IMU frame_id Xsens, I have attached my URDF( was getting better results with a 2 degrees pitch)
https://gist.github.com/shannonwarren/7f3c7001c1ef10748513cc5491d12f38

And the configuration file is
https://gist.github.com/shannonwarren/3781a0e401ae7ad874c772f8acd1ffe2

My log file with information on constraint histogram and IMU correction is
https://gist.github.com/shannonwarren/b1709f076f6123a1ac25ffdab2981428

A couple of questions on the log file generated. Firstly, the IMU correction at the start is very high and slowly stabilizes to a low value( shouldn't this value be as close to zero as possible)? If yes, would only calibration help? Additionally, the imu_gravity_time_constant parameter, how do I tune it for my IMU?

[shannonwarren]

A couple of images of the map generated, my total trajectory length is around 700m

pointcloud

Loop closed out here

x-ray image

[gaschler]

TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true

Is this really necessary?

TRAJECTORY_BUILDER_3D.submaps.num_range_data = 100

Please confirm this is 10-60 seconds worth of data.

My log file with information on constraint histogram and IMU correction is https://gist.github.com/shannonwarren/b1709f076f6123a1ac25ffdab2981428

The cartographer_rosbag_validate text output would be helpful.

POSE_GRAPH.constraint_builder.max_constraint_distance = 20.

Very uncommon to set.

POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 50.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 30.

Can likely be smaller. Costs CPU.

Just a hint, I'd try to use slightly larger voxel sizes. And also look at the submaps while Cartographer is running and confirm there are loop closure (=inter) contraints where you expect them.

Firstly, the IMU correction at the start is very high and slowly stabilizes to a low value( shouldn't this value be as close to zero as possible)?

Correct. It should be small.
You could tune local SLAM (no loop closure) with very high IMU weights to fix your urdf settings.

[shannonwarren]

Thanks Gaschler,

The rosbag validate is attached here:
https://gist.github.com/shannonwarren/b0ca062dd7d8d33510eb715b952a7653

[shannonwarren]

Setting the online correlative scan matching to true was just an initial safe bet since we were mapping offline and resources was not a concern. However turning it off doesn't make much of a difference.

I increased the submap_num_range data to around 200 which accounts for roughly 20 secs of collected scans worth of data.

Regarding the inter loop closure constraints, the initial min_score was 0.55 and had a couple of inter constraints during loop closure, I reduced the value to 0.4.

With a score of 0.40

[gaschler]

I took a quick look at your rosbag_validate results

W0415 09:39:24.988806 29022 rosbag_validate_main.cc:103] frame_id xsens time 1554318293464915990: IMU linear acceleration is 30.6687 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration -12.1051 0.743974 028.1689
W0415 09:39:25.067034 29022 rosbag_validate_main.cc:103] frame_id xsens time 1554318294615032911: IMU linear acceleration is 2.85751 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 1.02299 1.08618 2.43702
W0415 09:39:25.537302 29022 rosbag_validate_main.cc:103] frame_id xsens time 1554318300885647058: IMU linear acceleration is 31.6321 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration -11.7112 0-6.0759 028.7492

I0415 09:40:13.449625 29022 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/mti/sensor/imu" (frame_id: "xsens"):
Count: 72005  Min: 0.000318  Max: 0.024058  Mean: 0.010000

Your IMU data is very spiky and noisy, and also low frequency (100 Hz).
I think a good way to improve you map is improving the IMU input.

Also, please take a manual look at the gyroscope data, for instance with rqt plot.
cartographer_rosbag_validate does not analyse that.

Looking at your rviz screenshot, you seem to filter a lot of ground plane points. I'm not sure, but it may help to keep more of those.

Loop closure constraints look fine in number.
Please manually check their residuals, they are not shown in your screenshot.

[shannonwarren]

Thanks again Gaschler. The translation and rotation residuals seem fine ( as low as possible )
https://gist.github.com/shannonwarren/b1709f076f6123a1ac25ffdab2981428

I looked up the gyroscope readings in rqt_plot and they are very spiky in yaw and pitch (maybe cause of the lack of suspension on my testing cart and not a very smooth surface)

I will increase the frequency of the IMU to 500hz and further investigate.

[shannonwarren]

Hi Gaschler,

Thanks for all the assistance. Local SLAM works much better with a higher IMU frequency(set it to 400 hz). Following your previous suggestions on parameter tuning, I have been able to achieve low drift and highly accurate maps over a km long without NavsatFix and when provided by setting its weight very high in the pose graph it works even better( rtf fix ). Will be working on localization using cartographer now. Thanks again

[shannonwarren]

Regarding localization, can cartographer use gnss_pose as the initial guess to localize?

[MrGitGo]

Hey @shannonwarren did you use the assets_writer and used a bag_file to build this:

I am asking because I am looking for a way to generate a 3d map while I am mapping (online/live). And I looked for a solution but I think it works only with the bag_files

[shannonwarren]

@MrGitGo Yes I used the assets writer, Cartographer doesn't support building 3D(point cloud) maps on the fly however for visualizations you could increase the decay time in Rviz however its not optimized.

[huangshuqing]

hey,@shannonwarren .Could you provide the final lua file for my reference. I am also tuning the cartographer 3D with a VLP16 laser and a xsens MTi-710 imu.