Slam Toolbox: Huge Drift in Heading when Mapping with a Depth Camera

edit

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