Slam Toolbox: Huge Drift in Heading when Mapping with a Depth Camera
I am using Ros2 slam_toolbox for mapping a room.
Here is what I am doing: 1) Run the Gazebo simulation with Depth Camera plug in. 2) Convert Pointcload2 to LaserScan using the ros2 pointcloud_to_laserscan package. [Note: the range of the cameara is -0.6 to 0.6 radians and I have set the pointclould_to_laserscan package to scan from -0.5 to 0.5 radians] Is this range to small for slam_toolbox to work properly?] 3) Use slam_toolbox to generate a map.
The problem is a huge drift between the "map" and "odom" frames, especially in the heading angle.
See this picture: Drift
How can I get rid of this drift?
Can I make slam_toolbox to give more weight to odometery rather than the laser scan? [Note: I have turned off use_scan_matching and use_scan_barycenter but that has NO effect!] [Note: I have lowered distance_variance_penalty and angle_variance_penalty with NO effect.]
Here are the setting for slam_toolbox and pointclound_to_laserscan:
slam_toolbox:
ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
# map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.05 # 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5 # 0.00000000005
angle_variance_penalty: 1.0 # 0.0000000001
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
and
pointcloud_to_laserscan:
ros__parameters:
angle_increment: 0.0019
angle_max: 0.5
angle_min: -0.5
inf_epsilon: 1.0
max_height: 0.7
min_height: -1.3
queue_size: 20
range_max: 20.0
range_min: 0.1
scan_time: 0.05
target_frame: camera_link
transform_tolerance: 0.01
use_inf: true
use_sim_time: true