在之前的博客中（https://blog.csdn.net/aliexken/article/details/109256534），我们已经介绍了Fast Marching算法的原理以及实现方法。在点云上给定一个源点，通过算法，我们能够得到一个距离场，如下图所示。
有时，我们不仅需要这个距离场，我们还希望知道如何获得点云中两点的测地路径，进而帮助实现一些几何分析方法。我查阅了PCL库，好像没有给出直接的解法。这里，我们给出一个解决方法。
实现非常简单，伪代码如下：
1. 输入源点和终点的index：S和T;
2. 计算基于S的测地距离场；
3. 基于S的测地距离场，对T进行路径回溯：
3.1 搜索T的邻居，找到测地距离场值最小的一个，加入到路径队列，并将其更新T；
3.2 如果T=S，结束，否则，loop 3.1.
4. 输出路径队列。
这里需要注意的是，该结果只是一个拟合结果，其误差与点云的密度正相关。如果要得到更加精确的结果，需要对点云进行合适的插值。如果未来有机会的话，我会给出具体的解决方法。