Cartographer: map->odom tf is unstable when using both scans and pointcloud as input
Hi. I am using Cartographer/Cartographer_ROS with ROS eloquent. I am struggling with the transform from map-> odom, which are computed by cartographer. I am using 2 lidar scans and 1 3D camera.
The tests performed are with the robot standing still in the map (no moving).
When using only 2 lidar scans (num_laser_scans = 2, num_point_clouds = 0), the map->odom transform is stable, with just some very small noise which not affects the map.
The transform do look like this:
At time 1324.100000000
- Translation: [1.944, 0.436, -0.311]
- Rotation: in Quaternion [0.000, 0.001, 0.003, 1.0]
When using only the pointcloud as input ( num_point_clouds = 1, num_laser_scans = 0), the transform does still look good and stable, and pretty much similar to the previous one.
At time 1324.100000000
- Translation: [1.965, 0.480, -0.311]
- Rotation: in Quaternion [0.000, 0.001, 0.003, 1.0]
But when I try to combine the lidars and the camera (num_point_clouds = 1, num_laser_scans = 2), the transform is jumping around, and the orientation is drifting, making the map look bad (looks like two maps on top of each other, with a rotation between).
Two following transforms look like this:
At time 1306.50000000
- Translation: [2.062, 0.039, -0.336]
- Rotation: in Quaternion [-0.017, 0.003, -0.125, 0.992]
At time 1306.160000000
- Translation: [2.016, 0.630, -0.312]
- Rotation: in Quaternion [-0.000, 0.001, 0.006, 1.000]
Which ofcourse make the map rotate based. Especially the y-coordinates are changing much, and also the z in the quaternions.
Can someone please help me figure out why this is happening? Thankyou!!
Here is the rest of my lua file:
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "doom",
odom_frame = "doom",
provide_odom_frame = false,
publish_frame_projected_to_2d = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 2,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 1,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 25
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 25.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
return options