The Problem (TL;DR)

I can get Google Cartographer to run in 2D mode, but not 3D. I want to write a .launch and .lua file that will get Cartographer (in 3D mode) to smoothly run over my bagfile.

The Problem (full explanation)

I've got a Velodyne HDL-32 mounted on a Clearpath Jackal UGV, and I'm trying to get Google Cartographer (in 3D mode) working with it. I've collected a short bagfile with all the necessary sensor and TF data, and I can get Cartographer working in 2D mode (see image below).

Completed map from Cartographer (2D):

But I can't for the life of me get it working in 3D, mostly because I don't really understand how to tune the myriad of settings Cartographer offers. Unfortunately I haven't found any demo files that use a similar setup (which I could therefore copy). The closest thing is the 3ddemopackpack.launch and backpack_3d.lua files. However, when I try a modified version of these files, this is what I get:

My best result from Cartographer (3D)

It looks like the raw data simply isn't being matched properly, and I suspect that fixing this may well be a case of tweaking the right parameters. However, none of the tinkering or tweaking I've done so far has produced anything better than what you see above. Everything under "options={}" in the .lua files is well documented here, and I think I've got those right, but the other parameters aren't documented at all and unfortunately I'm mostly guesstimating at what they do.

I'd like know how to write/configure a set of launch/lua files to get Cartographer working on my system. I suspect that my use case - one UGV and one 3D LiDAR - will be a relatively common as Cartographer grows in popularity. So hopefully any answers or advice here can help others with a similar setup. Thanks in advance for any help or advice anyone can provide. The details of my system are below.

System Information

• OS: Ubuntu 14.04 (laptop), ROS Indigo Igloo (robot)
• Robot: Clearpath Robotics Jackal UGV (real, not simulated)
• Sensors: IMU, wheel encoders, Velodyne HDL-32E
• TF tree: tf_frames
• Working 2D config files: cartographer_2d.launch, cartographerconfiguration2d.lua
• Video example of working 2D Cartographer: cartographer2drviz_output.mp4
• NOT working 3D config files: cartograph_3d.launch, cartographerconfiguration3d.lua
• Sample bagfile: test_data.bag
• Notes: The /odom -> /base_link transformation is provided by an EKF, courtesy of robot_localization, which fuses IMU and wheel encoder data. The bagfile contains both PointCloud2 data (from the Velodyne drivers) on the /velodyne_points topic. It also contains LaserScan data (from the pointcloudtolaserscan package) on the /scan topic. So in theory the same bagfile can be fed to the 2D or 3D version of Cartographer.

Updates

Update 1

With some help from the developers, I've got a working configuration. The three key parameter changes for me were:

1. TRAJECTORY_BUILDER_3D.scans_per_accumulation = 1 I don't really understand this parameter but it seems to be the number of scans (i.e. 360 degree rotation) per PointCloud2 message. Setting this to 1 made the difference between getting nothing and something.
2. TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true As per the tuning guide, the correlative scan matcher matches each new scan to the existing submap. This defaults to off, but without it the map quickly becomes a black smudge.
3. SPARSE_POSE_GRAPH.matcher_rotation_weight = 1.6e5 Every time a new submap is generated and matched to the previous one, this is the variable that determines the penalty for rotating the new submap. I.e. the higher it is, the less the new submap will be rotated to fit. Defaults to 1.6e3, I've had to set it higher (likely due to IMU problems that I'm currently experiencing) otherwise each new submap is added at a ridiculous angle.

I've updated the 3D .lua file with my most recent changes. This at least gives me a recognizable result:

Although it's still far from perfect, and the global scan matcher needs a lot of tuning to correctly re-align the submaps.

Update 2

Long story short, Cartographer is VERY dependent on good IMU data, and my IMU yaw drifts far more than it should. I've implemented a zero velocity update to periodically de-bias the yaw angular velocity from the IMU and this has helped quite a lot.

Prior to semi-resolving my IMU issues I had to weight a lot of the rotation penalties quite highly, to prevent submaps from being mis-aligned at creation. Now, I have been able to reduce those weights which has somewhat improved the performance of later scan-matching.

I've also found that loop-closure is better and more reliable if I reverse the direction of travel and go down the long straight before taking the square-ish detours around the cubicles.

So now I get this:

I've updated the 3D config file with the one I used to achieve the image above. I'll update the bagfile shortly. Obviously there is still room for improvement, and there are two issues in particular I want to focus on:

1. The vertical drift (see below)
2. The patchy occupancy map - free space is never marked solid white like it should be. The 2D version of my data and 3D backpack demo do this correctly, but my 3D data doesn't.