Loop closure repeatbility

[67bug]

I am trying out slam_toolbox for mapping and am running into issues with loop closure. 

• OS: Ubuntu 18.04
• ROS: Melodic
• slam-toolbox: 1.1.6-1bionic.20221025.212553
• Lidars: 2 YDLidar TG15s overlapped in their Fielf of view, but each has about a 220-degree FOV, combined with laser matcher
• IMU: yes
• Drive: Differential
• Ceres: Local source install
• Site: a 100m long x 20m "room". There are baggage belts that reduce the ability to "see" the whole room from time to time, but this was addressed with intended loop closures. Lots of reflective walls

I am running offline.launch:

<launch>

  <arg name="scan_topic_out" default="/scan_merged"/>
  <arg name="destination_frame" default="lidar"/>
  <arg name="scan_topics_in" default="/scan_1 /scan_2"/>

  <param name="/use_sim_time" value="true" />

   <node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" output="screen">
            <param name="destination_frame" value="$(arg destination_frame)"/>
            <param name="scan_destination_topic" value="$(arg scan_topic_out)"/>
            <param name="laserscan_topics" value ="$(arg scan_topics_in)" />
            <param name="angle_min" value="-3.14159"/>
            <param name="angle_max" value="3.14159"/>
            <param name="angle_increment" value="0.00311202835292"/>
            <param name="scan_time" value="0.1"/>
            <param name="range_min" value="0.05"/>
            <param name="range_max" value="50"/>
    </node>

  <node pkg="slam_toolbox" type="sync_slam_toolbox_node" name="slam_toolbox" output="screen">
    <rosparam command="load" file="<path>/mapper_params_offline.yaml" />
  </node>

</launch>

The offline yaml file looks like such:

# offline mapping
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT #DOGLEG # changing per https://github.com/SteveMacenski/slam_toolbox/issues/198#issuecomment-624895645
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None # HuberLoss # changing per https://github.com/SteveMacenski/slam_toolbox/issues/198#issuecomment-624895645

# ROS Parameters
odom_frame: bbv/odom
map_frame: map
base_frame: base_link
scan_topic: /scan_merged
mode: mapping #localization
debug_logging: true
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry, default 0.02
map_update_interval: 10.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 14400.
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 # default 10
scan_buffer_maximum_scan_distance: 10 # default 10
link_match_minimum_response_fine: 0.1  
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0 # default 3
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

Here is where things get interesting. When i ran this the first time (last night) when I was at the site, a perfect map was generated and i saw a whole bunch of loop closures

Today, i tried re-running (as I was debugging something else in my side of the code) and i see no loop closures happening -- same bag file, same settings. And obviously the map is not good at all. What could be causing this?

Bag file link odom is the output of an ekf and is the real odom scan_1 is the right lidar scan_2 is the left lidar

Terminal output for the good runs

Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://172.17.0.1:35967/

SUMMARY
========

PARAMETERS
 * /laserscan_multi_merger/angle_increment: 0.00311202835292
 * /laserscan_multi_merger/angle_max: 3.14159
 * /laserscan_multi_merger/angle_min: -3.14159
 * /laserscan_multi_merger/destination_frame: lidar
 * /laserscan_multi_merger/laserscan_topics: /scan_1 /scan_2
 * /laserscan_multi_merger/range_max: 50
 * /laserscan_multi_merger/range_min: 0.05
 * /laserscan_multi_merger/scan_destination_topic: /scan_merged
 * /laserscan_multi_merger/scan_time: 0.1
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /slam_toolbox/angle_variance_penalty: 1.0
 * /slam_toolbox/base_frame: base_link
 * /slam_toolbox/ceres_dogleg_type: TRADITIONAL_DOGLEG
 * /slam_toolbox/ceres_linear_solver: SPARSE_NORMAL_CHO...
 * /slam_toolbox/ceres_loss_function: None
 * /slam_toolbox/ceres_preconditioner: SCHUR_JACOBI
 * /slam_toolbox/ceres_trust_strategy: LEVENBERG_MARQUARDT
 * /slam_toolbox/coarse_angle_resolution: 0.0349
 * /slam_toolbox/coarse_search_angle_offset: 0.349
 * /slam_toolbox/correlation_search_space_dimension: 0.5
 * /slam_toolbox/correlation_search_space_resolution: 0.01
 * /slam_toolbox/correlation_search_space_smear_deviation: 0.1
 * /slam_toolbox/debug_logging: True
 * /slam_toolbox/distance_variance_penalty: 0.5
 * /slam_toolbox/do_loop_closing: True
 * /slam_toolbox/enable_interactive_mode: True
 * /slam_toolbox/fine_search_angle_offset: 0.00349
 * /slam_toolbox/link_match_minimum_response_fine: 0.1
 * /slam_toolbox/link_scan_maximum_distance: 1.5
 * /slam_toolbox/loop_match_maximum_variance_coarse: 3.0
 * /slam_toolbox/loop_match_minimum_chain_size: 10
 * /slam_toolbox/loop_match_minimum_response_coarse: 0.35
 * /slam_toolbox/loop_match_minimum_response_fine: 0.45
 * /slam_toolbox/loop_search_maximum_distance: 3.0
 * /slam_toolbox/loop_search_space_dimension: 8.0
 * /slam_toolbox/loop_search_space_resolution: 0.05
 * /slam_toolbox/loop_search_space_smear_deviation: 0.03
 * /slam_toolbox/map_frame: map
 * /slam_toolbox/map_update_interval: 10.0
 * /slam_toolbox/max_laser_range: 20.0
 * /slam_toolbox/minimum_angle_penalty: 0.9
 * /slam_toolbox/minimum_distance_penalty: 0.5
 * /slam_toolbox/minimum_time_interval: 0.5
 * /slam_toolbox/minimum_travel_distance: 0.5
 * /slam_toolbox/minimum_travel_heading: 0.5
 * /slam_toolbox/mode: mapping
 * /slam_toolbox/odom_frame: bbv/odom
 * /slam_toolbox/resolution: 0.05
 * /slam_toolbox/scan_buffer_maximum_scan_distance: 10
 * /slam_toolbox/scan_buffer_size: 10
 * /slam_toolbox/scan_topic: /scan_merged
 * /slam_toolbox/solver_plugin: solver_plugins::C...
 * /slam_toolbox/stack_size_to_use: 40000000
 * /slam_toolbox/tf_buffer_duration: 14400.0
 * /slam_toolbox/throttle_scans: 1
 * /slam_toolbox/transform_publish_period: 0.02
 * /slam_toolbox/transform_timeout: 0.2
 * /slam_toolbox/use_response_expansion: True
 * /slam_toolbox/use_scan_barycenter: True
 * /slam_toolbox/use_scan_matching: True
 * /use_sim_time: True

NODES
  /
    laserscan_multi_merger (ira_laser_tools/laserscan_multi_merger)
    slam_toolbox (slam_toolbox/sync_slam_toolbox_node)

ROS_MASTER_URI=http://172.17.0.1:11311

process[laserscan_multi_merger-1]: started with pid [18052]
process[slam_toolbox-2]: started with pid [18053]
[ INFO] [1686211694.352617151]: Waiting for topics ...
[ INFO] [1686211694.362451476]: Node using stack size 40000000
[DEBUG] [1686211694.411993999]: Attempting to create managed instance for class solver_plugins::CeresSolver.
[DEBUG] [1686211694.412053244]: Class solver_plugins::CeresSolver maps to library libceres_solver_plugin in classes_available_.
[DEBUG] [1686211694.412274643]: Iterating through all possible paths where libceres_solver_plugin could be located...
[DEBUG] [1686211694.412292657]: Checking path /home/rajeev/catkin_ws_DFWNov2022/devel/lib/libceres_solver_plugin.so 
[DEBUG] [1686211694.412313821]: Checking path /home/rajeev/catkin_ws_DFWNov2022/devel/lib/libceres_solver_plugin.so 
[DEBUG] [1686211694.412336819]: Checking path /opt/ros/melodic/lib/libceres_solver_plugin.so 
[DEBUG] [1686211694.412407121]: Library libceres_solver_plugin found at explicit path /opt/ros/melodic/lib/libceres_solver_plugin.so.
[DEBUG] [1686211694.416978589]: solver_plugins::CeresSolver maps to real class type solver_plugins::CeresSolver
[ INFO] [1686211694.418884519]: CeresSolver: Using SCHUR_JACOBI preconditioner.
[DEBUG] [1686211694.418945591]: boost::shared_ptr to object of real type solver_plugins::CeresSolver created.
[ INFO] [1686211694.418977230]: Using plugin solver_plugins::CeresSolver
[ INFO] [1686211695.353306029]: Waiting for topics ...
[ INFO] [1686211696.354023270]: Waiting for topics ...
[ INFO] [1686211697.354908837]: Waiting for topics ...
[ INFO] [1686211698.355378123]: Waiting for topics ...
[ INFO] [1686211699.356115909]: Waiting for topics ...
[ INFO] [1686211700.356778178]: Waiting for topics ...
[ INFO] [1686211701.358452373]: Waiting for topics ...
[ INFO] [1686211702.359600320]: Waiting for topics ...
[ INFO] [1686211703.360183183]: Waiting for topics ...
[ INFO] [1686211704.360905115]: Waiting for topics ...
[ INFO] [1686211705.362307309]: Waiting for topics ...
[ INFO] [1686211706.363820388]: Waiting for topics ...
[ INFO] [1686211707.364540768]: Waiting for topics ...
[ INFO] [1686211708.365056478]: Waiting for topics ...
[ INFO] [1686211709.365735830]: Waiting for topics ...
[ INFO] [1686211710.366335649]: Waiting for topics ...
[ INFO] [1686211711.367467138]: Waiting for topics ...
[ INFO] [1686211712.368358147]: Waiting for topics ...
[ INFO] [1686211713.369391495]: Waiting for topics ...
[ INFO] [1686211714.369962903, 1686208753.917664990]: Waiting for topics ...
[ INFO] [1686211715.370606705, 1686208754.417589967]: Subscribing to topics 2
[DEBUG] [1686211715.809409968, 1686208754.634546282]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208754.544, count now 0
[DEBUG] [1686211715.809758067, 1686208754.634546282]: laser lidar's pose wrt base: 0.000 0.000 0.042 0.000
Registering sensor: [Custom Described Lidar]
[DEBUG] [1686211716.007585831, 1686208754.735570060]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208754.645, count now 0

<More of the same as the graph size increases>

[DEBUG] [1686212014.329572964, 1686208903.894825340]: Graph size: 120
[DEBUG] [1686212014.478906839, 1686208903.968222353]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208903.879, count now 0
[DEBUG] [1686212014.672038453, 1686208904.069258567]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208903.977, count now 0
[DEBUG] [1686212014.869053828, 1686208904.165269490]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.076, count now 0
[DEBUG] [1686212015.064126016, 1686208904.261343746]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.175, count now 0
[DEBUG] [1686212015.262882461, 1686208904.362445045]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.273, count now 0
[DEBUG] [1686212015.483098822, 1686208904.473563192]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.372, count now 0
[DEBUG] [1686212015.655329806, 1686208904.559380902]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.470, count now 0
[DEBUG] [1686212015.857590294, 1686208904.660578683]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.569, count now 0
[DEBUG] [1686212016.054715655, 1686208904.756687932]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.668, count now 0
[DEBUG] [1686212016.249746403, 1686208904.857637520]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.767, count now 0
[DEBUG] [1686212016.445788759, 1686208904.953658794]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.865, count now 0
[DEBUG] [1686212016.644435815, 1686208905.054382298]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208904.963, count now 0
[DEBUG] [1686212016.842430252, 1686208905.150158599]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.062, count now 0
[DEBUG] [1686212017.035112268, 1686208905.246134920]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.160, count now 0
[DEBUG] [1686212017.232914501, 1686208905.347279070]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.259, count now 0
[DEBUG] [1686212017.432771342, 1686208905.448322391]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.357, count now 0
[DEBUG] [1686212017.630939933, 1686208905.544461849]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.456, count now 0
[DEBUG] [1686212017.826197067, 1686208905.645504247]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.555, count now 0
[DEBUG] [1686212018.021546130, 1686208905.741274910]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.653, count now 0
[DEBUG] [1686212018.229734107, 1686208905.847306513]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.752, count now 0
[DEBUG] [1686212018.426987828, 1686208905.943295577]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.854, count now 0
[DEBUG] [1686212018.619943758, 1686208906.038926928]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208905.948, count now 0
[DEBUG] [1686212018.807486494, 1686208906.134991911]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.047, count now 0
[DEBUG] [1686212019.005386105, 1686208906.236024734]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.145, count now 0
[DEBUG] [1686212019.203465719, 1686208906.331897815]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.244, count now 0
[DEBUG] [1686212019.399770433, 1686208906.428339754]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.343, count now 0
[DEBUG] [1686212019.600211390, 1686208906.529226148]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.441, count now 0
[DEBUG] [1686212019.802245355, 1686208906.630392170]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.540, count now 0
[DEBUG] [1686212019.994248085, 1686208906.726416614]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.640, count now 0
[DEBUG] [1686212020.198233723, 1686208906.832358609]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.737, count now 0
[DEBUG] [1686212020.392601220, 1686208906.928311691]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.836, count now 0
[DEBUG] [1686212020.582839021, 1686208907.024346273]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208906.933, count now 0
[DEBUG] [1686212020.783667049, 1686208907.120509319]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.032, count now 0
[DEBUG] [1686212020.983839119, 1686208907.221357450]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.133, count now 0
[DEBUG] [1686212021.182186289, 1686208907.322353563]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.231, count now 0
[DEBUG] [1686212021.378031868, 1686208907.418397994]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.330, count now 0
[DEBUG] [1686212021.574262134, 1686208907.519386341]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.429, count now 0
[DEBUG] [1686212021.773232060, 1686208907.615274875]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.527, count now 0
[DEBUG] [1686212021.970337541, 1686208907.716333987]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.626, count now 0
[DEBUG] [1686212022.166463313, 1686208907.812674603]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.724, count now 0
[DEBUG] [1686212022.361158486, 1686208907.913715626]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.823, count now 0
[DEBUG] [1686212022.558917267, 1686208908.009590952]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208907.921, count now 0
[DEBUG] [1686212022.755123143, 1686208908.110700110]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.020, count now 0
[DEBUG] [1686212022.953643930, 1686208908.206686572]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.119, count now 0
[DEBUG] [1686212023.150100432, 1686208908.307531011]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.217, count now 0
[DEBUG] [1686212023.350867079, 1686208908.408343198]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.316, count now 0
[DEBUG] [1686212023.545190424, 1686208908.504315957]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.415, count now 0
[DEBUG] [1686212023.744054140, 1686208908.605364052]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.513, count now 0
[DEBUG] [1686212023.946980018, 1686208908.706555608]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.612, count now 0
[DEBUG] [1686212024.136035145, 1686208908.797462552]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.711, count now 0
[DEBUG] [1686212024.334227876, 1686208908.898660122]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.809, count now 0
[DEBUG] [1686212024.534491062, 1686208908.999625338]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208908.908, count now 0
[DEBUG] [1686212024.773180143, 1686208909.115639763]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.007, count now 0
[DEBUG] [1686212024.959350607, 1686208909.211490579]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.112, count now 0
[DEBUG] [1686212025.157932286, 1686208909.307540992]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.203, count now 0
[DEBUG] [1686212025.349341056, 1686208909.403890087]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.302, count now 0
[DEBUG] [1686212025.543034618, 1686208909.504794338]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.400, count now 0
[DEBUG] [1686212025.745745439, 1686208909.606054949]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.498, count now 0
[DEBUG] [1686212025.933903358, 1686208909.697054642]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.597, count now 0
[DEBUG] [1686212026.128108133, 1686208909.793042230]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.695, count now 0
[DEBUG] [1686212026.332281465, 1686208909.899072283]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.793, count now 0
[DEBUG] [1686212026.526814125, 1686208909.995212580]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.892, count now 0
[DEBUG] [1686212026.716100769, 1686208910.091276460]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208909.990, count now 0
[DEBUG] [1686212026.909794053, 1686208910.187201422]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.088, count now 0
[DEBUG] [1686212027.104612026, 1686208910.283050686]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.187, count now 0
[DEBUG] [1686212027.306393018, 1686208910.384108254]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.285, count now 0
[DEBUG] [1686212027.494237088, 1686208910.479954827]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.383, count now 0
[DEBUG] [1686212027.689026479, 1686208910.575902578]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.483, count now 0
[DEBUG] [1686212027.885385856, 1686208910.671925168]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.580, count now 0
[DEBUG] [1686212028.081878465, 1686208910.772752708]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.678, count now 0
[DEBUG] [1686212028.278342337, 1686208910.868700987]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.777, count now 0
[DEBUG] [1686212028.471565881, 1686208910.964449842]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.875, count now 0
[DEBUG] [1686212028.670098431, 1686208911.065506269]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208910.974, count now 0
[DEBUG] [1686212028.863581699, 1686208911.161291571]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.072, count now 0
[DEBUG] [1686212029.066540766, 1686208911.262283384]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.171, count now 0
[DEBUG] [1686212029.253935916, 1686208911.358280139]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.269, count now 0
[DEBUG] [1686212029.466860695, 1686208911.464344164]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.368, count now 0
[DEBUG] [1686212029.658680673, 1686208911.560232828]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.466, count now 0
[DEBUG] [1686212029.846704466, 1686208911.656566177]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.565, count now 0
[DEBUG] [1686212030.059624424, 1686208911.762833741]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.664, count now 0
[DEBUG] [1686212030.239030069, 1686208911.848706157]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.767, count now 0
[DEBUG] [1686212030.435270355, 1686208911.949514431]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.860, count now 0
[DEBUG] [1686212030.638374230, 1686208912.050546155]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208911.958, count now 0
[DEBUG] [1686212030.837779720, 1686208912.150149592]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208912.057, count now 0
[DEBUG] [1686212031.028728168, 1686208912.245687433]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208912.156, count now 0
[DEBUG] [1686212031.166900366, 1686208912.316143440]: CeresSolver: Setting first node as a constant pose:0.00, -0.00, -0.02.

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

                                     Original                  Reduced
Parameter blocks                          381                      378
Parameters                                381                      378
Residual blocks                           128                      128
Residual                                  384                      384

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                    50                       50
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                      378

Cost:
Initial                          3.159827e+04
Final                            2.591161e+02
Change                           3.133916e+04

Minimizer iterations                        4
Successful steps                            4
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0008

  Residual evaluation                  0.0057
  Jacobian evaluation                  0.0207
  Linear solver                        0.0008
Minimizer                              0.0281

Postprocessor                          0.0007
Total                                  0.0296

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 1.642663e-04 <= 1.000000e-03.)

[DEBUG] [1686212031.220701907, 1686208912.341271782]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208912.255, count now 0

<More of the same as graph size increases>

[DEBUG] [1686212034.314017395, 1686208913.887647260]: Graph size: 128
[DEBUG] [1686212034.369650851, 1686208913.917946556]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208913.827, count now 0
[DEBUG] [1686212034.565619451, 1686208914.013948132]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208913.925, count now 0
[DEBUG] [1686212034.779481471, 1686208914.119747651]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.023, count now 0
[DEBUG] [1686212034.959081700, 1686208914.210633001]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.122, count now 0
[DEBUG] [1686212035.159626712, 1686208914.311536932]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.220, count now 0
[DEBUG] [1686212035.353645322, 1686208914.407506914]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.320, count now 0
[DEBUG] [1686212035.551729411, 1686208914.505856145]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.417, count now 0
[DEBUG] [1686212035.753885816, 1686208914.606828391]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.515, count now 0
[DEBUG] [1686212035.943201135, 1686208914.702789432]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.614, count now 0
[DEBUG] [1686212036.148970039, 1686208914.803749173]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.712, count now 0
[DEBUG] [1686212036.336739962, 1686208914.899934996]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.810, count now 0
[DEBUG] [1686212036.535374866, 1686208915.000956835]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208914.910, count now 0
[DEBUG] [1686212036.729252755, 1686208915.096896269]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.008, count now 0
[DEBUG] [1686212036.926057299, 1686208915.192796816]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.106, count now 0
[DEBUG] [1686212037.120515866, 1686208915.288661012]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.204, count now 0
[DEBUG] [1686212037.320858229, 1686208915.389695615]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.302, count now 0
[DEBUG] [1686212037.518238028, 1686208915.490665410]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.400, count now 0
[DEBUG] [1686212037.708027899, 1686208915.586800053]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.499, count now 0
[DEBUG] [1686212037.905309262, 1686208915.684141863]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.595, count now 0
[DEBUG] [1686212038.115085571, 1686208915.790212519]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.693, count now 0
[DEBUG] [1686212038.309474334, 1686208915.886119220]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.791, count now 0
[DEBUG] [1686212038.496529439, 1686208915.977130523]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.891, count now 0
[DEBUG] [1686212038.695241361, 1686208916.078107982]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208915.990, count now 0
[DEBUG] [1686212038.902949684, 1686208916.184220932]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.087, count now 0
[DEBUG] [1686212039.087287574, 1686208916.275199225]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.186, count now 0
[DEBUG] [1686212039.281318446, 1686208916.371009951]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.283, count now 0
[DEBUG] [1686212039.479456995, 1686208916.472238219]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.382, count now 0
[DEBUG] [1686212039.674780417, 1686208916.568146605]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.480, count now 0
[DEBUG] [1686212039.877735743, 1686208916.669071856]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.578, count now 0
[DEBUG] [1686212040.071516209, 1686208916.765052957]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.677, count now 0
[DEBUG] [1686212040.263474413, 1686208916.861173971]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.776, count now 0
[DEBUG] [1686212040.462337718, 1686208916.962462563]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.873, count now 0
[DEBUG] [1686212040.662451778, 1686208917.060395363]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208916.971, count now 0
[DEBUG] [1686212040.857675393, 1686208917.161587068]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.070, count now 0
[DEBUG] [1686212041.062703480, 1686208917.260298942]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.168, count now 0
[DEBUG] [1686212041.253974141, 1686208917.356119181]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.268, count now 0
[DEBUG] [1686212041.453898868, 1686208917.457219119]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.366, count now 0
[DEBUG] [1686212041.642083735, 1686208917.554029725]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.464, count now 0
[DEBUG] [1686212041.839545869, 1686208917.649922723]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.562, count now 0
[DEBUG] [1686212042.036547002, 1686208917.750945123]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.661, count now 0
[DEBUG] [1686212042.234419388, 1686208917.846905281]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.759, count now 0
[DEBUG] [1686212042.435665358, 1686208917.947754154]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.858, count now 0
[DEBUG] [1686212042.630745474, 1686208918.048718852]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208917.956, count now 0
[DEBUG] [1686212042.828799688, 1686208918.147142123]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208918.055, count now 0
[DEBUG] [1686212043.020013532, 1686208918.242781993]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208918.153, count now 0
[DEBUG] [1686212043.217062833, 1686208918.338341789]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208918.251, count now 0

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

                                     Original                  Reduced
Parameter blocks                          396                      393
Parameters                                396                      393
Residual blocks                           134                      134
Residual                                  402                      402

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                    50                       50
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                      393

Cost:
Initial                          6.091215e+02
Final                            2.931335e+02
Change                           3.159880e+02

Minimizer iterations                        2
Successful steps                            2
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0008

  Residual evaluation                  0.0022
  Jacobian evaluation                  0.0053
  Linear solver                        0.0004
Minimizer                              0.0087

Postprocessor                          0.0007
Total                                  0.0102

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 2.731502e-04 <= 1.000000e-03.)

[DEBUG] [1686212043.413900156, 1686208918.439043952]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208918.350, count now 0

<more of the same as the graph size increases>

[DEBUG] [1686212054.358729088, 1686208923.911787985]: Graph size: 135
[DEBUG] [1686212054.418771310, 1686208923.942253989]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208923.852, count now 0
[DEBUG] [1686212054.612741612, 1686208924.038060210]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208923.950, count now 0
[DEBUG] [1686212054.809788040, 1686208924.133640349]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208924.047, count now 0
[DEBUG] [1686212055.007246506, 1686208924.234150012]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208924.145, count now 0

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

                                     Original                  Reduced
Parameter blocks                          408                      405
Parameters                                408                      405
Residual blocks                           139                      139
Residual                                  417                      417

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                    50                       50
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                      405

Cost:
Initial                          2.963885e+02
Final                            2.933504e+02
Change                           3.038080e+00

Minimizer iterations                        2
Successful steps                            2
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0008

  Residual evaluation                  0.0021
  Jacobian evaluation                  0.0050
  Linear solver                        0.0004
Minimizer                              0.0084

Postprocessor                          0.0007
Total                                  0.0099

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 2.426644e-04 <= 1.000000e-03.)

[DEBUG] [1686212055.204528044, 1686208924.335070218]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208924.244, count now 0

<more of the same as the graph size increases>

[DEBUG] [1686212074.289252333, 1686208933.874387625]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208933.783, count now 0
[DEBUG] [1686212074.332948996, 1686208933.899610309]: Graph size: 142
[DEBUG] [1686212074.476596578, 1686208933.970334837]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208933.882, count now 0
[DEBUG] [1686212074.675611983, 1686208934.066402470]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208933.979, count now 0
[DEBUG] [1686212074.878138269, 1686208934.172276176]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.077, count now 0
[DEBUG] [1686212075.067476856, 1686208934.262988751]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.176, count now 0
[DEBUG] [1686212075.265164677, 1686208934.363925194]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.274, count now 0
[DEBUG] [1686212075.463012457, 1686208934.460008697]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.373, count now 0
[DEBUG] [1686212075.656866647, 1686208934.560956067]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.472, count now 0
[DEBUG] [1686212075.853400333, 1686208934.656584559]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.570, count now 0
[DEBUG] [1686212076.054819452, 1686208934.757600363]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.668, count now 0
[DEBUG] [1686212076.248604288, 1686208934.853618951]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.768, count now 0
[DEBUG] [1686212076.442425645, 1686208934.953925391]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.864, count now 0
[DEBUG] [1686212076.642233869, 1686208935.049774629]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208934.962, count now 0
[DEBUG] [1686212076.850297564, 1686208935.155919313]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.061, count now 0
[DEBUG] [1686212077.039913994, 1686208935.251789921]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.160, count now 0
[DEBUG] [1686212077.236726826, 1686208935.347708269]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.259, count now 0
[DEBUG] [1686212077.426894172, 1686208935.443534205]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.356, count now 0
[DEBUG] [1686212077.625421069, 1686208935.544660926]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.454, count now 0
[DEBUG] [1686212077.821107167, 1686208935.640903015]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.552, count now 0
[DEBUG] [1686212078.018230798, 1686208935.741892436]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.651, count now 0
[DEBUG] [1686212078.221025029, 1686208935.842599197]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.749, count now 0
[DEBUG] [1686212078.411656739, 1686208935.938327862]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.848, count now 0
[DEBUG] [1686212078.606346852, 1686208936.034221166]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208935.946, count now 0
[DEBUG] [1686212078.802414158, 1686208936.130038775]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.044, count now 0
[DEBUG] [1686212079.002179028, 1686208936.231040123]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.142, count now 0
[DEBUG] [1686212079.209007244, 1686208936.334522186]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.240, count now 0
[DEBUG] [1686212079.396647956, 1686208936.430182500]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.341, count now 0
[DEBUG] [1686212079.591104743, 1686208936.526124214]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.437, count now 0
[DEBUG] [1686212079.803519229, 1686208936.631947155]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.535, count now 0
[DEBUG] [1686212079.981988314, 1686208936.722725012]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.634, count now 0
[DEBUG] [1686212080.177442059, 1686208936.818665124]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.731, count now 0
[DEBUG] [1686212080.372591511, 1686208936.919603550]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.829, count now 0
[DEBUG] [1686212080.569391811, 1686208937.015444467]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208936.926, count now 0
[DEBUG] [1686212080.763565221, 1686208937.111715312]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.025, count now 0
[DEBUG] [1686212080.958244223, 1686208937.207560411]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.122, count now 0
[DEBUG] [1686212081.157793064, 1686208937.308508125]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.220, count now 0
[DEBUG] [1686212081.362018997, 1686208937.409510824]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.319, count now 0
[DEBUG] [1686212081.548197145, 1686208937.505530397]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.417, count now 0
[DEBUG] [1686212081.747712589, 1686208937.606388463]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.515, count now 0
[DEBUG] [1686212081.944386729, 1686208937.702363104]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.615, count now 0
[DEBUG] [1686212082.139958374, 1686208937.803111632]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.712, count now 0
[DEBUG] [1686212082.334008540, 1686208937.898331396]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.810, count now 0
[DEBUG] [1686212082.531153114, 1686208937.997771380]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208937.908, count now 0

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

                                     Original                  Reduced
Parameter blocks                          435                      432
Parameters                                435                      432
Residual blocks                           149                      149
Residual                                  447                      447

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                    50                       50
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                      432

Cost:
Initial                          1.210024e+03
Final                            3.622110e+02
Change                           8.478129e+02

Minimizer iterations                        2
Successful steps                            2
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0009

  Residual evaluation                  0.0023
  Jacobian evaluation                  0.0060
  Linear solver                        0.0005
Minimizer                              0.0097

Postprocessor                          0.0009
Total                                  0.0115

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 5.110768e-04 <= 1.000000e-03.)

[DEBUG] [1686212082.729402364, 1686208938.097797955]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686208938.007, count now 0

<more of the same as the graph size increases>

[DEBUG] [1686212394.686238695, 1686209094.076300223]: Graph size: 274
[DEBUG] [1686212394.839239142, 1686209094.152031799]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.061, count now 0
[DEBUG] [1686212395.032839073, 1686209094.247896352]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.159, count now 0
[DEBUG] [1686212395.232618339, 1686209094.348998429]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.257, count now 0
[DEBUG] [1686212395.430092147, 1686209094.445082004]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.356, count now 0
[DEBUG] [1686212395.625704239, 1686209094.546055062]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.455, count now 0
[DEBUG] [1686212395.821757489, 1686209094.641841858]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.553, count now 0
[DEBUG] [1686212396.016544833, 1686209094.737651721]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.651, count now 0
[DEBUG] [1686212396.224653119, 1686209094.844048568]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.750, count now 0
[DEBUG] [1686212396.411655731, 1686209094.934624253]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.848, count now 0
[DEBUG] [1686212396.607066840, 1686209095.035702651]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209094.946, count now 0
[DEBUG] [1686212396.803235967, 1686209095.131470522]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.045, count now 0
[DEBUG] [1686212397.002856488, 1686209095.232522379]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.143, count now 0
[DEBUG] [1686212397.196518977, 1686209095.328618022]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.242, count now 0
[DEBUG] [1686212397.393146768, 1686209095.429712856]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.340, count now 0
[DEBUG] [1686212397.598286987, 1686209095.530479401]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.438, count now 0
[DEBUG] [1686212397.790664612, 1686209095.626174914]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.538, count now 0
[DEBUG] [1686212397.984043175, 1686209095.721813586]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.636, count now 0
[DEBUG] [1686212398.178837902, 1686209095.822589637]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.733, count now 0
[DEBUG] [1686212398.380123100, 1686209095.923318972]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.831, count now 0
[DEBUG] [1686212398.571272949, 1686209096.014294178]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209095.929, count now 0
[DEBUG] [1686212398.768329349, 1686209096.114856089]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.027, count now 0
[DEBUG] [1686212398.966169930, 1686209096.215777184]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.125, count now 0
[DEBUG] [1686212399.162806135, 1686209096.311674246]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.223, count now 0
[DEBUG] [1686212399.367490268, 1686209096.412585284]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.322, count now 0
[DEBUG] [1686212399.558251342, 1686209096.508592245]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.424, count now 0
[DEBUG] [1686212399.765855703, 1686209096.614536772]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.519, count now 0
[DEBUG] [1686212399.949010758, 1686209096.705385912]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.617, count now 0
[DEBUG] [1686212400.145209928, 1686209096.801309348]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.715, count now 0
[DEBUG] [1686212400.352398578, 1686209096.907269602]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.814, count now 0
[DEBUG] [1686212400.541131168, 1686209097.003181476]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209096.912, count now 0
[DEBUG] [1686212400.738357757, 1686209097.099233917]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.011, count now 0
[DEBUG] [1686212400.930580230, 1686209097.198102685]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.109, count now 0
[DEBUG] [1686212401.140677174, 1686209097.303660642]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.207, count now 0
[DEBUG] [1686212401.327021559, 1686209097.394677994]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.306, count now 0
[DEBUG] [1686212401.521749727, 1686209097.490646905]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.404, count now 0
[DEBUG] [1686212401.718039179, 1686209097.591637544]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.502, count now 0
[DEBUG] [1686212401.920560168, 1686209097.692584239]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.601, count now 0
[DEBUG] [1686212402.113496897, 1686209097.788493473]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.700, count now 0
[DEBUG] [1686212402.311401409, 1686209097.884287305]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.797, count now 0
[DEBUG] [1686212402.505665066, 1686209097.985159265]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.896, count now 0
[DEBUG] [1686212402.703246423, 1686209098.081139287]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209097.994, count now 0
[DEBUG] [1686212402.902021538, 1686209098.182042875]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.092, count now 0
[DEBUG] [1686212403.096095185, 1686209098.277682133]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.191, count now 0
[DEBUG] [1686212403.295922435, 1686209098.380302269]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.290, count now 0
[DEBUG] [1686212403.488458717, 1686209098.475827997]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.388, count now 0

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

                                     Original                  Reduced
Parameter blocks                          834                      831
Parameters                                834                      831
Residual blocks                           283                      283
Residual                                  849                      849

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                    50                       50
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                      831

Cost:
Initial                          4.104806e+04
Final                            1.133135e+03
Change                           3.991493e+04

Minimizer iterations                        4
Successful steps                            4
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0017

  Residual evaluation                  0.0081
  Jacobian evaluation                  0.0198
  Linear solver                        0.0015
Minimizer                              0.0312

Postprocessor                          0.0014
Total                                  0.0344

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 5.405248e-04 <= 1.000000e-03.)

[DEBUG] [1686212403.690646041, 1686209098.576827052]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.485, count now 0
[DEBUG] [1686212403.886857178, 1686209098.672579104]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.585, count now 0
[DEBUG] [1686212404.082543589, 1686209098.773478418]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.684, count now 0
[DEBUG] [1686212404.279028593, 1686209098.869275208]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.782, count now 0
[DEBUG] [1686212404.476474176, 1686209098.970394927]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.881, count now 0
[DEBUG] [1686212404.677041920, 1686209099.071367519]: MessageFilter [target=bbv/odom ]: Added message in frame lidar at time 1686209098.979, count now 0
[DEBUG] [1686212404.878615948, 16862090

<more of the same as the graph size increases>

So, i know that loop closure is happening the the result is evident in the map generated. In every other run since, i see zero loop closures.

What would cause this?

[SteveMacenski]

What is Waiting for topics ...? I don't see that in the codebase.

Today, i tried re-running (as I was debugging something else in my side of the code) and i see no loop closures happening -- same bag file, same settings. And obviously the map is not good at all. What could be causing this?

Was the difference that you were continuing a session or using a bag file vs running on live data? I don't know why you would see an entire run that would not generate loop closures when it did beforehand. I'm trying to understand if anything about your situation is different.

"My map isn't exactly the same" I could understand, but just "no loop closures happen... ever" is really odd. But everytime you see those Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp) Ceres prints, that is a loop closure process occurring. Ceres is only called when we've found a loop, set the constraint, and ask Ceres to reoptimize. So if you see that, loop closure is happening, but if your map isn't updating to reflect that, that's odd and makes me think something's up with Ceres or the use of Ceres.

So perhaps your custom install of Ceres could be to blame if it has a bug or for some reason isn't compatible?

[67bug]

Thanks for the note, @SteveMacenski

Forgive me for the lack of clarity. The Waiting for Topics is a mesage from the laserscan_multi_merger node that is combining scan_1 and scan_2 to generate scan_merged

Was the difference that you were continuing a session or using a bag file vs running on live data? I don't know why you would see an entire run that would not generate loop closures when it did beforehand. I'm trying to understand if anything about your situation is different.

I run like such:

1. Generate a bag file with scans and odometry and save it with rosbag record
2. After the fact, i run offline.launch which allows me to play with different parameters for loop closure (with the intent of understanding how these impact the map)
3. Rinse and repeat with different settings in the mappers_params_offline.yaml

It is indeed odd that I had loop closures in one run and did not see any in any run since.

Might I ask this: When running offline, if the processing queue gets large, and of course keeps increasing because the CPU is unable to keep up, will all the messages in the queue eventually get processed? Or will the processing stop when the rosbag play stops? Why do i ask this?

When i run rosbag play without any rate control, the processing queue gets filled and slam_toolbox lags the rosbag play (this is expected). But what i observe is that the bag finishes running and slam_toolbox also stops updating the map which caught me by surprise (may be my observation is not accurate, so wanted to get your view on this)

Hence, I started using the rate option --rate 0.5 which allow the cpu to catch up to publishes. What i am unsure of is if this rate factor is dropping messages that cause the "close enough" pose to get published to ensure that the loop closure is triggered. I will experiment with this.

Now to address the question listed above about playing with rate control and slam_toolbox stopping processing, I will try using the --queue option for rosbag play and the --keep-alive option.

Thoughts?

So perhaps your custom install of Ceres could be to blame if it has a bug or for some reason isn't compatible?

Noted. How would i go about exploring this? As in where do i go hunting?

All this said, this is a great toolbox and FAR more intuitive to use than Cartographer. So, thank you!

[SteveMacenski]

When running offline, if the processing queue gets large, and of course keeps increasing because the CPU is unable to keep up, will all the messages in the queue eventually get processed?

Yes, we store the queue and it gets to them as they can. But I will be honest in saying that I see performance degradation of the map quality if you just throw everything into the system all at once and let it process at max rate.

Part of the reason that I know of is TF2 related. TF2 only keeps so much of the time in its buffers. So If you don't have TF up and providing the relevant data at the relevant times, then obviously things start to degrade by no fault of SLAM Toolbox. Some of the data is just no longer available or might try to use the most recent possible data which could be waaay into the future relative to that scan.

But what i observe is that the bag finishes running and slam_toolbox also stops updating the map which caught me by surprise

I don't recall that happening from my personal experience, but there might be a rosbag setting you're missing to not bring things down once complete. There will be data missing though when it stops (TF, maybe others it relies on like the ros clock topic). Sorry, this is a case where I've been off of ROS 1 for so long that my memory is starting to fade about my workflows with respect to it. I'm pretty sure the clock though is part of it.

Noted. How would i go about exploring this? As in where do i go hunting?

When you don't see any loop closures, do you still see the Ceres prints?

All this said, this is a great toolbox and FAR more intuitive to use than Cartographer. So, thank you!

Thanks!

[67bug]

Yes, we store the queue and it gets to them as they can. But I will be honest in saying that I see performance degradation of the map quality if you just throw everything into the system all at once and let it process at max rate.

Part of the reason that I know of is TF2 related. TF2 only keeps so much of the time in its buffers. So If you don't have TF up and providing the relevant data at the relevant times, then obviously things start to degrade by no fault of SLAM Toolbox. Some of the data is just no longer available or might try to use the most recent possible data which could be waaay into the future relative to that scan.

That has exactly been my finding in that there are definitely artifacts like double walls that originally made me think it was a urdf issue, but when i run it slower and ensure that the queue does not get built up to more than say 10 messages, the "double wall" artifact is gone. So in all candor, it is not slam-toolbox, but the nuances of running large bags fast bag files on a computer that is not able to keep up

I don't recall that happening from my personal experience, but there might be a rosbag setting you're missing to not bring things down once complete. There will be data missing though when it stops (TF, maybe others it relies on like the ros clock topic). Sorry, this is a case where I've been off of ROS 1 for so long that my memory is starting to fade about my workflows with respect to it. I'm pretty sure the clock though is part of it.

i have tentatively circumvented this issue by increasing the queue size on rosbag play. Why that adverb? Because i don't know this to be universally true yet

ROS1 vs ROS2

Most of our framework for everything else is built up on ROS1, so migration is going to be a bit tedious; so I am exploring what i can in without changing the foundation just yet. But a migration is on the horizon.

When you don't see any loop closures, do you still see the Ceres prints?

No, Ceres prints don't show up at all when there is no loop closure. So i know there is causality there. That's why i was poking at "how do i know it is a ceres issue? Ceres is not even being called, so my working hypothesis is as such (i assume i am entitled to be categorically wrong given my limited understanding of slam-toolbox and the karto-sdk):

1. Running at fractional rates in rosbag play is likely skipping messages needed for loop closure to kick in
2. The same seems to happen when i run rosbag play at a rate of 1, if i don't manage the queue size
3. One way around these two is to increase loop_search_maximum_distance from the default 3.0 to a slightly larger value

Based on item 3 above, one follow-up quesiton is how loop_search_maximum_distance and loop_search_space_dimension are related to each other. Perhaps i should look at the source instead to save you some headspace!

[SteveMacenski]

Got it, that's why I asked :smile: If you put logs in the Ceres plugin, do you see it being called at all? Just making sure that its not an issue that it is being called, just not doing anything usefully due to invalid inputs, constraints not being reflected, or something else which would point to it not being something in the library core.

I'm still unclear as to why it works sometimes and not others for you. That doesn't seem sensible. What if you try doing it with only 1 lidar stream instead of merging them? I'm wondering if some of this oddity is related to the scan combiner resulting in badly formatted data on the output which for some reason is harder to correlate correctly. There have been reports historically from the RP lidars of things not working super well due to the cheap-o mechanics of it and a poorly setup driver. I'm not familiar with the node you're using the merge them or that driver in particular, so that could be related.

One way around these two is to increase loop_search_maximum_distance from the default 3.0 to a slightly larger value

And that worked? I'd definitely recommend taking a look at the source code.

Most of our framework for everything else is built up on ROS1, so migration is going to be a bit tedious; so I am exploring what i can in without changing the foundation just yet. But a migration is on the horizon.

Let me know if I can be of assistance - ROS 2 Migrations are something Open Navigation offers :smile:

[67bug]

Thanks for your responses

I'm still unclear as to why it works sometimes and not others for you. That doesn't seem sensible. What if you try doing it with only 1 lidar stream instead of merging them? I'm wondering if some of this oddity is related to the scan combiner resulting in badly formatted data on the output which for some reason is harder to correlate correctly. There have been reports historically from the RP lidars of things not working super well due to the cheap-o mechanics of it and a poorly setup driver. I'm not familiar with the node you're using the merge them or that driver in particular, so that could be related.

This is worth a try. Now that i think i have a working solution, i am going back to old bag files from other large setups to make sure things are kosher with slam-toolbox. Once i wrap that up (assuming it goes well), i will come back to this experiment you are suggesting

And that worked? I'd definitely recommend taking a look at the source code.

Yes, interestingly it did. I will dig into the source code. Thanks again for your time and thoughts. I'll post my findings here.

Open Navigation: Noted! 😄

[RoboCrafty]

Hi there @67bug ! Could you please post your findings?

[67bug]

@CraftyCranberry I have a bunch of findings that i will capture here, but I have to admit that none of them fit logic as I look at how the code should work. The repo is really designed for large spaces, but there is something strange in my specific implementation that i can't quite figure out precisely. I suspect that either I am seeing a small motor-odom drift or a small imu drift that is making this problem particularly bad for me, or i am dropping messages when i am recoding the rosbag or when playing it back.

But here are my findings:

1. I saw the processing queue getting long and slam_toolbox complaining about this. To address this, i broke the rosbag play duration to smaller chunks to enable the processor to catch up and never have an issue with queuing. This was a promising solution, but is painful to handle especially when the area gets large
2. I noticed that there were times when in multiple runs, the loop closure did not happen at the same instant. For example, I would go from map point A (including pose) to map point B and then come back to map point A [and expect a loop closure], and then head on to map point C (not in the area of A to B zone) and then back to map point A. This loop closure happened almost every time, but did not a few other times -- this defied logic -- so what i did here is drove the robot in such a way that i always came back to the loop closure point A and stayed there for a few seconds with small odom changes to ensure that I had a human-interruptable way to checking if the loop closure occurred or not. I suspect -- i have no way to prove it -- that the message queue is either dropping messages or slam_toolbox is skipping messages without reporting an issue (again, the code does not show a possible way for this to happen). Stopping for a few seconds at the loop closure point ensured that any message drop was not happening. Again, not scientific at all!
3. I found that making multiple runs in a desire to get better loop closure was flawed. In other words, expecting multiple returns to map point B to cause a better loop closure does not always result in a better loop closure. Meaning, that visiting a new map point just once is adequate. It saves time and above all, keeps the 'error' limited in scope. This variation in how I mapped seems to work well and is extremely repeatable.
4. I broke the area into a bunch of smaller regions and ensured that i always started with the same orientation for mapping [to make stitching easier]. So imagine mapping a 1000mx20m room in 100mx20m segments with some overlap and stitching manually offline with an image editor. So for a large area, i tesselate the area into smaller "stitchable" zones and then do an offline stitching of the smaller maps. i know this is something built into slam_toolbox with the continuous mapping, but i wanted an idiot-proof way to make mapping repeatable.

The only other thing halfway reasonable that i can say here, @CraftyCranberry, is that keeping the graphs small-ish helped create great overall small maps and consequently great large maps. This helps me in a few other ways as the individual tesselated maps also change a lot so having to redo a map requires me to just map that one small area of the overall map.