Cartographer_ros: finish trajectory service crashes

[srik901]

finish trajectory service called with

rosservice call /finish_trajectory "stem: "full_traj'" gives
F0215 12:31:23.674871 21269 hybrid_grid.h:384] Check failed: new_bits <= 8 (9 vs. 8)
This works for smaller trajectories with 2-3 GB bag files. This was with a 9GB bag file with two VLP-16 and a xsens MTI-g 700 imu. The same bag file when split into two works for finishing trajectory and is able to write both the ply and the png files.

* Check failure stack trace: *
@ 0x7f2bd3d655cd google::LogMessage::Fail()
@ 0x7f2bd3d67433 google::LogMessage::SendToLog()
@ 0x7f2bd3d6515b google::LogMessage::Flush()
@ 0x7f2bd3d67e1e google::LogMessageFatal::~LogMessageFatal()
@ 0x5d2c70 cartographer::io::XRayPointsProcessor::Insert()
@ 0x5d3294 cartographer::io::XRayPointsProcessor::Process()
@ 0x5cf1b1 cartographer::io::PlyWritingPointsProcessor::Process()
@ 0x51aee6 cartographer_ros::WriteAssets()
@ 0x50e6f0 cartographer_ros::MapBuilderBridge::WriteAssets()
@ 0x4ef35a _ZN5boost6detail8function21function_obj_invoker2IZN16cartographer_ros12_GLOBAL__N_13RunEvEUlRN21cartographer_ros_msgs24FinishTrajectoryRequest_ISaIvEEERNS5_25FinishTrajectoryResponse_IS7_EEE4_bS9_SC_E6invokeERNS1_15function_bufferES9_SC_
@ 0x4f87e0 ros::ServiceCallbackHelperT<>::call()
@ 0x7f2bd0e25811 ros::ServiceCallback::call()
@ 0x7f2bd0e78cf0 ros::CallbackQueue::callOneCB()
@ 0x7f2bd0e7a0f3 ros::CallbackQueue::callAvailable()
@ 0x7f2bd0ed2691 ros::SingleThreadedSpinner::spin()
@ 0x7f2bd0eb772b ros::spin()
@ 0x4f0dde cartographer_ros::(anonymous namespace)::Run()
@ 0x4ed514 main
@ 0x7f2bcf8d1830 __libc_start_main
@ 0x4eef99 _start

@ (nil) (unknown)

Launch file

textfile="/home/srik/catkin_ws/src/cartographer_ros/cartographer_ros/urdf/car_velodyne_imu.urdf" />

type="robot_state_publisher" />

type="cartographer_node" args="
-configuration_directory /home/srik/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files
-configuration_basename car_velodyne_imu.lua"
output="screen">






args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />

urdf

---

Configuration file

include "map_builder.lua"

options = {
map_builder = MAP_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = false,
num_point_clouds = 2,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 2e-2
}

TRAJECTORY_BUILDER_3D.scans_per_accumulation = 2

-- No point of trying to SLAM over the points on your car.
TRAJECTORY_BUILDER_3D.laser_min_range = 2

-- These were just my first guess: use more points for SLAMing and adapt a bit for the ranges that are bigger for cars.
--change both the high and low to 8 and 400 from 5 and 250
TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_length = 8.
TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.min_num_points = 400.
TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_length = 8.
TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.min_num_points = 400.

-- The submaps felt pretty big - since the car moves faster, we want them to be
-- slightly smaller. You are also slamming at 10cm - which might be aggressive
-- for cars and for the quality of the laser. I increased 'high_resolution',
-- you might need to increase 'low_resolution'. Increasing the
-- '*num_iterations' in the various optimization problems also trades
-- performance/quality.
--sri changed both high and low to 0.15 and rotation weight to 1
TRAJECTORY_BUILDER_3D.submaps.num_laser_fans = 80
TRAJECTORY_BUILDER_3D.submaps.high_resolution = .25
TRAJECTORY_BUILDER_3D.submaps.low_resolution = .25
--TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight = 1

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
MAP_BUILDER.sparse_pose_graph.optimization_problem.huber_scale = 5e2
--MAP_BUILDER.sparse_pose_graph.optimization_problem.rotation_weight = 1

-- Trying loop closing too often will cost CPU and not buy you a lot. There is
-- little point in trying more than once per submap.
MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 100
MAP_BUILDER.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
MAP_BUILDER.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 100
-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure
-- constraints.
MAP_BUILDER.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter

-- This is probably the most important change: lower the matching score for
-- adding constraints.
--changed the angular search to 90 deg from 30 deg
MAP_BUILDER.sparse_pose_graph.constraint_builder.min_score = 0.4

MAP_BUILDER.sparse_pose_graph.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 20.
MAP_BUILDER.sparse_pose_graph.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 5.
MAP_BUILDER.sparse_pose_graph.constraint_builder.fast_correlative_scan_matcher_3d.angular_search_window = math.rad(90.)

return options

[SirVer]

This indicates that your HybridGrid became too big. What is the area you are trying to cover here?

Two possible solutions:
1) Change the hybrid grid template parameters to allow for larger maps in the source code. This will require more memory and cost performance.
2) SLAM at a higher resolution. If your high resolution is 25 cm, your low resolution should probably be a meter or so.

[srik901]

Tried the following:

high resolution at 0.25 and low resolution at 1m (using only a single laser and using both lasers)
high resolution at 0.5 and low resolution at 0.5m (using only a single laser and using both lasers

All of them fail at the finish trajectory rosservice call with the same error at the end. The bag files are ~1079 seconds long. I can run it successfully upto 700 seconds.

The trajectory that I am mapping is approximately 10 km long. All of the above work if the trajectory is ~5km long ie I can split into two separate runs of 5km each and they work

Could you let me know how I should go about changing the hybrid grid parameters?

[srik901]

The above are the two split trajectories. My bag files are split every minute. So I can run the first 10 bag files and it works, as soon as I add the 11th bag file rosservice /finishtrajectory gives the above error

[srik901]

Also tried 0.25m and 2.0m f for high resolution and low resolution using only a single LIDAR. Still get the same error

[SirVer]

Could you make your data and configuration available?

[SirVer]

Closing for inactivity. Please speak up if this is still an issue for you - in which case I'd require a repro data set.

[nobertgmuedu]

Hi I had same problem, hopefully resolution will fix it. The dataset that I have that is reproducing this issue is http://kitti.is.tue.mpg.de/kitti/raw_data/2011_10_03_drive_0034/2011_10_03_drive_0034_sync.zip with calibration file http://kitti.is.tue.mpg.de/kitti/raw_data/2011_10_03_calib.zip. I used kitti2bag to turn it into bag files.

[wohe]

As @SirVer said this is due to the submaps getting too large, the size here being in voxels. You can improve this by increasing the voxel size (resolution), reducing the max_range or high_resolution_max_range, or reducing num_range_data. If you need more help, please provide your full configuration so that we can reproduce.

[nobertgmuedu]

Thank You, this weekend I will organize what I have and upload it so you can inspect and reproduce what I'm doing. Using Kitti Dataset, 2011_10_03 drive 34, it has 5 loop closures (outside road data) however with my current configuration it seems like there is some drift and it is not closing the loops. In the case of the first loop, the road has drifted slightly above the plane of where it should be and slightly to the left. When it approaches the second loop on what should be the same road, its maping a slightly offset road on a slightly different plane. When all is done however, I am unable to create ply file or png files with asset writer with same above error. I've tried numerous different adjustments. At one point I watched asset writer climb to 48gb memory (my workstation had 16gb ram w/ 32gb of swap) and then it would die, i since increased the swap to 128gb, and now i'm back at the error above with map growing to large. I will uploaded and provide full details this weekend. Thank You!!

-Nathan Obert/ George Mason University

[nobertgmuedu]

Hi, I have uploaded my configurations ( I have changed then around a bunch of different ways kinda trial and error). Any advice you can provide would be awesome.

I put the files up on github at: https://github.com/nobertgmuedu/gmukitti

We are using Kitti Dataset, with Cartographer ROS. I am using a modified kitti2bag to convert the kitti datafiles to bag files (provided on github). It basically adds compression setting, ditches the images, and also ditches the TF frame, which allows me to configure robot with URDF instead. I hosted a bag file on amazon S3 at https://s3.amazonaws.com/kittigmu/kitti_2011_10_03_drive_0027_synced.bag .

Obviously the launch files, configurations, etc are all on github as well, should be ready to run with minimal if any hassle.

Here is a quick overview of the route, https://github.com/nobertgmuedu/gmukitti/blob/master/kitti_2011_10_03_drive_0027_synced.png there should be 5 loop closures (i marked green, purple, blue, red, and yellow) and near the end it pretty much totally gets lost running in circles. To be fair I think this is one of the most complex Kitti files in their entire dataset. The black arrows are where the car "should" be traveling, instead of the big loop you see it doing at the end.

This is the same dataset that the asset writing it crashing on too. With the smaller resolution it does better on some of the loop closure, like the blue dots "nearly" touch.

Lastly, here is a youtube video of the same dataset being navigated with other software for comparison sakes: https://www.youtube.com/watch?v=KYvOqUB_odg

Anyways, any feedback/suggestions on getting KITTI working better with Cartographer are more than welcome.

Thanks!

-Nathan

[ojura]

That's an interesting approach to life-long mapping in the linked video - do not add keyframes in visited areas, but do it when exploring new areas. Perhaps Cartographer's submap/node trimming could be expanded to behave like this, instead of always doing FIFO discarding? The planned functionality of being able to trim trajectories in the middle sounds like a way towards this. Sorry for going off topic here, perhaps an issue could be opened for this if there isn't one already.

[SirVer]

@nobertgmuedu I took some time looking into this issue now. Specfically I looked at https://s3.amazonaws.com/kittigmu/kitti_2011_10_03_drive_0027_synced.bag and I can repro your quality issues. Some observations:

• The car in the bag starts out driving - this violates Cartographer's zero velocity initial state and therefore it takes a while until the SLAM starts tracking.
• The original IMU in the car had 100 Hz frequency, the Kitti dataset only contains "synchronized" data to the velodyne (full revolutions) which reduces this to 10 Hz. That is not enough to capture corners and strong accelerations, so the SLAM drifts whenever the car de- or accelerates or turns corners. This dataset would work better with pure point cloud matching, but Cartographer does not do that - we always use IMU in 3d. Full revolutions are also not ideal, though Andreas invested some time to "unwarp" the data using imu interpolation. The closest to this is enabling the real_time_correlative_scan_matcher which makes Cartographer quite slow.
• Running Cartographer, you see that the car is constantly dropping, after a while it is at z=-100m. This indicates that either the URDF is wrong and there is an angle between the IMU and the lidar or that the IMU has a bias.

I think LOAM would do much better on the data, Cartographer really requires a fast IMU. If that is given, it can do quite well on this kind of setup - for example #259. Sorry I cannot help much more here.

[ikhann]

1. Change the hybrid grid template parameters to allow for larger maps in the source code. This will require more memory and cost performance.

Hi,

Could you help how to change the hybrid grid template parameters?