Faster ray casting methods for AMCL

[kctess5]

I posted in the forum about this already, and I was recommended to file an issue here.

I have implemented several fast ray casting methods for 2D occupancy grids, analogous to map_calc_range used in AMCL's beam model particle filter. They are all part of my C++ library called RangeLibc under an Apache 2.0 license. Docs with a general performance comparison can be found here, including the same Bresenham's Line algorithm currently used in AMCL. I've seen very good performance in my own particle filter implementation - I can maintain high-thousands of particles in real time (~30Hz) with 61 ray casts per particle on the CPU onboard a Jetson TX1.

I am interested in integrating some or most of these methods with AMCL so that users can benefit from the potentially order-of-magnitude performance improvement. I'm confident in my ability to integrate these methods with AMCL, but I would like some recommendations on how to proceed in terms of consistency with ACML best practices.

1. Should RangeLibc be a standalone package which is included, or should the relevant code be copied directly into the AMCL codebase? I assume the latter is preferable, since RangeLibc will require some modifications for integration.
2. If it should be integrated into the codebase directly, where should the code live?
3. My library has minimal dependencies, but it does require some code to compute a euclidean distance transform which is from an old commit of this repository (also Apache 2.0). I also use gflags (BSD) and lodepng (zlib) though those libraries are not strictly necessary as they support debugging/command line usage. Naturally, I make heavy use of C++ standard library functions and data structures. I would at least have to include the distance_transform.h code if I directly integrate my library.
4. The various fast ray casting methods use acceleration datastructures which of course need to be initialized, and the relevant memory objects need to be made available to the calc_range code. Maybe this should happen in handleMapMessage or maybe SetModelBeam. In any case, it should not be done if the likelihood field model is used since it does not require ray casting.
5. In RangeLibc, data->range_max is assumed to be constant and known at initialization time. Is there a good way to make this work for AMCL? I notice it is determined at the laserReceived callback. It is workable either way but would require some modification, and would make the precomputation of (at least) the "GLT" method slower since a pessimistic default would have to be used.
6. Since my algorithms use acceleration structures, they can be problematic for very quickly changing maps. I propose implementing the new algorithms alongside the existing implementation so that users may pick the best algorithm for their particular use case.
   • I assume putting the initialization code in the map callback would prevent the map from being out of sync with the acceleration structures
   • RM and CDDT both can be initialized very quickly, so maps that change occasionally would still be ok. GLT is slower to initialize but very fast at runtime so it would be best suited for static maps.
7. Similar to how my own particle filter implementation works, I assume that I should add a rosparam to specify which algorithm to use for ray casting.
   • Additionally, the discrete ray cast methods (CDDT/GLT) both have a "theta discretization" parameter. In practice I find the value doesn't matter much for PF performance so long as it is above a baseline value of around 100.

Please let me know if you have other questions, I would be happy to provide more information about RangeLibc.

Cheers!

[mikeferguson]

If you're changing the particle filter and ray casting methods, you're not updating AMCL, you're pretty much replacing it. Given that, I'd suggest releasing a standalone package, that provides one or more nodes that have similar ROS interfaces and parameters to AMCL, and then documenting and advertising those nodes so users can try them out and decide what to use on their robot.

Standalone packages that take on some or all of the functionality is not new. The navigation stack currently includes robot_pose_ekf -- but that is for legacy purposes. I've said several times we probably won't port robot_pose_ekf forward to a ROS 2.0 navigation stack, since it is inferior to other solutions such as the "graft" package, which was released standalone and AFAIK is used on most real robots building built today. Graft has a different set of parameters, but exposes the same set of ROS topics.

Also, note that for SLAM, we have multiple packages (gmapping, slam_karto, hector_slam, etc) that all have nearly identical ROS interfaces. In the case of gmapping and slam_karto, they have almost identical ROS parameters. The result is that users can choose which package to use, and maintainers can individually improve each algorithm without impacting the others.

[kctess5]

Thanks for the response.

I've written this package: https://github.com/mit-racecar/TA_example_labs/tree/master/ta_lab5 for particle filtering. It doesn't implement KLD sampling but it is quite fast on our platform. We are working on making it a bit more accessible/consistent and well documented (will have more time for this over the summer).

I thought it might be good to contribute my fast ray casting methods to ACML, since it would only require changing the map_calc_range function (with a few supporting changes) and would result in large speedup for the beam model. There is quite a bit left to AMCL that would be unchanged, including basically all of the particle filter with the exception of minor constructor/callback hooks.

Cheers!

[oscarmendezm]

Did you ever get around to doing this? I would be interested in seeing the performance increase.

[ethanguo0327]

yeah I'm interested in this too,really curious about how's it going with this work

[EwingKang]

I've written this package: https://github.com/mit-racecar/TA_example_labs/tree/master/ta_lab5 for particle filtering.

Too bad the repo link is broken. I would really love to see the performance boost in AMCL.

[SteveMacenski]

They didnt force push it out of the git history, just look back a commit or two

https://github.com/mit-racecar/TA_example_labs/tree/10c0ba6e63afaa95a90e2118c003860e54e0dd97