[發明專利]一種基于三維曲線匹配的點云相對位姿估計方法

有效

• 申請號: 201310742491.2
• 申請日: 2013-12-30
• 公開（公告）號: CN103729882A
• 公開（公告）日: 2014-04-16
• 發明（設計）人: 熊蓉;李千山
• 申請（專利權）人: 浙江大學
• 主分類號: G06T17/00
• 分類號: G06T17/00
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 張法高

1.一種基于三維曲線匹配的點云相對位姿估計方法，其特征在于它的步驟如下：

1）分別提取兩幀輸入點云的邊緣輪廓，得到代表三維邊緣輪廓曲線的三維點集????????????????????????????????????????????????、；

2）分別針對三維點集、提取得到有序點集的集合、,每個有序點集或均表示了一條三維曲線；

3）將每個有序點集或進行分解，分解后的每個子集表示一個圓弧段，建立每段圓弧的特征描述向量；

4）匹配兩幀點云邊緣曲線圓弧段的特征描述向量，建立匹配關系，計算兩幀點云的相對位姿變換矩陣,為旋轉矩陣，為平移矩陣。

2.根據權利要求1所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的步驟1）為：a）對于無序點云，搜索每個點周圍半徑小于范圍內的數量不超過的所有最近鄰點，得到點集，為點集擬合平面，以點集在平面內的投影點位置為自變量，點集到平面的距離為函數值，擬合二元二次函數，得到Hessian矩陣，計算Hessian矩陣的特征值和，假定，若且，其中、分別為相應閾值，則認為該點為邊緣點；b）對于有序點云,即深度圖，利用Canny算法提取邊緣點集。

3.根據權利要求2所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的為點集擬合平面的方法為：計算點集的均值，得到平面的中心；計算的特征向量，其最小特征值對應的特征向量即為平面的法向量；平面的中心和法向量即表示了一個經過中心、法向量為的平面。

4.根據權利要求2所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的擬合二元二次函數并得到Hessian矩陣的方法為：對于點集中每一點，假定最大的兩個特征值對應的特征向量分別為和，計算一個以、為自變量，為值的鍵值對，?

最終形成一組向的鍵值映射,并利用最小二乘法求取Hessian矩陣

，

表示為

。

5.根據權利要求1所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的步驟2）為：使用區域增長算法，以每次從三維點集或中隨機選取的區域增長種子點為起始，以點集所表示的曲線一階連續為條件，通過不斷吸納符合增長條件的新點，有序地拓展曲線點集，若不能再拓展，則另取種子點并拓展另一個曲線點集，直至所有點被拓展完畢。

6.根據權利要求5所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的曲線一階連續條件為：

其中為點云中兩近鄰點、之間的距離，、分別為兩點離坐標原點的距離，為閾值。

7.根據權利要求1所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的步驟3）為：對于每一個表示三維曲線的有序點集或，依次計算每個點處曲線的曲率和扭率，

其中

為有序點集中的下一個點，隨后將表示三維曲線的有序點集拆分成若干子集，每個子集內的點具有相同的曲線曲率，且扭率為零，即該子集表示了一個圓弧，記該圓弧曲率為，弧長為，中心坐標為，依據右手坐標系規則，以圓弧法方向、圓弧起始點切法向以及與這兩個方向垂直的方向為三個軸，建立坐標系，記該坐標系和世界坐標系所成角度向量為,定義該圓弧的特征描述向量為

。

8.根據權利要求1所述的基于三維曲線匹配的點云相對位姿估計方法，其特征在于，所述的步驟4）為：?針對兩幀點云邊緣曲線圓弧段的特征描述向量，建立k-d樹，在k-d樹中，建立兩幀點云邊緣曲線圓弧段之間的匹配關系，運用隨機抽樣一致性算法，去除部分錯誤匹配，建立代價函數

其中為兩幀點云邊緣曲線圓弧段之間的匹配關系中一組匹配關系，為所有存在的匹配關系，、分別為兩幀點云中的一段圓弧段的特征描述向量，

是評估特征描述向量、之間距離的函數，與兩幀點云的相對位姿轉換矩陣有關，的定義為

其中、為指定權重的參數，

最后，優化求解

得到兩幀點云的相對位姿變換矩陣。