Overlapping while mapping (Even with good odometry)

[RoboCrafty]

Required Info:

• Operating System:
  • Ubuntu 22.04
• Installation type:
  • Binaries
• ROS Version
  • ROS 2 Humble
• Version or commit hash:
  • Not sure?
• Laser unit:
  • RP Lidar A2

Steps to reproduce the issue

• Launch Robot base driver (Including Odom and IMU)
• Launch EKF Filter from Robot_localisation package to fuse odom with IMU (Otherwise there's quite a bit of drift after a while)
• Launch Online Async Mapping

A link to my rosbag. please note that I was recording raw odometry so there is no filtered odom in this rosbag. Not sure how helpful it'd be.

Here's what my filtered Odometry looks like though:

Only a drift of about 50 CM after traveling 240 ish meters

Expected behavior

• A good map with no overlaps

Actual behavior

• With scan matching on the map is horrible.
  

• Without scan matching and instead, just loop closure, it's much better. But still, there's an overlap while on the way back from the long straight path. The loop closure doesn't seem to make a difference. There no "jump" of the map to correct the small drifts to prevent the overlaps
  

Additional information

Here's the ekf yaml:

  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: true
    mode: mapping

    # 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: "/home/ros2_ws/src/tarkbot_robot/maps/roomV3_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.02 #if 0 never publishes odometry
    map_update_interval: 5.0
    resolution: 0.05
    max_laser_range: 12.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: false
    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: 6.0
    loop_search_maximum_distance: 20.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: 0.5   

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.9  
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

I have already tried playing around with params relating to loop closure and scan matching but saw no success so far. Since loop closure doesnt actually make a difference here im starting to think it could be due to the fact that the drift is very small so loop closure ignores that? Which param exactly tells loop closure to loop for even the smallest differences and try to correct it? if that makes sense.

[RoboCrafty]

Hey @SteveMacenski Could you please have a look once you're free? Would appreciate any suggestions. Thank you!

[RoboCrafty]

Anyone else would like to shed some light? Seems Steve's busy. I need to get my mapping right very soon since I'm running outta rime :(

[oenzan]

I believe if you decrease correlation_search_space_dimension, scanMatcher pose correction much closer to your local odom.

[RoboCrafty]

I believe if you decrease correlation_search_space_dimension, scanMatcher pose correction much closer to your local odom.

Thanks for the response.
I tried changing that without success. By any chance would you be free to try out the rosbag?

[oenzan]

    
    minimum_travel_distance: 0.36
    minimum_travel_heading: 0.8 
    
    scan_buffer_size: 10

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.16
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.01
    
    # Scan Matcher Parameters
    distance_variance_penalty: 0.11
    angle_variance_penalty: 0.37
    
    fine_search_angle_offset: 0.00649
    coarse_search_angle_offset: 0.65
    coarse_angle_resolution: 0.0649
    minimum_distance_penalty: 0.4
    minimum_angle_penalty: 0.41
    use_response_expansion: true

i used it as a configuration of scanMatch. Maybe it helps.

[RoboCrafty]

    
    minimum_travel_distance: 0.36
    minimum_travel_heading: 0.8 
    
    scan_buffer_size: 10

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.16
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.01
    
    # Scan Matcher Parameters
    distance_variance_penalty: 0.11
    angle_variance_penalty: 0.37
    
    fine_search_angle_offset: 0.00649
    coarse_search_angle_offset: 0.65
    coarse_angle_resolution: 0.0649
    minimum_distance_penalty: 0.4
    minimum_angle_penalty: 0.41
    use_response_expansion: true

i used it as a configuration of scanMatch. Maybe it helps.

Nope unfortunately for me they made it worse.
Here's how it looked with your params:

And after a bunch more tuning I was able to make it much better but its still not perfect. Seems like loop closure is still not working. There are still 2 main problems with it.

1. Loop closure isnt working so theres a slight drift in the map and some walls overlap (The one near the elevator, the boxy shape)
2. The map is tilted for some reason.

But here's how it looks

My current params:

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: true
    mode: mapping

    # 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: "/home/ros2_ws/src/tarkbot_robot/maps/roomV3_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.02 #if 0 never publishes odometry
    map_update_interval: 5.0
    resolution: 0.03
    max_laser_range: 12.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    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.1
    minimum_travel_heading: 0.5
    scan_buffer_size: 70
    scan_buffer_maximum_scan_distance: 10.0 #no
    link_match_minimum_response_fine: 0.1
    link_scan_maximum_distance: 0.75
    loop_search_maximum_distance: 2.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.3   
    angle_variance_penalty: 0.1

    fine_search_angle_offset: 0.00849     
    coarse_search_angle_offset: 0.849  
    coarse_angle_resolution: 0.0849        
    minimum_angle_penalty: 0.1
    minimum_distance_penalty: 0.1
    use_response_expansion: true

[deepak-anzo]

I am facing the same issue. Map quality is very bad bad seems the loop clouser is not working.
I am using parameter -

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_link
scan_topic: /scan
mode: mapping

# 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: /home/dev/dev_ws/my_map_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.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.25
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.25
minimum_travel_heading: 0.25
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 20.0
link_match_minimum_response_fine: 0.1  
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 50.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: true

The map I Get.

Please help to fine tune the mapping.

[RoboCrafty]

Yeah thats bad. I have no idea how to tune that. Steve would know, we are both kinda lost here. hows your odometry?

[deepak-anzo]

My Odometry is good enough. I am ot using any IMU though. Does your robot have an IMU? Is it necessary to use sensor fusion with IMU?

[RoboCrafty]

How good? is it better than mine? Yes I do have an IMU. My odometry without fusing is quite bad.

Is it necessary to use sensor fusion with IMU?

How else do you want to use sensor fusion?

[deepak-anzo]

Currently, I am not using any sensor fusion, I just use encoder data only. I find the robot odometry is good, it shows very less drift or error. Though I want to use a good quality industrial IMU. Which IMU you use for your robot?

[RoboCrafty]

lol, MPU6050 haha. My fused odometry is very good so i think this is fine.

[oenzan]

#609 (comment)

According to that did you check timestamps? As i understand, slamtoolbox accepts scanner postion as a closest time stamp to your scanner's time stamp.

slam_toolbox/include/slam_toolbox/get_pose_helper.hpp

Line 42 in 94cec98

bool getOdomPose(karto::Pose2 & karto_pose, const rclcpp::Time & t)

[RoboCrafty]

Thank you. How exactly do i do that?

[oenzan]

You can check if odom -> base_link tf timestamp's are periodic like the predefined value.

[RoboCrafty]

Which predefined value? I can run the tf tree to see the average rate and stuff. Is that what you're referring to?

[oenzan]

transforms:
- header:
    stamp:
      sec: 1710748383
      nanosec: 218937345
    frame_id: odom
  child_frame_id: base_footprint
  transform:
    translation:
      x: 9.809736556356288
      y: -16.149839693885067
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.5729942508840101
      w: 0.8195593867767437
---
transforms:
- header:
    stamp:
      sec: 1710748383
      nanosec: 277539038
    frame_id: odom
  child_frame_id: base_footprint
  transform:
    translation:
      x: 9.809747665544675
      y: -16.14987965289475
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.5730028894809985
      w: 0.8195533470411958
---

In the example above, it should be 30 ms, but sometimes it is 60 ms. As I understand, it has the potential to cause map corruption. Also check if the timestamp of your scanner topic matches the timestamp of tf.

[RoboCrafty]

Thank you @oenzan but it turns out it was the lidar! All these months I was in the delusion that I was using a 12m lidar but I recently learnt I was not! I never bothered checking the range of my lidar because this thought never crossed my mind. Checking the datasheet of RPLIDAR A2M8 shows it is a 12m lidar, but there are actually a couple variations of this under the same model, turns out the one I was given by my professor was a 6m one. In reality its only like 4.5 - 5 meters. Switched to A2M12 and it was all gone. The map was perffffeecttttt with the default values hahah.
Here it is: