search

SteveMacenski / slam_toolbox

Slam Toolbox for lifelong mapping and localization in potentially massive maps with ROS
GNU Lesser General Public License v2.1
1.69k stars 529 forks source link

Map making error #167

Closed joekeo closed 4 years ago

joekeo commented 4 years ago

Hello,

I am back on working with slam toolbox for a project. I am having issues when trying to map a simple environment. While mapping the map suddenly jump out of place and start mapping over the current map. I believe this is the loop closure trying to sort it out. But it seems that it produces bad results more often than not.

Any Idea why this happens? The gif below shows the situation.

slam_toolbox

The terminal shows the following information while making this gif:

[DEBUG] [1581581040.900090972]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581040.729, count now 0
[DEBUG] [1581581040.926237223]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581040.828, count now 0
[DEBUG] [1581581041.030092919]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581040.926, count now 0
[DEBUG] [1581581041.128735011]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.030, count now 0
[ INFO] [1581581041.486543340]: Metric Scores: Initial: 0.979684, IOU: 0.825835, Area: 0.878564, Num Con: 2, Reading: 0.950073, outcome score: 0.925971.
[ INFO] [1581581041.486620428]: Metric Scores: Initial: 0.976547, IOU: 0.487986, Area: 0.945577, Num Con: 3, Reading: 0.945087, outcome score: 0.974217.
[ INFO] [1581581041.486715412]: Metric Scores: Initial: 1.000000, IOU: 0.690285, Area: 0.968534, Num Con: 3, Reading: 0.941263, outcome score: 0.999000.
[ INFO] [1581581041.486795689]: Metric Scores: Initial: 0.974171, IOU: 0.395300, Area: 0.905351, Num Con: 3, Reading: 0.918221, outcome score: 0.971768.
[ INFO] [1581581041.486837989]: Metric Scores: Initial: 0.966891, IOU: 0.455146, Area: 0.938594, Num Con: 2, Reading: 0.942611, outcome score: 0.909576.
[ INFO] [1581581041.486919772]: Metric Scores: Initial: 0.091987, IOU: 0.321895, Area: 0.850516, Num Con: 2, Reading: 0.889049, outcome score: 0.039956.
[ INFO] [1581581041.486988954]: Metric Scores: Initial: 0.969430, IOU: 0.374391, Area: 0.890271, Num Con: 3, Reading: 0.906743, outcome score: 0.966797.
[ INFO] [1581581041.487068215]: Removing node 8 from graph with score: 0.039956 and old score: 0.091987.
[DEBUG] [1581581041.487296449]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.128, count now 0
[DEBUG] [1581581041.487359782]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.226, count now 0
[DEBUG] [1581581041.487401735]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.329, count now 0
[DEBUG] [1581581041.527087076]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.427, count now 0
[DEBUG] [1581581041.625617097]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.526, count now 0
[DEBUG] [1581581041.728279405]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1581581041.625, count now 0
[ INFO] [1581581042.059681338]: Metric Scores: Initial: 0.925971, IOU: 0.762233, Area: 0.844611, Num Con: 2, Reading: 0.947137, outcome score: 0.874294.
[ INFO] [1581581042.059762211]: Metric Scores: Initial: 0.974217, IOU: 0.451811, Area: 0.903682, Num Con: 3, Reading: 0.945087, outcome score: 0.971819.
[ INFO] [1581581042.059821063]: Metric Scores: Initial: 0.999000, IOU: 0.633377, Area: 0.925447, Num Con: 3, Reading: 0.936858, outcome score: 0.997944.
[ INFO] [1581581042.059861559]: Metric Scores: Initial: 0.971768, IOU: 0.365991, Area: 0.862623, Num Con: 2, Reading: 0.911047, outcome score: 0.919011.
[ INFO] [1581581042.059918327]: Metric Scores: Initial: 0.909576, IOU: 0.421151, Area: 0.895727, Num Con: 2, Reading: 0.938307, outcome score: 0.854832.
[ INFO] [1581581042.060010989]: Metric Scores: Initial: 1.000000, IOU: 0.406680, Area: 0.883359, Num Con: 3, Reading: 0.929598, outcome score: 0.999000.
[ INFO] [1581581042.060082990]: Metric Scores: Initial: 0.966797, IOU: 0.346735, Area: 0.847804, Num Con: 2, Reading: 0.899570, outcome score: 0.914929.

And my configuration looks like this:

# 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
enable_interactive_mode: true

# lifelong params
lifelong_search_use_tree: false
lifelong_minimum_score: 0.1
lifelong_iou_match: 0.85
lifelong_node_removal_score: 0.04
lifelong_overlap_score_scale: 0.06
lifelong_constraint_multiplier: 0.08
lifelong_nearby_penalty: 0.001
lifelong_candidates_scale: 0.03

# 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: true
throttle_scans: 20
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 30.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 10.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps

# 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
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: true
SteveMacenski commented 4 years ago

That print out only happens in lifelong mapping mode, which I think I print warnings everywhere that its incomplete and a research tool for the time being.

Switch back to one of the non-lifelong mapping executables. You can still update maps over time by serializing and deserializing, but automatically degrading laser scan nodes is a work in progress and is available as a reference implementation at the moment.

Admittedly, its a work in progress on the backburner but I’m hoping either I will get back to it or some interested third party will come along to work with me on building it. Its in an experimental subdirectory in the codebase

joekeo commented 4 years ago

Thanks for your reply. I modified my launch file to run the online async mode (based of the launch files and config from the repo) and still got some issues. The image below shows an example of them. image Do you have any pointer to which parameters I should be tuning to avoid this? I switched to online sync mode and the maps seems much better (almost perfect) but it 'jumps' around a lot (start with north in one direction then the whole map jumps to another orientation/place, and so on) which makes autonomous map making quite tricky.

joekeo commented 4 years ago

If you want I can make a gif out of it to show it.

joekeo commented 4 years ago

Also when I fire the localisation mode using:

<launch>

  <node pkg="slam_toolbox" type="localization_slam_toolbox_node" name="slam_toolbox" output="screen">
    <rosparam command="load" file="$(find envirobot_base)/config/slam_toolbox/mapper_params_localisation.yaml" />
  </node>

</launch>

and these as config

# 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 #localization

# if you'd like to start localizing on bringup in a map and pose
map_file_name: twyford
map_start_pose: [0,0,0]

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.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps

# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 3
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1  
link_scan_maximum_distance: 1.5
do_loop_closing: true 
loop_match_minimum_chain_size: 3
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
loop_search_maximum_distance: 5.0

# 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

I am getting this error quite consistently unless I remove hte loop closure function.

slam_toolbox_localisation_error

I think there might be an issue with the loop closure setting or function, as with at least lifelong mapping, online async and localisation modes, there are strage behaviours (like the previous comments) when doing the loop closure. As soon as a remove the loop closure part, the system seems to behave better.

SteveMacenski commented 4 years ago

I’m going to need more information: OS, install, ROS distro, laser type/manufacturer/driver, etc

If you have a bag of data that shows this behavior I can test with, that is best.

I have never seen that behavior before outside of lifelong mode, which you shouldnt use (or use any derivatives of, if you happen to be mapping or localizing against a map originally made from it, dont) for the above reasons.

joekeo commented 4 years ago

ubuntu 18.04, melodic, using the melodic-devel branch. the lidar is an RPLidar S1 using the ros package for that lidar. here is a small bag file, let me know what do you think? https://we.tl/t-TPJPL30SKA

SteveMacenski commented 4 years ago

I’d be curious if you tried this with another non-360 lidar if it continued to happen. Also, how are you liking the S1? I’ve wanted to poke at it, but I wasn’t going to shell out the money myself when I have better lidars at home already. The map quality posted isn’t reassuring of its quality, but that could be this problem too 🙃

I will admit, I’m super swamped right now. I’ll try to look at this over the weekend but my availability is a little short. If this is new, try looking at recentish commits and see if you spot something. Also check that the Ceres library didn’t update & mess something up.

How did you install ceres? Just from rosdep or a custom build / different version? Ive heard from users having problems with ceres if they didn’t use the default version, but I didn’t get specifics as to how that manifested.

SteveMacenski commented 4 years ago

Hi,

I got a chance to play with it today @joekeo. I'm not seeing the issue you describe on my side, see gif below. Sorry for the gif quality, I'm still working out how to do screen casts on my new monitor (its stupid big and stupid high update rate) so I had to substantially compress it to get it to load here (and <1gb).

If you see, the costmap follows the robot, then around the time that I think you see the jumping it deviates. I think that's because its using the transformations from your issue situation, but not reflective of the "working" transforms I was producing.

out

My steps to reproduce:

  1. Downloaded your bag file
  2. Ran 
    rosbag filter 2020-02-14-15-46-24.bag output.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "map" and m.transforms[0].child_frame_id != "odom"'
output.bag

on the bag file, to remove the transformations your recorded from your SLAM runs, so I was just left with the raw data to provide my own.

  1. Ran: roslaunch slam_toolbox online_sync.launch with a roscore and sim time. I changed the base frame from base_footprint to base_link for your TF tree but made no other modifications to the algorithm or configuration. This was done on the melodic branch as it exists right now.

I think from that "jumping" there's an issue with your configuration or if you manually installed Ceres - is your laser inverted or not inverted? Is your TF tree correct for that laser inversion?

In your gif - I'm seeing: Robot moving, robot flipping position, you dragging rviz window around, and then the map orientation correcting in the next publish update cycle. Is that correct? Is there anything you're doing between "working correctly" and before it flips around? That shouldn't happen if the first nodes are being properly held constant. In the Ceres plugin solver I check if we have a first node and we lock down the pose and orientation of it so that it cannot move as the graph updates.

Please confirm how you installed Ceres.

joekeo commented 4 years ago

Many thanks for your help. Today i will be looking at this and let you know what was the issue (If we can find it)

SteveMacenski commented 4 years ago

Update?

joekeo commented 4 years ago

Sorry for the big radio silence, Been working on fixing some electronics of the robot and took much longer than expected. I am still experiencing this issue, I will put now all my attention to the TF now and make an update soon if nothing gets in the way.

Please confirm how you installed Ceres.

I am not sure what do you mean by this, I cloned your repository and built it. Should I make a separate Ceres installation? (sorry if it is a dumb question)

SteveMacenski commented 4 years ago

Well it seemed to work on my machine without an issue after I threw out the TF generated by your run (which is a typical requirement for SLAM testing).

That makes me think your install, version, or settings if not using the defaults I mention above are off. Given you see the error when Ceres updates, it makes me think that’s suspect. How was Ceres installed? It doesn’t just come batteries included with Ubuntu so you must have installed it somehow. Did you use rosdep to install them or did you build a version of it (for instance, from running cartographer, which requires a snowflake version). Nothing else, sometimes nuking a workspace and clean rebuilding helps when you see weird stuff that others can’t reproduce. Pull latest melodic.

Or maybe architecture? What’s this running on?

joekeo commented 4 years ago

I did: rosdep install --from-paths src/slam_toolbox --ignore-src -r -y The a clean install. I used to have cartographer installed, do you think that might be the issue? I will install ceres according to: http://ceres-solver.org/installation.html# and let you know how this goes.

SteveMacenski commented 4 years ago

Install ceres via rosdep only. If you had cartographer around you may need to track down ceres there on your system and purge it. Or run through docker or something containerized.

joekeo commented 4 years ago

Good idea, will do that. Will let you know how it goes

SteveMacenski commented 4 years ago

There’s a dockerfile in the repo (though I just glanced at it and now that eloquent is the default branch the clone step needs to specify melodic-devel. If you use that it would be great if you could submit the one-line PR).

joekeo commented 4 years ago

Solved! The error was due to a bad installation of Ceres. I purged it then installed it again and everything seems to work fine. Many thanks for your time and help :)

SteveMacenski commented 4 years ago

Do you happen to know what version/install?

I’d merge a PR that set the get package to the exact rosdep version to help resolve.

From my quickness to ask, you can probably tell this has come up before.

joekeo commented 4 years ago

So... I thought that it was resolved, but it wasnt, I am going to do a clean install of the nav computer and document all steps and let you know.

On Tue, 10 Mar 2020 at 15:16, Steve Macenski notifications@github.com wrote:

Do you happen to know what version/install?

I’d merge a PR that set the get package to the exact rosdep version to help resolve.

From my quickness to ask, you can probably tell this has come up before.

— You are receiving this because you modified the open/close state. Reply to this email directly, view it on GitHub https://github.com/SteveMacenski/slam_toolbox/issues/167?email_source=notifications&email_token=ABDLJIHVELA5F7EMIK2KXULRGZKT3A5CNFSM4KUM2BA2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOEOL2XYY#issuecomment-597142499, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABDLJIGR5SJY5AYOHC33YKTRGZKT3ANCNFSM4KUM2BAQ .

joekeo commented 4 years ago

So i did a clean install and installed slam_toolbox form rosdep only and the issue persists. slam_toolbox_solver_error

There seems to be 3 instances where there was some loop closure happening, I could not see form RViz the first loop closure happening, but in the GIF above you can see what happens with the second and third loop closure. On the second, the full frame of reference of the map "jumps" and changes the visual direction that the robot is driving to (is this expected/normal? Or is this a symptom of he same error?). On the third loop closure the map gets totally rekt (RIP map) and it becomes unusable.

If you can see something interesting in the debug logs for the solver summaries let me know, I dont really understand them. And on the bottom of the thread you can see the config file that I used, might be some useful info there.

I have checked my TF and I dont see anything out of the ordinary.

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

                                     Original                  Reduced
Parameter blocks                          270                      267
Parameters                                270                      267
Residual blocks                           106                      106
Residual                                  318                      318

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                      267

Cost:
Initial                          3.491924e+06
Final                            1.189354e+02
Change                           3.491806e+06

Minimizer iterations                        6
Successful steps                            6
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0011

  Residual evaluation                  0.0129
  Jacobian evaluation                  0.0460
  Linear solver                        0.0018
Minimizer                              0.0618

Postprocessor                          0.0008
Total                                  0.0636

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 9.189293e-04 <= 1.000000e-03)

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

                                     Original                  Reduced
Parameter blocks                          354                      351
Parameters                                354                      351
Residual blocks                           158                      158
Residual                                  474                      474

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                      351

Cost:
Initial                          5.942722e+09
Final                            7.130872e+08
Change                           5.229635e+09

Minimizer iterations                        3
Successful steps                            3
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0013

  Residual evaluation                  0.0072
  Jacobian evaluation                  0.0314
  Linear solver                        0.0014
Minimizer                              0.0413

Postprocessor                          0.0011
Total                                  0.0437

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.937233e-04 <= 1.000000e-03)

[DEBUG] [1455216595.094635577]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216594.973, count now 0
[DEBUG] [1455216595.185885616]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.094, count now 0
[DEBUG] [1455216595.277185491]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.186, count now 0
[DEBUG] [1455216595.398944337]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.277, count now 0
[DEBUG] [1455216595.490273788]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.399, 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                          354                      351
Parameters                                354                      351
Residual blocks                           158                      158
Residual                                  474                      474

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                      351

Cost:
Initial                          7.130872e+08
Final                            7.130872e+08
Change                           0.000000e+00

Minimizer iterations                        1
Successful steps                            1
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0013

  Residual evaluation                  0.0026
  Jacobian evaluation                  0.0102
  Linear solver                        0.0005
Minimizer                              0.0145

Postprocessor                          0.0012
Total                                  0.0170

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.935863e-04 <= 1.000000e-03)

[DEBUG] [1455216595.581446710]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.490, count now 0
[DEBUG] [1455216595.672727663]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.581, count now 0
[DEBUG] [1455216595.794449206]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.673, count now 0
[DEBUG] [1455216595.885747202]: MessageFilter [target=odom ]: Added message in frame horizontal_laser_link at time 1455216595.794, 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                          354                      351
Parameters                                354                      351
Residual blocks                           159                      159
Residual                                  477                      477

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                      351

Cost:
Initial                          7.133361e+08
Final                            7.133361e+08
Change                           0.000000e+00

Minimizer iterations                        1
Successful steps                            1
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0013

  Residual evaluation                  0.0026
  Jacobian evaluation                  0.0114
  Linear solver                        0.0005
Minimizer                              0.0156

Postprocessor                          0.0011
Total                                  0.0180

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 3.791258e-04 <= 1.000000e-03)

And this is my config file:

# 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 #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: true
throttle_scans: 1
transform_publish_period: 0.01 #if 0 never publishes odometry
map_update_interval: 1.0
resolution: 0.025
max_laser_range: 40.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.01
minimum_travel_heading: 0.01
scan_buffer_size: 2
scan_buffer_maximum_scan_distance: 10
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: true
SteveMacenski commented 4 years ago

The only 2 things that stand out in the param file is you’re updating the slam problem every 1cm. A typical number is 10-20cm. Not only will that not scale well with mapping spaces, it breaks the assumptions of most scan matching methods (even ICP). No global slam system should be run that frequently- you have fused odometry for a reason.

Try backing that out to reasonable numbers in that range. Also consult the default configs. Do those work for you?

Send me a bag. I can quickly sanity check.

SteveMacenski commented 4 years ago

Hi @joekeo did you resolve this issue? Are you still seeing it?

joekeo commented 4 years ago

Thanks for the follow up. As a matter of fact last week as was able to solve the issue. The reason was that (as you very accurately pointed out) that there was something wrong in the TF. We manage to track it down to the IMU (Phidget IMU) being rotated 180 (roll) degrees, so at first glance it seemed in the right position. It was not until we did the URDF from scratch that we found this.

As always thanks for all your help. You are a legend

SteveMacenski commented 4 years ago

Awesome, thank you for closing the loop here. I hate leaving strings that might imply deeply rooted regressions, it makes me very nervous :smile:

IMU off by 180 degrees... jeeze, how did that ever work. Well, good catch.

Minyanaing commented 3 years ago

We are currently facing that error. The map is quite good in starting. After a few time, the drifting occurs.

And we also get these error and warning

[ WARN] [1610087231.297515186]: Queue size has grown to: 65. Recommend stopping until message is gone if online mapping.
[ERROR] [1610087232.760521892]: Transform from base_link to odom failed: Lookup would require extrapolation into the past.  Requested time 1610087169.151361488 but the earliest data is at time 1610087183.263493838, when looking up transform from frame [base_footprint] to frame [odom]

Any suggestion to fix these.

Confirmation The package doesn't provide odom to base_footprint transform, is that true? Since, we provide ourselves to use slam_toolbox.

austin-InDro commented 2 years ago

We are currently facing that error. The map is quite good in starting. After a few time, the drifting occurs.

And we also get these error and warning

[ WARN] [1610087231.297515186]: Queue size has grown to: 65. Recommend stopping until message is gone if online mapping.
[ERROR] [1610087232.760521892]: Transform from base_link to odom failed: Lookup would require extrapolation into the past.  Requested time 1610087169.151361488 but the earliest data is at time 1610087183.263493838, when looking up transform from frame [base_footprint] to frame [odom]

Any suggestion to fix these.

Confirmation The package doesn't provide odom to base_footprint transform, is that true? Since, we provide ourselves to use slam_toolbox.

I am also getting this error. Any progress on a fix?