本文是對文章《TEASER：Fast and Certifiable Point Cloud Registration》的解讀。

摘要

這篇文章提出了第一個快速且可證明的算法，用于存在大量外點對應的情況下兩組3D點的配準。

可證明的算法嘗試求解一個困難優(yōu)化問題(比如帶外點的魯棒估計)，提供相對容易的檢測條件驗證返回的解是否最優(yōu)(比如，如果算法在外點存在情況下產生最精確的估計)或者界限解的次優(yōu)性或精確性。

為了達到這個目的，我們首先使用截斷最小二乘(TLS)代價函數將配準問題重新建模，使得估計對大量假對應點不敏感。

然后，我們提供一個通用的圖理論框架將尺度、旋轉和平移估計解耦，這樣就可以級聯(lián)地求解三個變換。

盡管每一個子問題仍然是非凸和組合的，但我們證明了：

(i) 通過一個adaptive voting機制可以在多項式時間內求解TLS尺度和分量形式平移估計。

(ii) TLS旋轉估計可以被松弛為一個半定規(guī)劃問題(SDP)，同時這個松弛是緊的，甚至是在極端外點率的情況下都可以被松弛。

(iii) 圖理論框架通過尋找最大派系允許外點的顯著修剪。

我們稱結果算法為TEASER(截斷最小二乘估計和半定松弛)。雖然求解大規(guī)模的SDP松弛通常是比較慢的，但我們開發(fā)了第二個快速的且可證明的算法，叫做TEASR++。

該算法使用漸進非凸性求解旋轉子問題，同時利用Douglas-Rachford Splitting高效地證明全局最優(yōu)性。

對于上述的兩個算法，我們在估計誤差上提供理論的界，這是第一個這樣解決魯棒配準問題的方法。

此外，我們在標準benchmarks，目標檢測數據集和3DMatch掃描匹配數據集上測試性能，展示了：

(i) 兩個算法統(tǒng)治了最先進的方法(如RANSAC，branch-&-bound，啟發(fā)式)，當尺度已知時它們對于超過99%外點率的情況都是魯棒的。

(ii) TEASER++毫秒級運行，是目前最快的魯棒配準算法。

(iii) TEASER++非常魯棒使得它能夠求解對應點未知的問題(如假設所有對所有的對應點情況)，并且它顯著地超越ICP，比Go-ICP更精確，同時要快幾個數量級。

我們發(fā)布了一個快速開源C++的TEASER ++實現。

主要貢獻

這篇文章提出了第一個可證明的算法，用于外點存在下的3D配準問題。我們使用截斷最小二乘(TLS)代價函數將配準問題重新建模，使得問題對大量假對應點不敏感，但這導致了一個困難的，組合的和非凸的優(yōu)化問題。

1.? 一個通用的框架，用于解耦尺度，旋轉和平移估計。我們方法的創(chuàng)新有四個部分：

(i) 我們開發(fā)了估計尺度的不變測量量。

(ii) 我們在噪聲未知但有界的假設下，將解耦形式化。

(iii) 我們提供了一個通用的圖理論框架，用于推導這些不變測量量。

(iv) 我們展示這個框架通過尋找不變測量量定義的圖的最大派系，允許修剪大量的外點。

解耦允許級聯(lián)地求解尺度、旋轉和平移。然而每個子問題仍然是組合的。

2.?證明：

(i) 使用adaptive voting機制能夠在多項式時間內精確地求解標量例子的TLS估計問題，這就能夠高效地進行尺度和分量形式平移的估計。

(ii) 我們能夠建立一個緊的半定規(guī)劃(SDP)松弛去估計旋轉，同時建立一個后驗條件去檢測松弛的質量。

我們注意到本文中求解的旋轉子問題在視覺(所謂的旋轉搜索)和航空航天(所謂的Wahba問題)。我們的SDP松弛是第一個用于魯棒旋轉搜索問題的可證明算法。

3.?驗證我們稱為截斷最小二乘估計和半定松弛算法返回解的質量的一系列理論結果。在無噪聲例子中，我們提供了易于檢查的條件，其中TEASER在外點存在的情況下恢復出了點云之間的變換。

在有噪聲例子中，我們在TEASER估計和真值變換之間的距離上提供了界。據我們所知，這些是有外點幾何估計問題中第一個非漸進誤差界，雖然統(tǒng)計學的魯棒估計文獻通常在歐氏空間中研究更簡單的問題并且聚焦?jié)u進界。

4.?實現TEASER的一個快速版本，稱為TEASER++，使用漸進非凸(GNC)估計旋轉而不需要求解大規(guī)模SDP。

我們展示了TEASER++是可證明的，特別地我們使用Douglas-Rachford Splitting設計一個可擴展的最優(yōu)證實算子，該算子能夠斷言GNC返回的估計值的全局最優(yōu)性。我們發(fā)布了一個快速開源C++的TEASER++的實現。

5.?在標準benchmarks和在目標檢測、掃描匹配的真實數據集上進行了大量的驗證。特別地，我們展示了。

(i) TEASER和TEASER++統(tǒng)治了最先進的算法(如RANSAC，branch-&-bound，啟發(fā)式)。當尺度已知時它們對于超過99%外點率的情況都是魯棒的。

(ii) TEASER++毫秒級運行，是目前最快的魯棒配準算法。

(iii) TEASER++非常魯棒使得它能夠求解對應點未知的問題(如假設所有對所有的對應點情況)，并且它顯著地超越ICP，比Go-ICP更精確，同時要快幾個數量級。

(iv) 當和基于深度學習的關鍵點檢測和匹配相結合時，TEASER++能夠提升配準性能。

6.在我們之前的工作中引入了TEASER和旋轉子問題的基于四元數松弛。

本文則將TEASER帶向成熟。

(i)在TEASER性能上提供顯著的理論結果。

(ii)提供一個快速的最優(yōu)性證實方法。

(iii) 開發(fā)一個快速的算法，TEASER++，使用GNC估計旋轉并且無需求解SDP，同時仍然是可證明的。

(iv) 發(fā)表了一個更全面的實驗驗證，包括在3DMatch數據集上的實際測試，以及沒有匹配點的配準例子。這些是同時在理論和實際方面的主要提升。

算法流程

1.使用截斷最小二乘代價函數的魯棒配準

2.解耦尺度，旋轉和平移估計

重新轉換測量值以得到尺度、旋轉和平移變換的不變量。

該不變量思想如圖所示

3.截斷最小二乘估計和半定松弛(TEASER)

4.魯棒的尺度和平移估計:Adaptive voting

上述第4行見Fig. 3(a)，第6、12行見Fig. 3(b)

第17行計算尺度估計值公式

5.魯棒的旋轉估計:半定松弛和快速證實

6.1 TEASE?

采用半定規(guī)劃(SDP)和凸松弛算法估計旋轉部分

6.2 TEASER++

采用 Certifiable GNC 算法估計旋轉部分，提升了算法效率和可證明能力

主要實驗結果

1.標準benchmarks測試

與Fast Global Registration (FGR) 、 Guaranteed Outlier REmoval (GORE)和RANSAC兩種變體進行精度和效率的比較

2.應用1:目標位姿估計和定位

給定來自FPFH特征描述子的對應點

3.應用2:掃描匹配

一個有趣的現象：TEASER++會證實一些不正確的解，這些解的旋轉誤差大多位于90°和180°附近(如右圖中藍色點)，這些點對應于對稱的場景，說明對稱場景允許多個配準結果。

?

TEASER++代碼解讀與運行

1.代碼地址

https://github.com/MIT-SPARK/TEASER-plusplus

提供了C++，Python和Matlab的程序。

2.代碼整體框架

(1) 輸入為兩組點云的匹配對和噪聲上界；

(2) 使用adaptive voting進行尺度估計，如果尺度已知，則進行已知尺度修剪外點操作；

(3) 產生內點圖；

(4) 在內點圖中尋找最大團從而選擇最大內點集；

(5) 最大內點集傳入GNC-TLS模塊進行旋轉估計；

(6) 使用adaptive voting進行平移估計；

(7) 輸出為尺度，旋轉和平移；

3.代碼解讀

(1) 首先是載入點云文件，讀入第一組點云數據；

(2) 將讀入的第一組點云數據轉換為Eigen格式的數據；

(3) 將第一組點云數據變?yōu)辇R次坐標形式；

(4) 對第一組點云應用一個任意SE(3)變換，產生一組無噪聲outlier-free的點云；

(5) 對第二組點云數據添加噪聲和外點；

(6) 配準算子參數設定，最大噪聲界(noise_bound，歐氏距離的L2-norm )為0.05，TLS中

?為1，是否估計尺度參數(estimate_scaling)默認為不估計false(如需要估計則設為true)；

GNC旋轉估計最大迭代次數(rotation_max_iterations)為100，GNC控制參數更新比值為1.4。

(即控制參數每次更新的調整程度，或，相關內容可以參考作者RAL2020 “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection”的 Remark 5)

采用TLS框架的GNC算法估計旋轉(rotation_estimation_algorithm)，即使用GNC_TLS計算旋轉部分的最優(yōu)解，旋轉估計部分的代價閾值(rotation_cost_threshold)為0.005。

(7) TEASER++求解，src和tgt為Eigen矩陣形式的匹配對；

(8) 運行結果，期望(Expected)旋轉、平移和估計(Estimated)旋轉、平移結果，匹配對(correspondences)數量，外點(outliers)數量，算法運行時間(Time)；

總結

此工作提出了一個快速的可證明的算法，用于極端外點率情況的基于對應點的配準問題。

使用了估計理論中的未知但有界噪聲，幾何中的不變測量，圖理論中的內點選擇最大團和優(yōu)化中的緊SDP松弛。

這篇TRO文章是由作者之前在RSS2019 “A Polynomial-time Solution for Robust Registration with Extreme Outlier Rates” 和 ICCV2019 “A Quaternion-based Certifiably Optimal Solution to the Wahba Problem with Outliers”兩個工作攛掇起來的。

使用了前者RSS2019中的不變測量概念，后者ICCV2019中旋轉參數化為四元數的想法，整體算法流程類似于前者的框架，總的來說是一個很出色的整體工作。

追溯著看，作者一系列的工作都很solid，有理論創(chuàng)新（數學證明），有實際實現（代碼規(guī)范），非常值得學習。

論文鏈接：
https://arxiv.org/pdf/2001.07715.pdf論文視頻：

https://youtu.be/xib1RSUoeeQ