技術標籤：位姿測量演算法機器學習計算機視覺

文章目錄

• 前言
• • • 三維座標系之間的轉換關係：R(旋轉矩陣) 、T（平移矩陣）
    • 數學模型描述
    • • • 布林莎模型:前提是旋轉角為微小旋轉角
        • 羅德里格矩陣模型
    • 尺度相同:
    • 存在的一些問題：
    • 主要參考來源：

---

前言

三維座標系之間的轉換關係：R(旋轉矩陣) 、T（平移矩陣）

  在大地測量、工程測量、攝影測量等領域中，座標系之間的轉換是必不可少的。空間座標轉換的實質是用公共點的２套座標和非公共點的１套座標推估非公共點的另１套座標。

  座標轉換過程通常分２步，先由公共點座標解算轉換引數，再由轉換引數轉換非公共點。轉換引數通常分為旋轉、平移和尺度引數，其中旋轉引數的確定是座標轉換的核心。

  傳統的三維座標轉換模型是用３個旋轉角作為旋轉引數，建立的模型是非線性的，常需要用泰勒級數展開的方法將模型線性化，計算比較繁雜。

  在小角度旋轉情況下，可對旋轉矩陣作近似處理，得到線性模型，如常用的布林莎模型。

  針對大旋角的座標轉換問題，多采用羅德里格矩陣表示旋轉矩陣的座標轉換方法，僅有３個旋轉引數，計算過程無需線性化，且能適用大旋角轉換。

數學模型描述

布林莎模型:前提是旋轉角為微小旋轉角

設矩陣 A 為 A 座標系下公共點的三維座標，矩陣B為B座標系下公共點的三維座標。由三維座標轉換模型可得，A、B 兩座標系的三維座標轉換方程如下所示：
[ x y z ] B = [ Δ x Δ y Δ z ] + ( 1 + k ) R [ x y z ] A (1) \left[\begin{array}{l}x \\ y \\ z\end{array}\right]_{B}=\left[\begin{array}{l}\Delta x \\ \Delta y \\ \Delta z\end{array}\right]+(1+k) \boldsymbol{R}\left[\begin{array}{l}x \\ y \\ z\end{array}\right]_{A}\tag{1}

⎣⎡​xyz​⎦⎤​B​=⎣⎡​ΔxΔyΔz​⎦⎤​+(1+k)R⎣⎡​xyz​⎦⎤​A​(1)
式中： Δ x 、 Δ y 、 Δ z Δx、Δy、Δz Δx、Δy、Δz 表示座標原點的平移量， k k k 為尺度因子， R R R 為 A 站位到 B 站位的旋轉矩陣。為了對座標轉換精度進行分析，將三維座標轉換模型作如下簡化:
[ x y z ] B = k [ x y z ] A + [ 0 − ε z ε y ε z 0 − ε x − ε y ε x 0 ] ⋅ [ x y z ] A + [ Δ x Δ y Δ z ] (2) \left[\begin{array}{c}x \\ y \\ z\end{array}\right]_{B}=k\left[\begin{array}{c}x \\ y \\ z\end{array}\right]_{A}+\left[\begin{array}{ccc}0 & -\varepsilon_{z} & \varepsilon_{y} \\ \varepsilon_{z} & 0 & -\varepsilon_{x} \\ -\varepsilon_{y} & \varepsilon_{x} & 0\end{array}\right] \cdot\left[\begin{array}{l}x \\ y \\ z\end{array}\right]_{A}+\left[\begin{array}{c}\Delta x \\ \Delta y \\ \Delta z\end{array}\right] \tag{2}

⎣⎡​xyz​⎦⎤​B​=k⎣⎡​xyz​⎦⎤​A​+⎣⎡​0εz​−εy​​−εz​0εx​​εy​−εx​0​⎦⎤​⋅⎣⎡​xyz​⎦⎤​A​+⎣⎡​ΔxΔyΔz​⎦⎤​(2)
應注意到，式(2)中 ε x 、 ε y 、 ε z ε_x 、ε_y 、ε_z εx​、εy​、εz​ 均為微小角，則$sin{x} = x ,cos{x}=1 $因此，應在利用座標轉換模型前通過初步的座標系轉換得到一組轉站引數的初值，使 A、B 兩座標系之間的轉換滿足微小角近似。式(2)可進一步寫成：
[ x y z ] B = [ 1 0 0 0 z − y x 0 1 0 − z 0 x y 0 0 1 y − x 0 z ] A ⋅ [ Δ x Δ y Δ z ε x ε y ε z k ] (3) \left[\begin{array}{l}x \\ y \\ z\end{array}\right]_{B}=\left[\begin{array}{lllllll}1 & 0 & 0 & 0 & z & -y & x \\ 0 & 1 & 0 & -z & 0 & x & y \\ 0 & 0 & 1 & y & -x & 0 & z\end{array}\right]_{A} \cdot\left[\begin{array}{l}\Delta x \\ \Delta y \\ \Delta z \\ \varepsilon_{x} \\ \varepsilon_{y} \\ \varepsilon_{z} \\ k\end{array}\right] \tag{3} ⎣⎡​xyz​⎦⎤​B​=⎣⎡​100​010​001​0−zy​z0−x​−yx0​xyz​⎦⎤​A​⋅⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​ΔxΔyΔzεx​εy​εz​k​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤​(3)
根據式(3)，可將 n 個公共點的座標轉換寫為$ B=AX$ 的形式，其中：
A = ( 1 0 0 0 z 1 − y 1 x 1 0 1 0 − z 1 0 x 1 y 1 0 0 1 y 1 − x 1 0 z 1 1 0 0 0 z 2 − y 2 x 2 0 1 0 − z 2 0 x 2 y 2 0 0 1 y 2 − x 2 0 z 2 1 0 0 0 z n − y n x n 0 1 0 − z n 0 x n y n 0 0 1 y n − x n 0 z n ) A (4) \boldsymbol{A}=\left(\begin{array}{ccccccc}1 & 0 & 0 & 0 & z_{1} & -y_{1} & x_{1} \\ 0 & 1 & 0 & -z_{1} & 0 & x_{1} & y_{1} \\ 0 & 0 & 1 & y_{1} & -x_{1} & 0 & z_{1} \\ 1 & 0 & 0 & 0 & z_{2} & -y_{2} & x_{2} \\ 0 & 1 & 0 & -z_{2} & 0 & x_{2} & y_{2} \\ 0 & 0 & 1 & y_{2} & -x_{2} & 0 & z_{2} \\ 1 & 0 & 0 & 0 & z_{n} & -y_{n} & x_{n} \\ 0 & 1 & 0 & -z_{n} & 0 & x_{n} & y_{n} \\ 0 & 0 & 1 & y_{n} & -x_{n} & 0 & z_{n}\end{array}\right)_{A} \tag{4} A=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛​100100100​010010010​001001001​0−z1​y1​0−z2​y2​0−zn​yn​​z1​0−x1​z2​0−x2​zn​0−xn​​−y1​x1​0−y2​x2​0−yn​xn​0​x1​y1​z1​x2​y2​z2​xn​yn​zn​​⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞​A​(4)
利用最小二乘法對轉換引數進行引數估計，可得轉換引數 X X X 的估值為:
X = ( A T Q − 1 A ) − 1 A T Q B (5) \boldsymbol{X}=\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{A}\right)^{-1} \boldsymbol{A}^{\mathrm{T}} \boldsymbol{Q} \boldsymbol{B}\tag{5} X=(ATQ−1A)−1ATQB(5)
式中： Q Q Q 為B 座標系下$ n$ 個公共點座標測量誤差的協方差矩陣,則得轉換引數方差陣為：
Q x = ( A T Q − 1 A ) − 1 = [ q 11 q 12 ⋯ q 17 q 21 q 22 ⋯ q 27 ⋮ q 71 q 72 ⋯ q 77 ] (6) \boldsymbol{Q}_{x}=\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{A}\right)^{-1}=\left[\begin{array}{cccc}q_{11} & q_{12} & \cdots & q_{17} \\ q_{21} & q_{22} & \cdots & q_{27} \\ & & \vdots & \\ q_{71} & q_{72} & \cdots & q_{77}\end{array}\right]\tag{6} Qx​=(ATQ−1A)−1=⎣⎢⎢⎢⎡​q11​q21​q71​​q12​q22​q72​​⋯⋯⋮⋯​q17​q27​q77​​⎦⎥⎥⎥⎤​(6)
對於空間任意一點 P P P，其座標轉換誤差方差陣為:
Q P = B P Q x B P T = [ q x x q x y q x z q y x q y y q y z q z x q z y q z z ] \boldsymbol{Q}_{P}=\boldsymbol{B}_{P} \boldsymbol{Q}_{x} \boldsymbol{B}_{P}^{\mathrm{T}}=\left[\begin{array}{lll}q_{x x} & q_{x y} & q_{x z} \\ q_{y x} & q_{y y} & q_{y z} \\ q_{z x} & q_{z y} & q_{z z}\end{array}\right] QP​=BP​Qx​BPT​=⎣⎡​qxx​qyx​qzx​​qxy​qyy​qzy​​qxz​qyz​qzz​​⎦⎤​

羅德里格矩陣模型

空間直角座標系轉換模型
[ X Y Z ] = λ R [ x y z ] + [ Δ X Δ Y Δ Z ] (1) \left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]=\lambda \boldsymbol{R}\left[\begin{array}{l}x \\ y \\ z\end{array}\right]+\left[\begin{array}{l}\Delta X \\ \Delta Y \\ \Delta Z\end{array}\right] \tag{1} ⎣⎡​XYZ​⎦⎤​=λR⎣⎡​xyz​⎦⎤​+⎣⎡​ΔXΔYΔZ​⎦⎤​(1)
λ \lambda λ為尺度比例因子，假設其初值是１； R R R 為 3 × 3 3×3 3×3的旋轉矩陣；

引入一個具有３個獨立元素的反對稱矩陣:
S = [ 0 − c − b c 0 − a b a 0 ] (2) S=\left[\begin{array}{rrr}0 & -c & -b \\ c & 0 & -a \\ b & a & 0\end{array}\right] \tag{2} S=⎣⎡​0cb​−c0a​−b−a0​⎦⎤​(2)
式中 $ a,b,c$ 相互獨立，則羅德里格矩陣可由反對稱矩陣構建為：
R = ( I + S ) ( I − S ) − 1 (3) R=(I+S)(I-S)^{-1} \tag{3} R=(I+S)(I−S)−1(3)
式中 R R R是個正交矩陣，其中 I I I 是 ３ 階單位陣,將 R R R 展開為:
KaTeX parse error: Expected group after '\begin{array}' at position 16: \begin{array} &̲ R=\frac{1}{1+a…
將旋轉矩陣帶入式（1）可解得平移向量:
[ X i Y i Z i ] = λ 1 + a 2 + b 2 + c 2 [ 1 + a 2 − b 2 − c 2 − 2 a b − 2 c − 2 b + 2 a c 2 c − 2 a b 1 − a 2 + b 2 − c 2 − 2 a − 2 b c 2 a c + 2 b 2 a − 2 b c 1 − a 2 − b 2 + c 2 ] [ x i y i z i ] + [ Δ X Δ Y Δ Z ] (5) \left[\begin{array}{c}X_{i} \\ Y_{i} \\ Z_{i}\end{array}\right]=\frac{\lambda}{1+a^{2}+b^{2}+c^{2}}\left[\begin{array}{ccc}1+a^{2}-b^{2}-c^{2} & -2 a b-2 c & -2 b+2 a c \\ 2 c-2 a b & 1-a^{2}+b^{2}-c^{2} & -2 a-2 b c \\ 2 a c+2 b & 2 a-2 b c & 1-a^{2}-b^{2}+c^{2}\end{array}\right]\left[\begin{array}{c}x_{i} \\ y_{i} \\ z_{i}\end{array}\right]+\left[\begin{array}{c}\Delta X \\ \Delta Y \\ \Delta Z\end{array}\right]\tag{5} ⎣⎡​Xi​Yi​Zi​​⎦⎤​=1+a2+b2+c2λ​⎣⎡​1+a2−b2−c22c−2ab2ac+2b​−2ab−2c1−a2+b2−c22a−2bc​−2b+2ac−2a−2bc1−a2−b2+c2​⎦⎤​⎣⎡​xi​yi​zi​​⎦⎤​+⎣⎡​ΔXΔYΔZ​⎦⎤​(5)
式（5）中未知引數共有 ７ 個，即 $λ 、ａ 、 ｂ 、 ｃ 、 ΔＸ 、 Δ Ｙ 、 ΔＺ $，進行線性化處理得到誤差方程：
v = A Δ x + l (6) \boldsymbol{v}=\boldsymbol{A} \Delta \boldsymbol{x}+\boldsymbol{l} \tag{6} v=AΔx+l(6)
對式（6）,利用最小二乘法可得到：
Δ x = ( A T P A ) − 1 ( A T P l ) (7) \boldsymbol{\Delta} \boldsymbol{x}=\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{P} \boldsymbol{A}\right)^{-1}\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{P} \boldsymbol{l}\right) \tag{7} Δx=(ATPA)−1(ATPl)(7)
式（7）中， P P P位單位矩陣；
A = [ ∂ X i ∂ λ ∂ X i ∂ a ∂ X i ∂ b ∂ X i ∂ c ∂ X i ∂ Δ X ∂ X i ∂ Δ Y ∂ X i ∂ Δ Z ∂ Y i ∂ λ ∂ Y i ∂ a ∂ Y i ∂ b ∂ Y i ∂ c ∂ Y i ∂ Δ X ∂ Y i ∂ Δ Y ∂ Y i ∂ Δ Z ∂ Z i ∂ λ ∂ Z i ∂ a ∂ Z i ∂ b ∂ Z i ∂ c ∂ Z i ∂ Δ X ∂ Z i ∂ Δ Y ∂ Z i ∂ Δ Z ] \boldsymbol{A}=\left[\begin{array}{ccccccc}\frac{\partial X_{i}}{\partial \lambda} & \frac{\partial X_{i}}{\partial a} & \frac{\partial X_{i}}{\partial b} & \frac{\partial X_{i}}{\partial c} & \frac{\partial X_{i}}{\partial \Delta X} & \frac{\partial X_{i}}{\partial \Delta Y} & \frac{\partial X_{i}}{\partial \Delta Z} \\ \frac{\partial Y_{i}}{\partial \lambda} & \frac{\partial Y_{i}}{\partial a} & \frac{\partial Y_{i}}{\partial b} & \frac{\partial Y_{i}}{\partial c} & \frac{\partial Y_{i}}{\partial \Delta X} & \frac{\partial Y_{i}}{\partial \Delta Y} & \frac{\partial Y_{i}}{\partial \Delta Z} \\ \frac{\partial Z_{i}}{\partial \lambda} & \frac{\partial Z_{i}}{\partial a} & \frac{\partial Z_{i}}{\partial b} & \frac{\partial Z_{i}}{\partial c} & \frac{\partial Z_{i}}{\partial \Delta X} & \frac{\partial Z_{i}}{\partial \Delta Y} & \frac{\partial Z_{i}}{\partial \Delta Z}\end{array}\right] A=⎣⎡​∂λ∂Xi​​∂λ∂Yi​​∂λ∂Zi​​​∂a∂Xi​​∂a∂Yi​​∂a∂Zi​​​∂b∂Xi​​∂b∂Yi​​∂b∂Zi​​​∂c∂Xi​​∂c∂Yi​​∂c∂Zi​​​∂ΔX∂Xi​​∂ΔX∂Yi​​∂ΔX∂Zi​​​∂ΔY∂Xi​​∂ΔY∂Yi​​∂ΔY∂Zi​​​∂ΔZ∂Xi​​∂ΔZ∂Yi​​∂ΔZ∂Zi​​​⎦⎤​

Δ x = [ Δ λ Δ a Δ b Δ c Δ X Δ Y Δ Z ] T \Delta \boldsymbol{x}=\left[\begin{array}{llllll}\Delta \lambda & \Delta a & \Delta b & \Delta c & \Delta X & \Delta Y & \Delta Z\end{array}\right]^{\mathrm{T}} Δx=[Δλ​Δa​Δb​Δc​ΔX​ΔY​ΔZ​]T

l = [ l 1 l 2 l 3 ] T \boldsymbol{l}=\left[\begin{array}{lll}l_{1} & l_{2} & l_{3}\end{array}\right]^{\mathrm{T}} l=[l1​​l2​​l3​​]T

轉換引數求解

引數解算過程可分３步，先求尺度引數，再求旋轉引數，最後求平移引數。 尺度引數可由２個公共點在不同座標系 下的距離之比算出：
λ = ( X 2 − X 1 ) 2 + ( Y 2 − Y 1 ) 2 + ( Z 2 − Z 1 ) 2 ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 + ( z 2 − z 1 ) 2 \lambda=\frac{\sqrt{\left(X_{2}-X_{1}\right)^{2}+\left(Y_{2}-Y_{1}\right)^{2}+\left(Z_{2}-Z_{1}\right)^{2}}}{\sqrt{\left(x_{2}-x_{1}\right)^{2}+\left(y_{2}-y_{1}\right)^{2}+\left(z_{2}-z_{1}\right)^{2}}} λ=(x2​−x1​)2+(y2​−y1​)2+(z2​−z1​)2 ​(X2​−X1​)2+(Y2​−Y1​)2+(Z2​−Z1​)2 ​​
公共點較多時，可求出各點間多個距離比，再取平均值；

在解算旋轉引數 $ａ,b，c $時，可以先消去平移引數，將２個公共點的座標代入式（ 1）,分別按照式（1） 做差得:
[ X 2 − X 1 Y 2 − Y 1 Z 2 − Z 1 ] = λ R [ x 2 − x 1 y 2 − y 1 z 2 − z 1 ] \left[\begin{array}{l}X_{2}-X_{1} \\ Y_{2}-Y_{1} \\ Z_{2}-Z_{1}\end{array}\right]=\lambda \boldsymbol{R}\left[\begin{array}{l}x_{2}-x_{1} \\ y_{2}-y_{1} \\ z_{2}-z_{1}\end{array}\right] ⎣⎡​X2​−X1​Y2​−Y1​Z2​−Z1​​⎦⎤​=λR⎣⎡​x2​−x1​y2​−y1​z2​−z1​​⎦⎤​
聯絡式（2）式（3） 可得：
[ 0 λ z 21 + Z 21 λ y 21 + Y 21 λ z 21 + Z 21 0 − λ x 21 − X 21 − λ y 21 − Y 21 − λ x 21 − X 21 0 ] [ a b c ] = [ λ x 21 − X 21 λ y 21 − Y 21 λ z 21 − Z 21 ] \left[\begin{array}{ccc}0 & \lambda z_{21}+Z_{21} & \lambda y_{21}+Y_{21} \\ \lambda z_{21}+Z_{21} & 0 & -\lambda x_{21}-X_{21} \\ -\lambda y_{21}-Y_{21} & -\lambda x_{21}-X_{21} & 0\end{array}\right]\left[\begin{array}{l}a \\ b \\ c\end{array}\right]=\left[\begin{array}{l}\lambda x_{21}-X_{21} \\ \lambda y_{21}-Y_{21} \\ \lambda z_{21}-Z_{21}\end{array}\right] ⎣⎡​0λz21​+Z21​−λy21​−Y21​​λz21​+Z21​0−λx21​−X21​​λy21​+Y21​−λx21​−X21​0​⎦⎤​⎣⎡​abc​⎦⎤​=⎣⎡​λx21​−X21​λy21​−Y21​λz21​−Z21​​⎦⎤​
式中：
x 21 = x 2 − x 1 ; y 21 = y 2 − y 1 ; z 21 = z 2 − z 1 X 21 = X 2 − X 1 ; Y 21 = Y 2 − Y 1 ; Z 21 = Z 2 − Z 1 \begin{array}{l}x_{21}=x_{2}-x_{1} ; y_{21}=y_{2}-y_{1} ; z_{21}=z_{2}-z_{1} \\ X_{21}=X_{2}-X_{1} ; Y_{21}=Y_{2}-Y_{1} ; Z_{21}=Z_{2}-Z_{1}\end{array} x21​=x2​−x1​;y21​=y2​−y1​;z21​=z2​−z1​X21​=X2​−X1​;Y21​=Y2​−Y1​;Z21​=Z2​−Z1​​
這個方程組左邊的係數矩陣為奇異陣，３個方程裡僅有２個獨立，需要至少２個這樣的方程組才能解算出ａ，ｂ，ｃ，也就是至少需要 ３ 個公共點。 當有ｎ個公共點時，可列出（ n－１）個形如上式的方程 ，共有 3（n-1）個方程，其總誤差方程為：
V = B 3 ( n − 1 ) × 3 X 3 × 1 − L 3 ( n − 1 ) × 3 V=\underset{3(n-1) \times 3}{B} \underset{3 \times 1}{X}-\underset{3(n-1) \times 3}{L} V=3(n−1)×3B​3×1X​−3(n−1)×3L​
式中：
B = [ 0 − λ z 21 − Z 21 − λ y 21 − Y 21 − λ z 21 − Z 21 0 λ x 21 + X 21 λ y 21 + Y 21 λ x 21 + X 21 0 ⋮ ⋮ ⋮ 0 − λ z n 1 − Z n 1 − λ y n 1 − Y n 1 − λ z n 1 − Z n 1 0 λ x n 1 + X n 1 λ y n 1 + Y n 1 λ x n 1 + X n 1 0 ] B=\left[\begin{array}{ccc}0 & -\lambda z_{21}-Z_{21} & -\lambda y_{21}-Y_{21} \\ -\lambda z_{21}-Z_{21} & 0 & \lambda x_{21}+X_{21} \\ \lambda y_{21}+Y_{21} & \lambda x_{21}+X_{21} & 0 \\ \vdots & \vdots & \vdots \\ 0 & -\lambda z_{n 1}-Z_{n 1} & -\lambda y_{n 1}-Y_{n 1} \\ -\lambda z_{n 1}-Z_{n 1} & 0 & \lambda x_{n 1}+X_{n 1} \\ \lambda y_{n 1}+Y_{n 1} & \lambda x_{n 1}+X_{n 1} & 0\end{array}\right] B=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​0−λz21​−Z21​λy21​+Y21​⋮0−λzn1​−Zn1​λyn1​+Yn1​​−λz21​−Z21​0λx21​+X21​⋮−λzn1​−Zn1​0λxn1​+Xn1​​−λy21​−Y21​λx21​+X21​0⋮−λyn1​−Yn1​λxn1​+Xn1​0​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

X = [ a , b , c ] T X=[a, b, c]^{\mathrm{T}} X=[a,b,c]T

L = [ X 21 − λ x 21 Y 21 − λ y 21 Z 21 − λ z 21 ⋯ X n 1 − λ x n 1 Y n 1 − λ y n 1 Z n 1 − λ z n 1 ] T \begin{array}{cccc}L= & {\left[X_{21}-\lambda x_{21}\right.} & Y_{21}-\lambda y_{21} & Z_{21}-\lambda z_{21} & \cdots \\ X_{n 1}-\lambda x_{n 1} & Y_{n 1}-\lambda y_{n 1} & \left.Z_{n 1}-\lambda z_{n 1}\right]^{\mathrm{T}}\end{array} L=Xn1​−λxn1​​[X21​−λx21​Yn1​−λyn1​​Y21​−λy21​Zn1​−λzn1​]T​Z21​−λz21​⋯

按最小二乘法間接平差原理求解未知數:
X = ( A T A ) − 1 A T L X=\left(A^{\mathrm{T}} A\right)^{-1} A^{\mathrm{T}} L X=(ATA)−1ATL
計算出 $ａ,b，c $後，即可求出旋轉矩陣，然後按下式求解平移引數：
[ Δ X Δ Y Δ Z ] = [ X i Y i Z i ] − λ R [ x i y i z i ] \left[\begin{array}{l}\Delta X \\ \Delta Y \\ \Delta Z\end{array}\right]=\left[\begin{array}{l}X_{i} \\ Y_{i} \\ Z_{i}\end{array}\right]-\lambda R\left[\begin{array}{l}x_{i} \\ y_{i} \\ z_{i}\end{array}\right] ⎣⎡​ΔXΔYΔZ​⎦⎤​=⎣⎡​Xi​Yi​Zi​​⎦⎤​−λR⎣⎡​xi​yi​zi​​⎦⎤​

尺度相同:

  三維重建方法通常會自己估計相機的$R,T 矩 陣 ， 這 些 矩 陣 定 義 了 一 個 世 界 坐 標 系 ， 在 使 用 客 觀 的 評 估 方 法 如 [ M i d d l e b u r y ] 來 評 估 精 度 時 ， 需 要 使 用 評 估 方 法 提 供 的 相 機 的 矩陣，這些矩陣定義了一個世界座標系，在使用客觀的評估方法如[Middlebury]來評估精度時，需要使用評估方法提供的相機的 矩陣，這些矩陣定義了一個世界坐標系，在使用客觀的評估方法如[Middlebury]來評估精度時，需要使用評估方法提供的相機的R,T$矩陣，這些矩陣定義了另外一個世界座標系，兩者通常會有尺度、旋轉、平移的差別，這就需要在座標系之間進行轉換。

  兩個相同尺度的世界座標系可以通過 R , T R,T R,T進行轉換，計算轉換關係需要知道雙方 N N N個對應點的座標，設為 A A A, B B B，則求解 B = R ∗ A + T B=R∗A+T B=R∗A+T即可。由於 N N N可能比較大，因此此方程通常為超定方程，可使用奇異值分解(Singular Value Decomposition (SVD))進行計算，其內部原理是最小二乘法。

對公共點座標進行重心化處理:
c e n t r o i d A = 1 N ∑ i = 1 N ( A ) centroid_A =\frac{1}{N} ∑^N_{i=1}(A) centroidA​=N1​i=1∑N​(A)

c e n t r o i d B = 1 N ∑ i = 1 N ( B ) centroid_B =\frac{1}{N} ∑^N_{i=1}(B) centroidB​=N1​i=1∑N​(B)

求：
H = ∑ i = 1 N ( P A i − c e n t r o i d A ) ( P B i − c e n t r o i d B ) T H=∑^N_{i=1}(P^{i}_{A}−centroid_A)(P^i_B−centroid_B)^T H=i=1∑N​(PAi​−centroidA​)(PBi​−centroidB​)T

[ U , S , V ] = S V D ( H ) [U,S,V]=SVD(H) [U,S,V]=SVD(H)

R = V U T R=V U^T R=VUT

T = − R ∗ c e n t r o i d A + c e n t r o i d B T=−R∗centroid_A+centroid_B T=−R∗centroidA​+centroidB​

  其中 c e n t r o i d A centroid_A centroidA​和 c e n t r o i d B centroid_B centroidB​是 A , B A,B A,B的平均中心。

%計算平均中心點
  centroid_A = mean(A);
  centroid_B = mean(B);

  N = size(A,1);
  H = (A - repmat(centroid_A, N, 1))' * (B - repmat(centroid_B, N, 1));

  [U,S,V] = svd(H);

  R = V*U';
  if det(R) < 0
  	printf('Reflection detected\n');
  	V(:,3) = -1*V(:,3);
  	R = V*U';
  end

  t = -R*centroid_A' + centroid_B';
  detr=det(R)

存在的一些問題：

  轉換模型精度與公共控制點的數量的關係值得探討；目前座標轉換精度的評價方式通常是利用轉站前後公共點座標差值的均方根值來進行評價。該方法只能獲得用於計算的公共點座標轉換精度，並不能得到空間中任意被測點的轉換精度分佈情況，對於提高整體測量精度以及改進公共點佈設方案缺乏指導意義。

主要參考來源：

[1]. 劉猛奎, 趙明金, 石波, 等. 基於 RANSAC 的座標系轉換抗差演算法研究[J]. 全球定位系統, 2019, 44(1): 39-47.

[2].韓夢澤, 李克昭. 基於羅德里格矩陣的空間座標轉換[J]. 測繪工程, 2016 (4): 25-27.

[3].張皓琳, 林嘉睿, 邾繼貴. 三維座標轉換精度及其影響因素的研究[J]. 光電工程, 2012, 39(10): 26-31.

[4].https://proj.org/operations/transformations/index.html；