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。