Posegraph map explode when use in localization mode ROS2 HUMBLE

[jarunyawat]

System Information: ROS 2 Humble SLAM Toolbox version: 2.6.8 from binaries OS version: Ubuntu 22.04

I tried to use a posegraph map that I generated using SLAM in a large warehouse, which is not a closed area. The size of the map is approximately 80x80 square meters. I used the PGM format to create the world for a Gazebo simulation. Initially, I was able to load the pose graph map, and when I teleoperated the robot, it could localize itself on the map and move for about 10 meters. However, after that, the map appears to "explode" or become distorted. Could you guide me on what might be causing this? Below are my localization settings.

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
    use_map_saver: false
    mode: localization #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: map_20_serial
    #map_start_pose: [0.0, 0.0, 0.0]
    map_start_at_dock: true

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.1 #if 0 never publishes odometry
    map_update_interval: 5.0
    resolution: 0.05
    min_laser_range: 0.0 #for rastering images
    max_laser_range: 20.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 1.0
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: false

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.1
    minimum_travel_heading: 0.2
    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      
    angle_variance_penalty: 1.0    

    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: false

Before explode Moving for 10 meters and then explode

[SteveMacenski]

Localization mode is not an appropriate solution for all applications. It is not a generic localizer that you can / should expect to work well for all applications and robot vendors. My suggestion is to use more standard localization solutions like AMCL instead 🙂 it fills a specific niche I had and others have had, but not as general-purpose of the SLAM itself. What likely happened here is you had some bad data get injected or some major odometry drift that had an incorrect match for your tuning and the backend graph optimization didn’t take that well.

If you want to try to make this work, I’d suggest setting the loss function to Huber and adjusting the correlation search grid ranges a bit.