在之前的博客中（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. 输出路径队列。
结果如下图：
这是最简单的实现方法，肉眼可见，该路径不够平滑。原因是每一个更新点的选择，没有考虑该点与源点的距离权重，进而使得路径回溯的方向出现抖动。为了解决该问题，增加路径权重即可，修改上述3.1步骤为：
3.1 计算T与邻居点的欧式距离，计算T与邻居点距离场的差，计算距离场差与欧氏距离的比值，取最大值为更新点。
结果稍微改善，但该结果仍然只是一个拟合结果，其误差与点云的密度正相关。如果要得到更加精确的结果，需要对点云进行合适的插值。如果未来有机会的话，我会给出具体的解决方法。