ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to detect loop closure and do relative pose graph SLAM in ROS+Gazebo?

asked 2017-06-29 20:25:34 -0600

Saurav Agarwal gravatar image

updated 2017-07-17 09:48:50 -0600

I'm implementing relative pose graph SLAM with ROS Kinetic + Gazebo. I can use the laser_scan_matcher node generate pose to pose (relative pose) odometry information (it uses canonical scan matcher to compute odometry from successive laser scans). Is there an open source library or a ROS package that allows passing in successive scans and odometry to output loop closure constraints between poses? Once loop closure constraints have been calculated one can use solvers like g2o, GTSAM or LAGO to solve for the robot trajectory, following which the laser scans can be superimposed on the aligned poses to output a map.

edit retag flag offensive close merge delete

3 Answers

Sort by » oldest newest most voted

answered 2017-07-13 09:29:04 -0600

Saurav Agarwal gravatar image

updated 2017-07-27 20:43:24 -0600

Update: Full progress on pose-graph SLAM with ROS will be posted on my blog here.

I'm going for the offline pose-graph SLAM approach where I can run the robot around some environment, gather the data, batch optimize the graph and generate a map which can then be used by AMCL for localization. As of now g2o and GTSAM both look like good options for the backend. I'm also trying out LAGO solver by Luca Carlone @ MIT, this solver is quite robust when the odometry is bad as it separates orientation and position estimation. I will keep updating this answer as I make progress. I see 4 components to this problem; (i) how to generate the pose to pose odometry ?(relatively easy these days in ROS), (ii) how to generate loop closure constraints between various poses? (iii) how to solve the graph optimization problem? (will not focus too much on this as there are bunch of awesome open source solvers like g2o, GTSAM, LAGO etc.) (iv) how to take an optimized graph and construct a usable map? Finally once I have a map, I should technically be able to use AMCL to localize.

  1. Pose to pose odometry: I tried using laser_scan_matcher to compute the transformation between poses (or keyframes) but soon realized that laser_scan_matcher (which uses CSM - Canonical Scan Matcher) doesn't work very well in cluttered environments. We have switched to using the scan matching from Hector SLAM which appears to be doing much better as of now.

  2. Front-End (Loop closure detection): open_karto appears to have a good loop closure detection module. Another option is FLIRT LIB

edit flag offensive delete link more


Laser scan matcher isnt working well for me too. And as of hector slam I tried it with intel data set and it diverges alot. link text is what we need as ICP really fails if the initial guess is wrong.

ZainMehdi gravatar image ZainMehdi  ( 2017-07-21 15:49:16 -0600 )edit

Hi,@ZainMehdi, Have you used olson's Correlative Scan Matching method? Would you like to share a copy of code of this scan matching algorithm? Thank you!

scopus gravatar image scopus  ( 2017-07-24 03:59:06 -0600 )edit

@scopus I havnt figured it out yet. Its inaccurate. I will share it with community as soon as I am done with accurate implementation. Meanwhile you can have a look at cartographer's implementation if it helps.

ZainMehdi gravatar image ZainMehdi  ( 2017-07-24 07:49:00 -0600 )edit

Try slam_karto_g2o (it uses open_karto for the front end and g2o for the backend).

Saurav Agarwal gravatar image Saurav Agarwal  ( 2017-08-14 19:02:08 -0600 )edit

answered 2017-07-17 01:59:19 -0600

Sebastian Kasperski gravatar image

If you are doing 2D SLAM, OpenKarto does indeed provide a loop closing implementation. I used it for mapping in ROS (nav2d) and found it to be quite robust. Detecting loop closures in 3D is much more difficult though.

edit flag offensive delete link more


Thanks for sharing Sebastian, I wasn't aware that nav2d used open karto. Nav2d will be very helpful, will look into it :)

Saurav Agarwal gravatar image Saurav Agarwal  ( 2017-07-17 09:21:52 -0600 )edit

It appears that nav2d is more robust in solving loop closure than the vanilla slam_karto package which (uses sparse bundle adjustment package for solving the graph). Are there some underlying changes to open karto by you to improve loop closure? I would love to learn!

Saurav Agarwal gravatar image Saurav Agarwal  ( 2017-07-29 15:04:25 -0600 )edit

No, I didn't do any changes beside some minor fixes. But nav2d uses OpenKarto 2.0, which might include some improvements. And try not to confuse Loop-Closing (front-end) and Graph-Optimization (back-end). SPO (sparse pose adjustment) is used by nav2d for optimization.

Sebastian Kasperski gravatar image Sebastian Kasperski  ( 2017-07-31 04:09:42 -0600 )edit

Of course, open karto is only giving us a front-end, the back-end is sba. Just to clarify, when I use vanilla slam_karto package (open_karto + sba) I get crashes when the sba solver (backend) is called. I didn't see this issue with nav2d. Will keep looking into it, thanks for the inputs!

Saurav Agarwal gravatar image Saurav Agarwal  ( 2017-07-31 10:39:44 -0600 )edit

answered 2017-07-12 02:07:59 -0600

ZainMehdi gravatar image

I am also doing some similar stuff. I have made some progress but its not working completely. For now there is no open source package that can detect loop closure for you. I am however working on one module that works with g2o but it will take some time for me to wrap it up. You can follow this. It describes in detail an efficient loop closing algorithm that generates robust and consistent loop closures and filters out the false ones. Simplest methods would be to check for pairwise consistency as mentioned in thesis. Moreover you can also use this to filter out false loop closures once you have them.

edit flag offensive delete link more


Thanks for the feedback! I'm currently looking into open_karto to see if its mapping module / loop closing could be used. I believe karto does sparse pose adjustment, which to me seems like a flavor of pose graph slam. lets keep this thread alive!

Saurav Agarwal gravatar image Saurav Agarwal  ( 2017-07-12 16:58:24 -0600 )edit

Let's collaborate on this if possible. The ROS community could benefit a lot from this knowledge.

Saurav Agarwal gravatar image Saurav Agarwal  ( 2017-07-12 17:01:59 -0600 )edit

I am in. Let me know your progress. I have partially working solutions. But they are not robust and cant be termed as solutions to be honest. I am developing a generic solution for online systems that process real time data. I am using g2o for optimization.

ZainMehdi gravatar image ZainMehdi  ( 2017-07-12 23:55:41 -0600 )edit

Question Tools



Asked: 2017-06-29 20:25:34 -0600

Seen: 3,786 times

Last updated: Jul 27 '17