近年來，隨著自主導航的廣泛應用，視覺同時定位與地圖構建(Visual simultaneous localization and mapping, V-SLAM)通過自身攜帶的視覺傳感器對周圍環境進行重構并且估計出自身的位置，準確定位與高精度的V-SLAM算法是實現自主導航的關鍵技術。

V-SLAM一般分為前端視覺部分和優化的后端部分基于特征點法的前端視覺部分，一直以來都是視覺里程計(Visual Odometry，VO)的主流方法，它的任務是估算相鄰圖像之間的相機運動，給后端優化提供較好的初值，后端則處理地圖模型的優化問題，依據前端視覺里程計測量的相機位姿，進行優化處理得到全局一致的軌跡和地圖，視覺里程計的實現方法，應用最廣泛的是通過ICP算法對相機進行運動估計，傳統ICP算法是建立在配準點集間完全包含的理論假設，然而實際點云配準中待配準點集間具有初始位姿偏差大，導致迭代精度不高，使得ICP算法陷入局部最優；匹配過程中隨著待匹配點對數量的增多，導致誤匹配累積增大，嚴重影響配準效果針對ICP存在的問題，研究者們進行了多種算法的改進，文獻提出一種針對家庭室內環境常見物體的三維建模方法，該方法通過手持相機環繞進行采集幀數，可以進行三維模型的重構，但是無法估計相機的位姿，后有文獻提出了一種快速三維SLAM方法，在對應點對的篩選策略上僅使用RANSAC算法，對特征點進行隨機配準，提高了算法的實時性和效率，但配準結果無法滿足全局要求，易陷入局部最優使配準效果不好，屆時有文獻提出了一種通過圖像配準的3D室內環境重建方法，采用尺度不變特征變換和隨機采樣一致性算法確定相鄰幀，將得到的變換矩陣作為精配準的初始點云數據該方法可以進行環境地圖重構，但是過度依賴圖像的幀序列，難以滿足真實環境下智能體的需求，而且無法估計移動機器人的位置和姿態。

針對以上研究現狀，本文提出了一種三階段點云配準的視覺SLAM方法，通過預處理、初配準、精配準三個階段對點云的重構和位姿估計進行處理，在預處理階段，選用RANSAC算法通過設定歐氏距離閾值篩選可靠內點來進行初始位置估計；將預處理的結果作為初值，通過基于剛體變換一致性的雙重距離閾值法完成初配準；在具有良好點云初始位置的條件下進行精配準，引入動態角度迭代因子，逐步減小誤匹配，在后端優化的關鍵幀選取中采用滑動窗口法和隨機采樣法相結合的方法，實現三維地圖的更新優化。

首先來看一下三維點云生成——Kinect標定

為了生成三維點云，需要獲取Kinect的內參矩陣來進行RGB相機與深度相機之間的剛體變換，對RGB攝像機和深度相機進行標定，利用視差補償模型對深度相機的畸變進行校正，RGB圖像與Depth圖像聯合標定前如下圖 (a)、標定后下圖(b)比較情況如下圖 所示：

RGB和Depth數據對準與點云生成

在Kinect相機完成標定后，通過RGB圖的像素坐標結合該點的Depth數據，就可以求得每個點的空間位置，從而可生成三維點云數據.空間點[x, y, z]和其在圖像中的像素坐標[u, v, d] (d指Depth數據)具有如下對應關系：

式中：fx、fy分別代表相機在x、y軸線上的焦距；cx、cy代表相機的光圈中心，s是深度圖的縮放比例因子(Depth數據與實際距離的比值)，本文取值為1000.通常情況下fx、fy、cx、cy被定義為相機的內參矩陣C：

再來看看三階段點云配準

1、預處理

ORB特征是目前最具代表性的實時圖像特征，它改善了快速檢測子FAST不定向的問題，并使用快速二進制描述子BRIEF，加快整個圖像的特征提取.提取ORB特征分為如下兩個步驟：

1) FAST角點提取：找出圖像中的“角點”

2) BRIEF描述子：對提取的特征點周圍圖像區域進行描述

在提取了ORB圖像特征之后進行點集元素的特征匹配， SLAM中的數據關聯問題通過特征匹配來解決，即確定當前時刻看到的路標與之前看到的路標之間的對應關系，匹配幀間的圖像描述子，可以為后續的姿態估計、優化等減輕工作量.為了符合在SLAM中的實時性需求，本文采用適合匹配點數量極多的FLANN(FAST Library for Approximate Nearest Neighbour，快速近似最近鄰)算法進行雙向匹配。

本文采用RANSAC算法對誤匹配進行剔除，對兩組相鄰的RGB圖的特征點對進行預處理，設定一個閾值d，剔除大于閾值的匹配點對，得到符合的內點(真實對應)，針對源點集A和目標點集B上的特征點集，d1和d2分別為A上一點在目標點集上的最近和次最近點的歐氏距離，依據工程上的經驗方法在本文中設定d2 < αd1(其中α=0.5), 取內點數量最小閾值為N=13，若不滿足則說明當前RGB圖不滿足本階段要求，進行下組RGB圖預處理.通過對內點進行最小二乘法的位姿估計，可對源點集A和目標源點集B進行一個微調，預處理后的兩片點云分別記為P和Q.相機第i時刻的位姿pi和第j時刻相機的位姿pj的位姿變換關系為: pj= pi Tij，其中

2、改進點云初配準

ICP算法用于精配準，需要滿足配準點云之間初始位置相近的條件，否則由于其對初值的極大依賴使得迭代易陷入局部最優，僅僅依靠第一步的預處理，并不能保證待配準點云初始狀態比較接近，所以不能直接使用ICP算法進行精配準，需要在預處理的基礎上對點云進行一次初配準，初配準的結果作為精配準的初始值使其具有良好的初始狀態，初配準的原理圖如下圖所示

初配準原理示意圖

同一片點云任一點與其鄰點的拓撲結構保持不變，具有剛體變換一致性.因此對于點云P中任意一點pi與其近鄰點p′i，在配對點云Q中也應是近鄰點.在預處理的基礎上獲取良好的初始狀態，若獲取的匹配點對是正確的匹配點對，則對于任意兩點對(pi，qj)和(p′i，q′j)應滿足點對間的距離相等，如式兩個約束條件：

式中：pi與p′i分別表示點云P中的任意一點與其近鄰點，qj與q′j分別表示pi與p′i在其配對點云Q中的對應匹配點. u1、u2為距離閾值，經過初配準可以剔除大部分噪聲點，獲得具有良好初始狀態的配對點云S與L。

3、 改進點云精配準

ICP匹配算法基于兩個點云完全重合的假設，采用全局搜索實現匹配點搜索.隨著地圖規模的擴大，全局搜索將帶來計算量的增加.另一方面實際匹配過程中兩片點云并不是全部點一一對應，存在大量的誤匹配點對，因此會導致容易陷入局部最優，甚至收斂失敗，所以比較好的點云初始位置，能更好地實現精配準.本文采用ICP算法的精配準目標是為了高效地配準具有良好初始狀態的三維點云，從而得到一個旋轉矩陣和平移向量.傳統的ICP算法通過最小二乘優化思想求解式，使其中的R和T最小化。

式中：n為匹配點對的數量，R為3×3的旋轉矩陣，T為×1的平移矩陣，Si和Li分別為初始和目標點集。

本文采用改進ICP算法完成精配準，其原理圖如下圖所示，完成匹配點對的搜索后，通過最小二乘優化誤差方程得到剛體變換矩陣，當ICP算法完成一次迭代之后，初始點云和目標點云的匹配可以更加準確。

精配準原理示意圖

這意味著點云S與其匹配點云L之間的偏移角減小，因此下一次ICP匹配點搜索可以縮小搜索范圍，不但減少了誤匹配，而且減少了搜索計算量.經過初配準的處理，待匹配點云之間的初始位置良好，再結合動態角度迭代因子，ICP算法每實現一次迭代，點對之間的法向量夾角會更小，通過動態閾值減小錯誤迭代次數，避免局部最優和獲得更好的位姿估計精度.本文將選擇匹配點對之間的方向向量夾角的正弦值作為匹配點誤差方程，匹配點的正弦值如式:

式中: ni和nj分別為點si與lj的近似法向量，θ為兩匹配點對的法向量夾角.匹配點的權重由式計算：

式中E為動態角度迭代閾值匹配點的數量為

本文改進后的ICP精配準步驟如下：

Step 1 初配準獲得初始對應點集Si0與Li0；

Step 2 由動態角度迭代閾值法進一步獲得精配準點集Si1={s1, …, sn1}與Li1={l1, …, ln1}；

Step 3 采取SVD求解點集Si1與Li1之間的旋轉矩陣R和平移向量t；

Step 4 根據式Si2=R1Si1+t1, 計算點集Si1經過一次迭代變換后的數據點集Si2；

Step 5 完成一次迭代后，E通過下式被縮小。

然后重復Step 2~Step 5，直到滿足

最后我們來看看它的后端優化

1、關鍵幀篩選

相機行進中會獲取室內的環境信息，若將全部采集幀的圖像位姿估計都作為狀態變量傳遞給后端優化，將導致后端優化中重建模型的規模急劇變大，程序占用的內存也會越來越高.于是需要從采集幀中選取代表性的圖像作為關鍵幀，而系統的精度與關鍵幀的選取有很大關系，當關鍵幀的選取十分稠密時，會增加回環檢測與全局優化的計算量，并且會增加很多的冗余信息，無法滿足實時性的要求；而當關鍵幀的選取比較稀疏時，則幀間配準容易失敗的情況增加，甚至導致跟蹤失敗.為了滿足幀間配準的成功率和系統實時性要求，對于當前幀，本文將采用下圖所示的滑動窗口法與隨機采樣法相結合的策略，選取相鄰的m幀，以及隨機采樣的n幀作為關鍵幀。

本文在隨機采樣時，考慮機器人在較短時間內返回當前位置的可能性較小，因此過去時間較長的幀選取的可能性大，過去時間較短的幀選取的可能性小，若當前時刻有數量為N+m的幀，去除滑動窗口法選取的最近鄰m幀，還剩N幀.那么第i幀被選中的概率為P(i)如下：

2、 位姿優化

由上文得到機器人在不同時刻的幀間變換關系，各時刻相對于初始時刻的變換關系如下式:

然而，相鄰幀間變換算法得到的矩陣Tk-1k是存在誤差的，所以機器人位姿會產生累積誤差，隨著時間的推移，累積誤差會越來越大，導致無法構建全局一致的地圖，本文采用g2o優化對位姿變換過程中的累積誤差進行優化，它將SLAM問題求解轉化為基于最小二乘法的優化問題：

式中：xi代表機器人在g2o圖中第i個節點的位姿; zij為節點xi和xj之間的邊，也就是位姿xi和xj之間的約束即變換矩陣Tij; Ωij表示xi和xj之間的信息矩陣; e(xi, xj, zij)是g2o圖中表示定點xi和xj滿足約束zij的程度，若位姿xi和xj完全滿足約束zij，那么e(xi, xj, zij)的值為0，即無需調整位姿xi和xj。

不同于傳統點云匹配方法，本文通過對RGB圖的彩色信息進行處理來獲取初始匹配內點進行微調；在不滿足精匹配初始位姿的要求下，采用匹配點對的雙重距離閾值法完成點云的初配準，從而獲取良好初始位姿；引入動態迭代角度因子改進ICP來進行點云的精配準，通過誤差的判定決定是否繼續迭代下去，每實現一次迭代都會縮小搜索范圍并且更加準確地剔除誤匹配，從而滿足V-SLAM系統對精度的需求，具有重要的現實意義。

今天分享的v-SLAM中點云配準算法，只是SLAM研究領域的一部分，如果在SLAM方面你想有更全面，更深入的研究，可以選擇在已經開發好的SLAM平臺上進行項目研究。

已經開發好的SLAM研究平臺，操作簡單，適用性強，相關配套視頻教材及例程可快速幫助研發者投入項目研究。

先來看看它的系統組成方面

1)無人機平臺

可提供多種無人機飛行平臺，飛行平臺集成光流、激光定高等傳感器，均可穩定懸停。飛行平臺均提供完善的二次開發接口，支持C++、Python、ROS、Matlab等多種開發接口。

2)視覺傳感器

支持Intel D435i、小覓等多種雙目視覺相機。

可根據客戶需求選配可見光相機、激光雷達、紅外相機等多種其他類型的傳感器。為客戶提供完善的驅動程序支持和二次開發接口。

3)機載計算機

可搭載TX2、樹莓派、Intel NUC等多種機載計算機。

4)開發工作站

提供雙目視覺slam實時狀態可視化界面和自主導航避障軌跡顯示界面。

5)視覺導航集成開發環境

提供全套基于機器視覺無人平臺自主導航避障系統軟件開發體系：包括VIO定位開發框架，MAVROS無人機二次開發SDK、自主導航避障框架、視覺仿真環境等。

6)實驗指導書

提供航線飛行和自主導航避障案例以及常見故障解決方案。

再來看看平臺具備的特點及應用價值

1) 緊扣目前無人機研究的熱點，提供視覺導航、SLAM、視覺避障、人工智能等領域最新的技術。

2)該系統提供全套的學科教學課程、示例算法，包括課程PPT及例程操作指導視頻等，可快速展開相關課程設計，進行實際教學應用階段。而且平臺由硬件平臺和軟件平臺組成，可實現slam理論教學和硬件實操。

可開設實驗課程如下：

?SLAM教學:

?飛思視覺slam硬件平臺介紹

?飛思視覺slam軟件平臺介紹

?相機原理、點云原理、GPU加速原理簡介。

?飛思視覺slam平臺支持的視覺slam架構，包括VINS，ORB_SLAM和Rovio，降低學校教師或學生學習SLAM的門檻。

?視覺slam主流技術介紹，包括多傳感器融合算法，KLT光流算法雙目VIO

?視覺詞袋技術介紹

?回環檢測技術介紹，包括重定位技術、全局位姿估計技術

?無人機通信技術，包括Mavlimk介紹和Mavros介紹

?自主導航避障算法介紹

?飛思視覺slam平臺應用實例介紹。

?航線飛行實例，自主導航避障實例

3)提供全套的機器視覺與視覺導航的開發環境、仿真環境、硬件平臺，所提供的硬件均提供完善的二次開發接口和實例代碼，省去用戶從頭搭建開發平臺的繁雜工作。

4)提供完整的無人平臺控制、通訊鏈路、視覺圖像采集、圖像識別、三維重建、定位解算等軟件算法解決方案。提供了VIO、LocalPlanner等多種實例程序，用戶只需要簡單配置即可實現完整的自主定位、自主建圖、自主導航、自主避障等功能。

針對SLAM技術我們后續還有更多精彩分享，歡迎大家留言關注！

總結

以上是生活随笔為你收集整理的python点云快速配准_分享一个V-SLAM中点云配准算法改进的方法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。