在點雲上計算兩點之間的測地線

在之前的部落格中（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. 輸出路徑佇列。

這裡需要注意的是，該結果只是一個擬合結果，其誤差與點雲的密度正相關。如果要得到更加精確的結果，需要對點雲進行合適的插值。如果未來有機會的話，我會給出具體的解決方法。