Optimal configuration for VLP16

[ozgudum]

I have spent considerable amount of time on tuning the configuration parameters to get a good registration and loop closure performance without any luck. I was able to get a reasonable result in one set but when I tried to use the same config for another, it didn't work well. Having too many parameters makes it hard to see the individual effects of each of parameter.

I have a similar hardware configuration (VLP16) with the one mentioned in this post. Changes and comments in this (SirVer@72c4519) commit helped a lot to understand the effects but I couldn't achieve loop closure in this particular bag file. Here is my current configuration. Reverted them back to original because previous set of tuned parameters messed up in this bag file.

This is what I currently get:

Obviously, the overlap is small and we can only get the loop closure at the very end. I am also wondering which parameters would possibly reduce the drift in the trajectory. Otherwise we would need to set a large search_window to get loop closure.

Explanations for the config parameters here and here were helpful but I wish they would be complete.

[ozgudum]

I have collected another dataset which, I believe, is more closer to what cartographer ideally expects. However, couldn't achieve loop closure yet. New bag file contains two loops and each lidar message contains only one velodyne UDP package. So, message frame rate increased to ~750 hz from 10 hz. Configuration file is still same, except that scans_per_accumulation parameter is changed from 1 to 75.

[SirVer]

@ozgudum Sorry for the late reply. I am quite back logged on issues these days.

I only looked at sensor.bag (the one with the tiny point clouds). My changes are the last few commits in here.

My final result is this:

Some assorted notes:

• The local SLAM tracks nicely, loop closure has issues.
• The local SLAM has a z-drift issue - after one loop without loop closure, the local slam thinks you are 20 meters below where you started. This makes me believe that either your URDF is not quite correct (angle between IMU and laser) or that your IMU has a bias. I cranked up the loop closure search window in z-direction to deal with this drift.
• Your loops are big (100's of meters driven), so your loop closure search space must be adapted to deal with larger drift accordingly. I cranked up the xy search window for this.
• These changes would have probably been enough to fix the issue, but it did not work at first. So I tweaked other parameters. Later I realized that I did not change MAP_BUILDER.sparse_pose_graph.constraint_builder.max_constraint_distance, so even though the sparse pose graph was looking for matches in a large area, it never actually added them to the optimization problem :/. @wohe Do we require this parameter for something still? It seems the search window is sufficient to handle this.

My parameters give good results, but loop closure is not real-time with these aggressive settings. This is not surprising: I arbitrarily picked really big values for the search space for loop closure. I did not spend time making the parameters faster again - I think you can tune them now to your needs.

[zerox3802567]

hi, @ozgudum
I just followed your current file and then run it, unfortunately, when I input the comment roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/abc.bag
the terminal showed the errors.
I had done some changes posted by @SirVer and it didn't work, either.

Do you have any idea about the error? I really need your help. Thanks a lot!

[mdemirst]

hi @zerox3802567, unfortunately it is been quite a long time since last time I worked on Cartographer and I don't have an idea about the error. Maybe @SirVer may give you some hints?

[SirVer]

posting screenshots of text is not a great way of sharing error messages. Please copy & past and put them into a gist.

@zerox3802567 Your problem is that your configuration is not up to modern requirements of cartographer. It says in the first few lines: Key 'use_odometry_data not in dictionary`. Compare your lua files and make sure they contain the keys that are there at master.

[liangxiujuan]

这是我运行检查数据的语句cartographer_rosbag_validate -bag_filename /media/autodrive/LidarDisk/testdata/data/loop.bag
这是我运行检查数据的结果
W0322 10:09:00.999109 15504 rosbag_validate_main.cc:352] frame_id "velodyne_link" on topic /velodyne has serialization time 1463514862.516749911 but sensor time 1463514862.387913000 differing by -0.128837 s.
E0322 10:09:01.002915 15504 rosbag_validate_main.cc:350] frame_id "imu_link" on topic /imu has serialization time 1463514862.945365562 but sensor time 1463514862.000000000 differing by -0.945366 s.
E0322 10:09:01.003001 15504 rosbag_validate_main.cc:350] frame_id "imu_link" on topic /imu has serialization time 1463514862.945365562 but sensor time 1463514862.004999876 differing by -0.940366 s.
E0322 10:09:01.003028 15504 rosbag_validate_main.cc:350] frame_id "imu_link" on topic /imu has serialization time 1463514862.945365562 but sensor time 1463514862.009999990 differing by -0.935366 s.
E0322 10:09:01.005043 15504 rosbag_validate_main.cc:160] Sensor with frame_id "velodyne_link" is not sequential in time.Previous range message ends at time 635991116628855770, current one at time 635991107350514460
E0322 10:09:01.005100 15504 rosbag_validate_main.cc:315] Sensor with frame_id "velodyne_link" jumps backwards in time. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor.
W0322 10:09:04.093914 15504 rosbag_validate_main.cc:203] Sensor with frame_id "velodyne_link" range measurements have longest overlap of 927.834 s
I0322 10:09:04.095104 15504 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/imu" (frame_id: "imu_link"):
Count: 88364 Min: 0.005000 Max: 0.015000 Mean: 0.005003
[0.005000, 0.006000) #################### Count: 88361 (99.996605%) Total: 88361 (99.996605%)
[0.006000, 0.007000) Count: 0 (0.000000%) Total: 88361 (99.996605%)
[0.007000, 0.008000) Count: 0 (0.000000%) Total: 88361 (99.996605%)
[0.008000, 0.009000) Count: 0 (0.000000%) Total: 88361 (99.996605%)
[0.009000, 0.010000) Count: 0 (0.000000%) Total: 88361 (99.996605%)
[0.010000, 0.011000) Count: 2 (0.002263%) Total: 88363 (99.998871%)
[0.011000, 0.012000) Count: 0 (0.000000%) Total: 88363 (99.998871%)
[0.012000, 0.013000) Count: 0 (0.000000%) Total: 88363 (99.998871%)
[0.013000, 0.014000) Count: 0 (0.000000%) Total: 88363 (99.998871%)
[0.014000, 0.015000] Count: 1 (0.001132%) Total: 88364 (100.000000%)
E0322 10:09:04.095139 15504 rosbag_validate_main.cc:382] Point data (frame_id: "velodyne_link") has a large gap, largest is 927.1 s, recommended is [0.0005, 0.05] s with no jitter.
I0322 10:09:04.095232 15504 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/velodyne" (frame_id: "velodyne_link"):
Count: 4443 Min: -927.834106 Max: 927.099548 Mean: 0.099322
[-927.834106, -742.340698) Count: 1 (0.022507%) Total: 1 (0.022507%)
[-742.340698, -556.847351) Count: 0 (0.000000%) Total: 1 (0.022507%)
[-556.847351, -371.354034) Count: 0 (0.000000%) Total: 1 (0.022507%)
[-371.354034, -185.860687) Count: 0 (0.000000%) Total: 1 (0.022507%)
[-185.860687, -0.367310) Count: 0 (0.000000%) Total: 1 (0.022507%)
[-0.367310, 185.126068) #################### Count: 4441 (99.954987%) Total: 4442 (99.977493%)
[185.126068, 370.619415) Count: 0 (0.000000%) Total: 4442 (99.977493%)
[370.619415, 556.112793) Count: 0 (0.000000%) Total: 4442 (99.977493%)
[556.112793, 741.606140) Count: 0 (0.000000%) Total: 4442 (99.977493%)
[741.606140, 927.099548] Count: 1 (0.022507%) Total: 4443 (100.000000%)

这是我运行程序的语句roslaunch cartographer_ros demo_vlp16-imu.launch bag_filename:=/media/autodrive/LidarDisk/testdata/data/loop.bag
这是我运行程序的结果
... logging to /home/autodrive/.ros/log/e7601f96-4c48-11e9-b5f1-9c5c8e75471b/roslaunch-navinfo-drive-17246.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://navinfo-drive:36831/

SUMMARY

PARAMETERS

• /robot_description: <...>
• /rosdistro: kinetic
• /rosversion: 1.12.14
• /use_sim_time: True

NODES
/
cartographer_node (cartographer_ros/cartographer_node)
playbag (rosbag/play)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [17256]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to e7601f96-4c48-11e9-b5f1-9c5c8e75471b
process[rosout-1]: started with pid [17270]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [17276]
process[cartographer_node-3]: started with pid [17289]
process[rviz-4]: started with pid [17296]
process[playbag-5]: started with pid [17308]
[ INFO] [1553221162.064140484]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer16/install_isolated/share/cartographer_ros/configuration_files/vlp16-imu.lua' for 'vlp16-imu.lua'.
[ INFO] [1553221162.064433298]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1553221162.064482927]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1553221162.064559138]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1553221162.064603825]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1553221162.064724999]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1553221162.064761309]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1553221162.064818156]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1553221162.064851914]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1553221162.064945879]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1553221162.064980683]: I0322 10:19:22.000000 17289 configuration_file_resolver.cc:41] Found '/home/autodrive/catkin_cartographer/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1553221162.071368082]: I0322 10:19:22.000000 17289 submap_3d.cc:321] Added submap 1
[ INFO] [1553221162.071446904]: I0322 10:19:22.000000 17289 map_builder_bridge.cc:130] Added trajectory with ID '0'.
[ INFO] [1553221162.615031407, 1463514862.951122758]: I0322 10:19:22.000000 17289 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '635991116623879130'.
F0322 10:19:23.173935 17289 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->GetTime() (635991116628855770 vs. 635991107350514460) Non-sorted data added to queue: '(0, points2)'
[FATAL] [1553221163.174290436, 1463514864.064097180]: F0322 10:19:23.000000 17289 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->GetTime() (635991116628855770 vs. 635991107350514460) Non-sorted data added to queue: '(0, points2)'
*** Check failure stack trace: ***
@ 0x7f97f73b35cd google::LogMessage::Fail()
@ 0x7f97f73b5433 google::LogMessage::SendToLog()
@ 0x7f97f73b315b google::LogMessage::Flush()
@ 0x7f97f73b5e1e google::LogMessageFatal::~LogMessageFatal()
@ 0x70e141 cartographer::sensor::OrderedMultiQueue::Dispatch()
@ 0x70ed58 cartographer::sensor::OrderedMultiQueue::Add()
@ 0x70acc0 cartographer::sensor::Collator::AddSensorData()
@ 0x65bf4d cartographer::mapping::CollatedTrajectoryBuilder::AddData()
@ 0x65e215 cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
@ 0x5ebb33 cartographer_ros::SensorBridge::HandleImuMessage()
@ 0x5bc927 cartographer_ros::Node::HandleImuMessage()
@ 0x5cc2ce boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0x5d9a09 ros::SubscriptionCallbackHelperT<>::call()
@ 0x7f97f4f2dd8d ros::SubscriptionQueue::call()
@ 0x7f97f4ed3838 ros::CallbackQueue::callOneCB()
@ 0x7f97f4ed523b ros::CallbackQueue::callAvailable()
@ 0x7f97f4f31e39 ros::SingleThreadedSpinner::spin()
@ 0x7f97f4f16e9b ros::spin()
@ 0x5b1da4 cartographer_ros::(anonymous namespace)::Run()
@ 0x5aee34 main
@ 0x7f97f3923830 __libc_start_main
@ 0x5b18c9 _start
@ (nil) (unknown)
[cartographer_node-3] process has died [pid 17289, exit code -6, cmd /home/autodrive/catkin_cartographer16/install_isolated/lib/cartographer_ros/cartographer_node -configuration_directory /home/autodrive/catkin_cartographer16/install_isolated/share/cartographer_ros/configuration_files -configuration_basename vlp16-imu.lua points2:=velodyne __name:=cartographer_node __log:=/home/autodrive/.ros/log/e7601f96-4c48-11e9-b5f1-9c5c8e75471b/cartographer_node-3.log].
log file: /home/autodrive/.ros/log/e7601f96-4c48-11e9-b5f1-9c5c8e75471b/cartographer_node-3*.log
================================================================================REQUIRED process [rviz-4] has died!
process has finished cleanly
log file: /home/autodrive/.ros/log/e7601f96-4c48-11e9-b5f1-9c5c8e75471b/rviz-4*.log
Initiating shutdown!

[playbag-5] killing on exit
[rviz-4] killing on exit
[robot_state_publisher-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
这是我报的错误
[FATAL] [1553221163.174290436, 1463514864.064097180]: F0322 10:19:23.000000 17289 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->GetTime() (635991116628855770 vs. 635991107350514460) Non-sorted data added to queue: '(0, points2)'
这个错误是说数据里的点云没有按照时间排列？请问您是怎么运行成功的？