Cartographer_ros: Outdoor mapping using odometry

[aalapshah12297]

Hello.
I have been using cartographer for SLAM on an outdoor ground robot using a laser scanner (8m range, ~13Hz) and odometry (~20Hz). My odometry data is obtained after fusing sensor data from 2 encoders and an IMU, so it is fairly accurate (positional drift is about 20m after a 1000m long run). The twist values are slightly noisy though. However, the outdoor environment is fairly sparse at some locations, so cartographer may often receive empty range data or very few points making scan matching infeasible. During such patches, I do not expect anything more than the trajectory just getting updated from the odometry data and the map->odom transform remaining pretty much the same. Thus, I have tuned cartographer to give more weight to odometry:

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 300
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 70

As such, I mostly need cartographer to correct the global map through loop closure. The local maps don't need much correction. However, for some reason, cartographer publishes a very wrong map->odom transform. In this image, without the map->odom transform given by cartographer, the scan would have clearly matched better with the map. Try the demo using my launch file and check around ROS time 1520619573 for a better picture.

To further test if my data is correct, I just superimposed the laser scans (you can set Decay Time for the laser scan to a very high value to see this yourself) in Rviz and compared the results with cartographer. The superimposed scans match better to the real environment.

You can find the relevant data here:
bag file,
config and launch files,
output from rosbag_validate.

Some comments regarding the output from rosbag_validate:
The serialization time vs sensor time problem occurs only at the beginning, so it shouldn't be an issue.
The 'exactly the same range measurements multiple times' problem may be because of almost empty range data?
The 'overlapping ranges' error seems faulty. I will add another issue for that separately.

[gaschler]

These are a number of interesting questions. Without testing the actual files, I try to answer:

The twist values are slightly noisy though.

AFAIK, we don't use twist, only differences between poses in odometry.

cartographer may often receive empty range data or very few points making scan matching infeasible

This use case is hardly tested. The optimization result of the local scan matcher is used, even though with few points it will not be helpful.

The serialization time vs sensor time problem occurs only at the beginning, so it shouldn't be an issue.

The warning is shown only a limited number of times. This would be very unusual that your serialization is out of sync only for a fraction of the recording. Also, it explains...

However, for some reason, cartographer publishes a very wrong map->odom transform. In this image, without the map->odom transform given by cartographer, the scan would have clearly matched better with the map.

This happens when your serialization timestamps do not match the header timestamps.
Internally, SLAM only uses header timestamps, but cartographer_ros will extrapolate to match ros::Time::now. This explains exactly what you are seeing in rviz. Just record your bag again.
(An output map .pbstream should be fine, though.)

The 'exactly the same range measurements multiple times' problem may be because of almost empty range data?

Right, it reports if the average of the returns matches and there are returns.

The 'overlapping ranges' error seems faulty. I will add another issue for that separately.

Correct.

[aalapshah12297]

AFAIK, we don't use twist, only differences between poses in odometry.

Ohh, yes. You mentioned that in the previous issue that I had posted.

This use case is hardly tested. The optimization result of the local scan matcher is used, even though with few points it will not be helpful.

Any tips on what you would do if you were to use cartographer in such a scenario?
Case 1:
_Suppose the vehicle is moving in a sparse environment with no stationary objects within the range of the laser scanner. There is a person walking behind the vehicle and the only data that the scanner gets is from here (say only about 5-7 points per scan, that don't usually match nicely). After some time though, the vehicle arrives at a place which again has enough stationary objects for a good map._
Case 2:
_Same as case 1, minus the person. (So basically cartographer receives empty range data for some time and later gets normal data again.)_
If I tune TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight, and
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight to sufficiently high values, what is the expected output in each case?

This would be very unusual that your serialization is out of sync only for a fraction of the recording.

Okay, I checked this properly. The delay is 0.8s to 0.1s for some initial scans and and ~0.08s later scans. Is that still bad? I think I have a way to reduce the delay slightly. I will get back to you with a better bag file soon.

Internally, SLAM only uses header timestamps, but cartographer_ros will extrapolate to match ros::Time::now.

I am not sure if I understood this correctly. Are you saying that cartographer_ros will extrapolate all odometry messages to ros::Time::now (data as well as times tamp changed), convert scans to point clouds (only data changed) by subdividing and recombining, and then pass on these values to cartographer via some internal data type, whose synchronization is handled by cartographer itself (I believe?).
In that case, there shouldn't be a problem because my odometry message is well timed. rostopic delay /odom gives:

average delay: -0.000
    min: -0.045s max: 0.023s std dev: 0.01835s window: 200

Just record your bag again. (An output map .pbstream should be fine, though.)

Record without making any improvements? I have tried this multiple times and I don't think this will fix it. Also, how do I get the .pbstream file?

Maybe you can pinpoint the problem when you test the bag file. (It is attached with my original post, but maybe you missed it because I added 3 links in a single line.) If there is a simple fix for my issue, I am willing to make edits to the source code too. In the meantime, I will try to reduce the lag in the laser scan and get back to you.

[ojura]

You can easily correct the stamps in the bag using python. Just go through all messages and write them in a new bag, using the header stamps (if they are correct) as the new serialization time.

[aalapshah12297]

Thanks for the suggestion. I didn't know such a thing was even possible :open_mouth:. I used the code available here to fix my bagfile. I have added the fixed bagfile to my google drive folder. Now rosbag_validate no longer gives the 'sensor time vs serialization time' error.
Unfortunately, the results are still equally bad. Here is a screenshot at a point where the map->odom transform error is more prominent.
The new bag file still works fine if I just superimpose the stamps in rviz. This time, I also experimented randomly with some parameters;

options = {
...
  num_subdivisions_per_laser_scan = 1,
...
}
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1

This (weird) choice of parameters seems to give good results for the part with high amount of range data, but even worse results for the part where there is very little range data (as expected). Moreover:

• not splitting scans that are ~0.1s long is a bad practice, and
• I do not wish to set the translation and rotation weights this low for reasons that I have mentioned in my first post.

So this does not solve the problem, but maybe this information can help you pinpoint the problem.

To summarize:

• Timestamps are no longer a problem,
• Superimposed scans give a good map (implying correct and synchronized odometry & tfs)
• Cartographer + high weights -> bad map
• Cartographer + low weights + no scan division -> good map, but infeasible

[gaschler]

Closing for inactivity, feel free to post when there is something new.