Fixed constraint between first scans for multilaser systems#539
Conversation
|
This is indeed interesting - that looks familiar to issues I had when trying to develop the Multi-robot support for this package in this branch awhile back. Have you taken a look at that and looking to add multi-robot support? |
|
Yes for my school project, we are extending SLAM toolbox to support multiple robots. We used the multi_laser branch as a guide/inspiration, and have made changes to suit our own purposes. We're making a lot of changes, so I'm not sure if a lot of that can/should be pulled into SLAM toolbox, but for this one thing I thought it was small enough, and close to the original code, that it should be PR'd. If you're interested, here's the branch for our system, and here's a video (sim only for now; we'll test IRL soon): |
|
I wish that was a proper fork so I could more easily check out the differences 😉 I took a glance through it and there are probably some good changes in there to make it all work - though I agree some of the changes like launch files / rviz config files probably shouldn't be added. Though if there were some parameter updates you found work better, I can test on some datasets I have and see if those are changes to pull in here too. If you had a few minutes to try to compare the functional changes that would get the multi-laser stuff working in here, that would be a huge improvement to this project and would definitely be noteworthy enough for a ROS Discourse announcement. Also would be nice to see your project live on long-term. It seems to me like this fix may have been one of the major things I missed when trying to get this to work myself. Got a video of a loop-closure happening because the 2 robots met somewhere in the middle of a run and corrected based on each other's graphs? That's the gooooood stuff 😆 Curious how far you are with this work - do you think you're largely done? |
|
Here you go. First robot maps the left half of the building; second robot goes from right to left. The long hallway causes drift, so the scans don't match up near the end of the hallway between the two robots. Then loop closure occurs, and the map is consistent: |
|
Would you be interested in a collaboration? I can work on making a proper PR for multi-robot SLAM toolbox by forking from ros2 branch and porting our changes (only those relevant to SLAM toolbox); we would appreciate getting more hands on testing the changes with datasets, as well as discovering / fixing issues that come up in multi-robot SLAM applications |
Seems sensible, but just to let you know, the "hardware" aspects of this is sensor processing which this library does for you already which has long been used on hardware. Changes to the underlying structure of the graph is deeper than the data itself coming in from sources, so any changes you make for this should not have issues on hardware platforms. That is, of course, except tuning of search neighborhoods / smear coefficients based on the odometry and error the map should try to correct for when doing loop closures from multiple-robots. The constraints on the NLLS problem are derivative of derivatives of the data coming in, so its sufficiently abstracted that hardware vs simulation isn't an important distinction. Though, its always great to see you work on robots running around so I would encourage it for fun's sake!
I'd certainly be happy to review / test your changes on bags I have to make sure things work fine! I only have ~2 multi-laser bags at my disposal (1 that's multi-robot, the other that's 2 lasers on a same platform - both of which are important applications this could enable). No need to have it be on ROS 2 if that's not what you're working with now. If we get things done and squared away on ROS 1, I can do the porting and conversions to ROS 2 for you so that all ROS 1/2 users can leverage the output of your project (probably easier if I just do it myself 😆 ). But all depends on what you're working on now. This sounds great! |
|
Understood. In that case, our repo was based off the ROS1 noetic-devel branch; I can start a PR to integrate the multi-robot SLAM we have so far for that branch. I've also added some visualizations that may be helpful, but that'd be a separate PR (RVIZ shows edges connecting nodes, and edges are color coded based on adjacent nodes, non-adjacent nodes, and nodes from different robots; also different robots have different colored nodes) What would be the next steps for this PR? |
|
Someone else recently added visualizing edges, so I think that’s ready to go in all the mainline branches. The different color schemes obviously is not! This PR is largely ready. I just wanted to continue this conversation where it made sense. I’ll merge tomorrow when I can cherry pick this to the other branches. |
* Fixed constraint between first scans for multilaser systems (#539) * restrction of resolution, throttle_scans and scan_buffer params (#542) * bumping 2.6.2 for Humble release Co-authored-by: jsongCMU <89307447+jsongCMU@users.noreply.github.com> Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
2 participants
Basic Info
Before and after fix:
mr_slam_prototype_loop_closure.webm
mr_slam_prototype_loop_closure_fixed.webm
Robots start 0.5 m apart. Before fix, when loop closure occurs the two first nodes are moved to the same place (incorrectly), resulting in certain walls showing up twice. This is most apparent with the south wall, where there is the original wall, then a copy of the original wall, 0.5 meters apart. After the fix, when loop closure occurs, the two first nodes have correct constraints between them, which results in the map not being distorted, and the walls showing up multiple times.
Description of contribution in a few bullet points
Fixed constraint calculation between first scans for systems with multiple laser sensors
Description of documentation updates required from your changes
None
Future work that may be required in bullet points
None