SVD法座標系轉換原理

SVD法是將在兩個不同座標系下的測量的多個點的座標值對構造成一個矩陣，對該矩陣進行奇異值分解得到姿態矩陣。
${}^1P = {}_1^2R{}^2P + {}_1^2T$
首先，使用兩組點構造出一個矩陣$H$
$H = \sum\limits_{i = 1}^m {\left( {\left( {{}^2{P_i} - {}^2\bar P} \right) \cdot {{\left( {{}^1{P_i} - {}^1\bar P} \right)}^T}} \right)}$
然後對$H$進行奇異值分解
$U \cdot \Delta \cdot {V^T} = SVD\left( H \right)$
然後計算$|V \cdot {U^T}|$的符號，如果大於0，則
${}_1^2R = V \cdot {U^T}$
如果小於0，把$V$的任意一列改變符號，仍然計算$V \cdot {U^T}$作爲${}_1^2R$。
在計算出${}_1^2R$後，代入
${}^1P = {}_1^2R{}^2P + {}_1^2T$
即可得到 ${}_1^2T$。
該方法的代碼實現可以參考筆者以前的博客
https://blog.csdn.net/iamqianrenzhan/article/details/103463932。