Autonomous Robot Navigationand Localization on 3D Mesh
Surfaces in ROS
Sebastian Pütz, Alexander Mock2Mesh Navigation runs with Move Base FlexNavigation Control

Path Planning• Universal & Flexible RobotNavigation Control System:• Navigation & Path Planning for
Universal Map Representation:

Move Base Flex (MBF)

Mesh Navigation (MeshNav)

github.com/magazino/move_base_flex

github.com/naturerobots/mesh_navigation

Sebastian Pütz and Jorge Santos Simón, Introducing a Highly Flexible
Navigation Framework: Move Base Flex presented at the ROSCon 2017,
Vancouver, Canada, Sep. 22, 2017, https://vimeo.com/236174072

Sebastian Pütz (2022), Navigation Control & Path Planning
for Autonomous Mobile Robots, Doctoral dissertation, Osnabrück
University, https://doi.org/10.48693/69

3

Mesh Tools

Sebastian Pütz, Introducing Tools for Storing, Rendering and Annotating
Triangle Meshes in ROS and RViz, presented at the ROSCon 2018, Madrid,Spain, Sep. 30, 2018, Available: https://vimeo.com/293617680

Sebastian Pütz, Thomas Wiemann, and Joachim Hertzberg, Tools for Visualizing,
Annotating and Storing Triangle Meshes in ROS and RViz, in Proc. 9th EuropeanConference on Mobile Robotics (ECMR 2019), Prague, Czech Republic, Sep. 20194

Real-World Datasets

Farmer’s Pit Stemwede

Stone Quarry Brockum

Markt Garden Ibbenbueren

Physics Campus Westerberg
5Dijkstra's Algorithm

vs.

Continuous Vector
Field Planner (CVP)​

6

Path Planning Algorithms

7

Significant Geodesic Path
Planning Evolutions

Find references in Sebastian Pütz (2022)
Navigation Control & Path Planning
for Autonomous Mobile Robots,
Doctoral dissertation, Osnabrück University,
https://doi.org/10.48693/69
8Modern Geodesic Path
Planning Algorithms
●●
●
●
●
●
●
●
●
●●●●
●
●

2002: Single Source FMM by Novotni and Klein (NK-FMM) [210]2004: Flat-Exact based on FMM by Kirsanov (K-FMM) [153, 154]
2004: Improved exact MMP by Kirsanov (K-MMP) [153, 154]
2005: Fast and improved MMP by Surazhsky et al. (SSK*) [290]2007: Fast Sweeping Method (meshFIM) by Qian et al. [236]2009: Improved CH (ICH) by Xin and Wang [313]2011: Fast Iterative Method (meshFIM) by Fu et al. [90]2013: Saddle Vertex Graph (SVG) by Ying et al. [320]2013: Geodesics Heat Method (GHM) by Crane et al. [51]2013: Short-Term Vector Dijkstra (STVD) by Campen et al. [35]2014: Parallel CH (PCH) by Ying et al. [321]2015: Fast Wavefront Propagation CH (FWP-CH) by Xu et al. [316]2015: Fast Wavefront Propagation MMP (FWP-MMP) by Xu et al. [316]2019: Basic S* by Bhattacharya [14]2021: Continous Vector Field Planner (CVP) by Sebastian Pütz [235]

Find references in Sebastian Pütz (2022)
Navigation Control & Path Planningfor Autonomous Mobile Robots,
Doctoral dissertation, Osnabrück University,
https://doi.org/10.48693/699

Geodesic Path Planning• Used in robotics, geographic, information systems,
medical imaging, flight simulation, and water flow
analysis, CAD

planar unfolded sequence of triangles• Rarely applied and implemented in terms of robot
navigation, not to mention applications for real
outdoor environments• Dijkstra’s algorithm can lead to suboptimal paths
on triangular meshes• Many geodesic algorithms are similar to Dijkstra’salgorithm and solve the SSSP problem, thus acomparison to Dijkstra’s algorithm is reasonable
and often done in the literature“If p is a geodesic path which connects the edge sequence Ethen the planar unfolding of p along the edge sequence E is astraight line segment” (1987, Mitchell et al. [199], Lemma 3.3)

10

Continuous Vector Field Planner
Update Step – Distance and Direction Computation

Sebastian Pütz, Thomas Wiemann, Malte kleine Piening and Joachim Hertzberg, Continuous Shortest Path Vector Field Navigation on 3D
Triangular Meshes for Mobile Robots, in 2021 IEEE International Conference on Robotics and Automation (ICRA), May., Xi’an China, 2021

1112

Height Diff

Roughness

CombinedDistance13Physics Campus
Westerberg, Osnabrück,
Germany

14Physics Campus
Westerberg, ColoredMesh Dataset

15

Physics Campus
Westerberg, Dataset

16

Physics Campus Westerberg,
Path Planning with CVP17• multilevel navigation• through a tunnel andover multiple ramps• CVP Path Length Error:
0.77%, 1.54m• Dijk. Path Length Error:
5.59%, 11.12m

Path Planner
CVP

Runtime [ms] Length [m]342.9
200.57

Dijkstra176.2

210.15MMP

1 839.5

199.0318

Experimental Results•

1.8 times more time than Dijkstra, inmost cases in under a second•

MMP takes around 21,4 times moretime than Dijkstra•Our CVP Planner produces negligibly
longer paths than the exact shortestpaths•

Moreover, it computes a continuous
vector field towards the goal

A* / Dijkstra
costmap_2dDijkstra
Mesh Map
CVP
Mesh Map

19

Summary•

Fully integrated with Move Base Flex:
••

•

•

•

Planner, Controller, Recovery Interfaces:
mbf_mesh_core
MBF Mesh Navigation Server:
mbf_mesh_nav

Layered Mesh Map representation:••

Trafficability / Drivability Layers can beloaded as pluginsAvailable plugins: Roughness, Height
Differences, Steepness, Inflation,

5 test datasets of real outdoor environments:•HDF5 Mesh Maps with layer information•Corresponding Gazebo simulations Worlds•

Launch and configuration files to reproduce
experimental results•

360 stars, 50 forks on GitHub•

Continuous Integration•

released to the official ROS repositories: (apt installros-noetic-mesh-navigation)

Mesh Path planners loadable as plugins:•CVP, Dijkstra, MMP
20

21

22

Localization in Meshes using Range Sensors
Use available range sensor data tolocalize the robot relative to a given
meshComputing
UnitsSensors

github.com/uos/rmcl, Open-Source!
Environments• CPU• GPU• LiDAR• Depth Cam• Large-scale• Outdoors / Indoors
23

RMCL – Pose Tracking: MICP-L

Given a coarse odometry prior, continuously register range sensor data
to the mesh map to track the robot‘s pose: MICP-L [1][1] A. Mock, S. Pütz, T. Wiemann, and J. Hertzberg, “MICP-L: Mesh ICP forRobot Localization using Hardware-Accelerated Ray Casting,” 2023. arXiv.

24Hardware-accelerated Ray Casting
Correspondences
Find RCCs:
1.

Given a pose estimate: (green arrow)

2.

For each scan direction (orange / purple dotted line)3.

Ray cast -> intersection with the map (purple triangle)

4.

Correspondence: intersection + normal & scan point (tips of orangearrows)

5.

Optimization: point to plane (blue arrow)
Robust if there are walls!

RCCs [2] implemented via Rmagine [3]• CPU: Intel Embree [2] I. Vizzo, X. Chen, N. Chebrolu, J. Behley, and C. Stachniss, “Poisson Surface Reconstruction
for LiDAR Odometry and Mapping,” in International Conference on Robotics and Automation• GPU: NVIDIA OptiX (ICRA). IEEE, 2021, pp. 5624–5630.
[3] A. Mock, T. Wiemann, and J. Hertzberg, “Rmagine: 3D Range Sensor Simulation in Polygonal
Maps via Ray Tracing for Embedded Hardware on Mobile Robots,” in International Conferenceon Robotics and Automation (ICRA). IEEE, 2023, pp. 9076–9082.

25

Hardware-accelerated Ray Casting
Correspondences

26

CPU:
- Reduction: OpenMP- SVD: Eigen
GPU:- Reduction: CUDA
- SVD: cuSOLVER

27

Convergence• Ray casting (RC) vs point 2 plane (P2L)• Sample initial pose guesses• Measure convergence rate and registration accuracy (MED)28

Hilti[7] M. Helmberger, K. Morin, B. Berner,N. Kumar, G. Cioffi, and D. Scaramuzza,“The Hilti SLAM Challenge Dataset,” IEEE
Robotics and Automation Letters (RAL),
vol. 7, no. 3, pp. 7518–7525, 2022.

29

MulRan[8] G. Kim, Y. S. Park, Y. Cho, J.
Jeong, and A. Kim, “MulRan:
Multimodal Range Dataset for
Urban Place Recognition,” inInternational Conference on
Robotics and Automation (ICRA).
IEEE, 2020, pp. 6246–6253.

30Run time
• VLP-16 – 900 azimuths. 14,400 measurements / scan

− Plot run time per pose w.r.t. number of faces (sphere)• Correct 1000 pose guesses in parallel− Repeat for various computing devices

AMD Ryzen 7 3700X

NVIDIA RTX 2070 SUPERIntel i7 1165G7

NVIDIA RTX 2060 Max-PIntel i78750H
NVIDIA GTX 1050 Ti Max-Q

31

Usage••••••Example launch and configuration files
File path to mesh map
Odometry estimation as tf frame
Configure sensor models
Multiple sensors in 2D and 3D
GPU or CPU

Example cofig file: micp_velodyne_cpu.yaml

Example launch file: my_micp_localization.launch

32Mesh Navigation & Mesh Localization

33

34

CodeVideos

CodeVideos

github.com/naturerobots/
mesh_navigation

youtube.com/@nature-robots

github.com/uos/rmcl

youtube.com/@uos-kbs

35

References – Mesh Navigation[35] Marcel Campen, Martin Heistermann, and Leif Kobbelt. Practical Anisotropic Geodesy.
Computer Graphics Forum, 32(5):63–71, August 2013. ISSN 01677055. doi: 10.1111/cgf.12173.
[38] Jindong Chen and Yijie Han. Shortest Paths on a Polyhedron. In Proceedings of the Sixth
Annual Symposium on Computational Geometry, SCG ’90, pages 360–369, New York, NY, USA,May 1990. Association for Computing Machinery. ISBN 978-0-89791-362-1. doi:10.1145/98524.98601.
[51] Keenan Crane, Clarisse Weischedel, and Max Wardetzky. Geodesics in Heat: A New Approachto Computing Distance Based on Heat Flow. ACM Transactions on Graphics, 32(5):1–11,September 2013. ISSN 07300301. doi: 10.1145/2516971.2516977.
[90] Zhisong Fu, Won-Ki Jeong, Yongsheng Pan, Robert M. Kirby, and Ross T. Whitaker. A Fast
Iterative Method for Solving the Eikonal Equation on Triangulated Surfaces. SIAM Journal on
Scientific Computing, 33(5):2468–2488, 2011. doi: 10.1137/100788951.
[136] Takashi Kanai and Hiromasa Suzuki. Approximate Shortest Path on a Polyhedral Surface
Based on Selective Refinement of the Discrete Graph and Its Applications. In Geometric Modelingand Processing 2000. Theory and Applications. Proceedings, pages 241–250, Hong Kong, China,
2000. IEEE. ISBN 978-0-7695-0562-6. doi: 10.1109/GMAP.2000.838256.
[199] (Slide 17) Joseph S. B. Mitchell, David M. Mount, and Christos H. Papadimitriou. TheDiscrete Geodesic Problem. SIAM Journal on Computing, 16(4):647–668, August 1987. doi:
10.1137/0216045.
[119] Peter Henry, Michael Krainin, Evan Herbst, Xiaofeng Ren, and Dieter Fox. Rgb-D Mapping:
Using Kinect-Style Depth Cameras for Dense 3d Modeling of Indoor Environments. International
Journal of Robotic Research - IJRR, 31:647–663, April 2012. doi: 10.1177/0278364911434148.
[152] Ron Kimmel and James A Sethian. Computing Geodesic Paths on Manifolds. Proceedings ofthe National Academy of Sciences, 95(15):8431–8435, 1998. doi: 10.1073/pnas.95.15. 8431.
[153] Danil Kirsanov and Steven J Gortler. Minimal Discrete Curves and Surfaces. PhD Thesis,Harvard University Cambridge, MA, 2004.

[172] Mark Lanthier, Anil Maheshwari, and Jörg-Rüdiger Sack. Approximating Weighted Shortest
Paths on Polyhedral Surfaces. In Proceedings of the Thirteenth Annual Symposium on
Computational Geometry, pages 274–283, 1997.
[199] Joseph S. B. Mitchell, David M. Mount, and Christos H. Papadimitriou. The Discrete Geodesic
Problem. SIAM Journal on Computing, 16(4):647–668, August 1987. ISSN 0097-5397, 1095-7111.
doi: 10.1137/0216045.
[210] Marcin Novotni and Reinhard Klein. Computing Geodesic Distances on Triangular Meshes.
Journal of WSCG, 10(1-2):341–347, 2002.
[218] Joseph O’Rourke, Subhash Suri, and Heather Booth. Shortest Paths on Polyhedral Surfaces.
In K. Mehlhorn, editor, STACS 85, pages 243–254, Berlin, Heidelberg, 1985. Springer Berlin
Heidelberg. ISBN 978-3-540-39136-4.
[236] Jianliang Qian, Yong-Tao Zhang, and Hong-Kai Zhao. Fast Sweeping Methods for Eikonal
Equations on Triangular Meshes. SIAM Journal on Numerical Analysis, 45(1):83–107, January2007. ISSN 0036-1429, 1095-7170. doi: 10.1137/050627083.
[276] Micha Sharir and Amir Schorr. On Shortest Paths in Polyhedral Spaces. SIAM Journal onComputing, 15(1):193–215, 1986. doi: 10.1137/0215014.
[290] Vitaly Surazhsky, Tatiana Surazhsky, Danil Kirsanov, Steven J. Gortler, and Hugues Hoppe.
Fast Exact and Approximate Geodesics on Meshes. ACM Transactions on Graphics (TOG),24(3):553–560, 2005. doi: 10.1145/1073204.1073228.
[313] Shi-Qing Xin and Guo-Jin Wang. Improving Chen and Han’s Algorithm on the DiscreteGeodesic Problem. ACM Transactions on Graphics (TOG), 28(4):1–8, 2009.
[316] Chunxu Xu, Tuanfeng Y. Wang, Yong-Jin Liu, Ligang Liu, and Ying He. Fast Wavefront
Propagation (FWP) for Computing Exact Geodesic Distances on Meshes. IEEE Transactions on
Visualization and Computer Graphics, 21(7):822–834, July 2015. ISSN 1941-0506. doi:
10.1109/TVCG.2015.2407404.

[154] Danil Kirsanov, Steven J. Gortler, and Hugues Hoppe. Fast Exact and Approximate Geodesic
Paths on Meshes. Harvard University Computer Science TR 10, 4, 2004.

[320] Xiang Ying, Xiaoning Wang, and Ying He. Saddle Vertex Graph (SVG): A Novel Solution tothe Discrete Geodesic Problem. ACM Transactions on Graphics (TOG), 32, November 2013. doi:
10.1145/2508363.2508379.

[171] Mark Lanthier. Shortest Path Problems on Polyhedral Surfaces. PhD Thesis, School ofComputer Science, Carleton University, Ottawa, Ontario, December 1999.

[321] Xiang Ying, Shi-Qing Xin, and Ying He. Parallel Chen-Han (PCH) Algorithm for Discrete
Geodesics. ACM Transactions on Graphics (TOG), 33(1):1–11, 2014. doi: 10.1145/2534161.

36References – Mesh Localization[1] A. Mock, S. Pütz, T. Wiemann, and J. Hertzberg, “MICP-L: Mesh ICP for Robot
Localization using Hardware-Accelerated Ray Casting,” 2023. arXiv.
[2] I. Vizzo, X. Chen, N. Chebrolu, J. Behley, and C. Stachniss, “Poisson Surface
Reconstruction for LiDAR Odometry and Mapping,” in International Conference on
Robotics and Automation (ICRA). IEEE, 2021, pp. 5624–5630.
[3] A. Mock, T. Wiemann, and J. Hertzberg, “Rmagine: 3D Range Sensor
Simulation in Polygonal Maps via Ray Tracing for Embedded Hardware on Mobile
Robots,” in International Conference on Robotics and Automation (ICRA). IEEE,
2023, pp. 9076–9082.
[4] I. Wald, S. Woop, C. Benthin, G. S. Johnson, and M. Ernst, “Embree: A Kernel
Framework for Efficient CPU Ray Tracing,” ACM Transactions on Graphics (TOG),
2014.
[5] S. G. Parker, J. Bigler, A. Dietrich, H. Friedrich, J. Hoberock, D. Luebke, D.
McAllister, M. McGuire, K. Morley, A. Robison, and M. Stich, “OptiX: A General
Purpose Ray Tracing Engine,” ACM Transactions on Graphics (TOG), 2010.
[6] S. Umeyama, “Least-squares estimation of transformation parameters
between two point patterns,” IEEE Pattern Analysis & Machine Intelligence(PAMI), vol. 13, no. 04, pp. 376–380, 1991.
[7] M. Helmberger, K. Morin, B. Berner, N. Kumar, G. Cioffi, and D. Scaramuzza,“The Hilti SLAM Challenge Dataset,” IEEE Robotics and Automation Letters (RAL),
vol. 7, no. 3, pp. 7518–7525, 2022.
[8] G. Kim, Y. S. Park, Y. Cho, J. Jeong, and A. Kim, “MulRan: Multimodal Range
Dataset for Urban Place Recognition,” in International Conference on Robotics
and Automation (ICRA). IEEE, 2020, pp. 6246–6253.

37