SIM3

• SIM(3)
• • 什么是SIM3?
  • 3對點計算旋轉可以嗎?
• 計算SIM3的平移
• 計算SIM3的尺度因子
• 計算SIM3的旋轉
• 計算 Sim3 的步驟總結

以下內容參考計算機視覺life公眾號

SIM(3)

什么是SIM3?

Sim3 (Similarity Transformation)的提出就是為了解決兩個坐標系之間的相似變換問題，只要我們能得到3對匹配好的不共線的三維點在兩個坐標系下的坐標，我們就能解出Sim3相似變換。這個也是Sim3中數字3的來源。計算Sim3 實際就是計算這三個參數：旋轉 、平移 、尺度因子 。

為什么三對不共線點就可以求解？

我們來感性的理解一下，我們有三對匹配的不共線三維點可以構成兩個三角形。我們根據三角形各自的法向量可以得到他們之間的旋轉，通過相似三角形面積能夠得到尺度，用前面得到的旋轉和尺度可以把兩個三角形平行放置，通過計算距離可以得到平移.

ORB-SLAM2系統在LoopClosing線程中，當檢測到閉環候選幀的時候，就需要對當前關鍵幀和對應的閉環候選幀之間計算其變換關系。這時需要用當前關鍵幀和其對應的閉環候選幀進行sim3求解，這里的sim3求解是對當前關鍵幀和閉環候選幀之間匹配的MapPoint進行sim3求解。通過sim3變換解出當前關鍵幀和閉環候選幀的匹配MapPoint之間的旋轉矩陣R、平移向量t、尺度變換s，也就得到了當前關鍵幀到閉環關鍵幀之間的sim3變換gScm。使用這個sim3變換gScm乘上閉環關鍵幀的sim3位姿gSmw，mg2oScw=gScm*gSmw的乘積mg2oScw就是當前關鍵幀的sim3位姿，之后在閉環校正中就可以使用這個sim3位姿轉換為SE3位姿后對當前關鍵幀進行位姿校正（當然也要對關鍵幀對應的MapPoints以及其共視的關鍵幀進行校正）.

3對點計算旋轉可以嗎?

假設坐標系1下有三個不共線三維點 P1,P2 ,P3 ，他們分別和坐標系2下的三個不共線三維點Q1 ,Q2 ,Q3一一匹配。

首先，我們根據坐標系1下的三個不共線三維點來構造一個新的坐標系。
沿著 x軸上的單位向量

沿著 y軸的單位向量

沿著 z軸的單位向量

同理，我們對于坐標系2下的Q1 ,Q2 ,Q3 也可以得到沿著3個坐標軸的單位向量,我們現在要 計算 坐標系1 到坐標系2的旋轉，記坐標系單位向量構成的基底矩陣為

假設坐標系1下有一個向量V1 ，它在坐標系2下記為 V2，因為向量本身沒有變化，根據坐標系定義有

那么從坐標系1到坐標系2的旋轉就是

看起來好像沒什么問題，但是實際上我們不會這樣使用，因為存在如下問題：
1、這個旋轉的結果和選擇點的順序關系密切，我們分別讓不同的點做坐標系原點，得到的結果不同。
2、這種情況不適用于匹配點大于3個的情況。
因此實際上我們不會使用以上方法。我們通常能夠拿到遠大于3個的三維匹配點對，我們會使用最小二乘法來得到更穩定、更精確的結果。

計算SIM3的平移

假設我們得到了 n>3組匹配的三維點，分別記為Pi和Qi，我們的目的是對于
每對匹配點，找到如下的變換關系：

其中 s是尺度因子， R是旋轉， t是平移。

如果數據是沒有任何噪音的理想數據，理論上我們可以找到滿足上述關系的尺度因子、旋轉和平移。但
實際上數據是不可避免會有噪音和誤差，所以我們轉換思路，定義一個誤差ei ，我們的目的就是尋找合適的尺度因子、旋轉和平移，使得它在所有數據上的誤差最小

在開始求解之前，我們先定義兩個三維點集合中所有三維點的均值（或者稱為質心、重心）

我們對每個三維點 Pi，Qi分別減去均值，得到去中心化后的坐標 ，則有：

下面開始推導我們的誤差方程：

為了推導不顯得那樣臃腫，其中我們簡記

根據前面的推導可得等式右邊中間項

這樣我們前面的誤差方程可以化簡為：

等式右邊的兩項都是大于等于0的平方項，并且只有第二項里的t0 和我們要求的t平移 有關，所以當t0=0時，我們可以得到平移的最優解

也就是說我們知道了旋轉 R和尺度s 就能根據三維點均值做差得到平移 t了。注意這里平移的方向是Pi->Qi

計算SIM3的尺度因子

我們的誤差函數也可以進一步簡化為：

由于向量的模長不受旋轉的影響，所以
為了后續更加清晰的表示，我們用簡單的符號代替上述式子里的部分內容，所以有

由于 R是已知的，我們很容易看出來上面是一個以s 為自變量的一元二次方程，要使得該方程誤差最小，我們可以得到此時尺度 s的取值：

但是，到這里還存在一個問題，我們對P,Q 做個調換后得到：

我們看到尺度并不具備對稱性，也就是從Pi->Qi 得到的尺度并不等于從 Qi->Pi得到的尺度的倒數。 這也說明我們前面方法得到的尺度并不穩定。所以需要重新構造誤差函數，使得我們得到的尺度是對稱的、穩定的.
論文里的構造方式是：

上面等式右邊第一項只和尺度s 有關的平方項，第二項和s 無關，但和旋轉R 有關，因此令第一項為0，我們就能得到最佳的尺度。

同時，第二項里的Sp和SQ 都是平方項， 所以令第二項里的 D最大，可以使得剩下的誤差函數最小。

計算SIM3的旋轉

下面我們考慮用四元數來代替矩陣來表達旋轉。
假設空間三維點 P=[x,y,z]，用一個虛四元數來表示為p=[0,x,y,z]T 。
我們現在的的代價函數可以做如下變換：

其中：

定義

引入M 是為了方便用其元素來表示N ，我們將上面的結果代入整理，則有：

然后我們對 N進行特征值分解，求得最大特征值對應的特征向量就是待求的用四元數表示的旋轉，注意這里旋轉的方向是Pi->Qi 。
至此，我們就得到Sim3 的三個參數：旋轉 R、平移t，尺度因子s 。

計算 Sim3 的步驟總結

1、先計算旋轉R 。
具體來說，先構建 M 矩陣。

然后得到矩陣N，我們再對 N進行特征值分解，求得最大特征值對應的特征向量就是待求的用四元數表示的旋轉，注意這里旋轉的方向是Pi->Qi。
2、根據上面計算的旋轉R 來計算尺度s 。
具體來說，可以使用以下兩種方法來計算，第一種是具有對稱性的尺度（推薦）

第二種是不具有對稱性的尺度（ORBSLAM使用）

3、根據旋轉R 和尺度s 計算平移t 。

總結

以上是生活随笔為你收集整理的淦ORB-SLAM2源码 09--SIM(3)算法的全部內容，希望文章能夠幫你解決所遇到的問題。

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