激光雷达标定外参时通过提取两个垂直平面的点云计算法向量可以标定到指定的坐标，其中用到了SVD分解的方法一直很困惑，查找资料后终于找到算法的出处

from Wikipedia

以Wolfgang Kabsch命名的Kabsch算法是一种计算最佳旋转矩阵的方法，它能使两组成对的点之间的RMSD（均方根偏差）最小。它在图形学、化学信息学中用于比较分子结构，也在生物信息学中用于比较蛋白质结构（特别是，见均方根偏差（生物信息学））。

该算法仅仅计算旋转矩阵，但是也需要计算平移向量。当平移和旋转都进行时，该算法有时被称为partial Procrustes superimposition（另见orthogonal Procrustes problem）。

描述

把$P$旋转为$Q$的算法起始于两组点对，$P$和$Q$。每组点可以用一个$N\times 3$矩阵表示。第一行为第一坐标系的第一个点，第二行为坐标系的第二个点，第$N$行为坐标系的第$N$个点，矩阵如下

$$ \begin{pmatrix} x_1&y_1&z_1\\ x_2&y_2&z_2\\ \vdots&\vdots&\vdots\\ x_N&y_N&z_N\\ \end{pmatrix} $$

算法有三步：平移，计算协方差矩阵，和计算最佳旋转矩阵

变换

两组坐标系首先都需要进行平移，使得他们的重心为坐标系统原点

计算协方差

第二部计算矩阵$H$，

$$ H=P^TQ $$

或者使用加法形式：

$$ H_{ij}=\sum_{k=1}^{N}{P_{ki}Q_{kj}} $$

当P和Q被视为数据矩阵时，它是一个交叉协方差矩阵。

计算最佳旋转矩阵

有可能根据矩阵公式计算出最佳旋转$R$

$$ R=(H^TH)^{\frac{1}{2}}H^{-1} $$

但如果考虑到所有的特殊情况（例如，$H$没有逆的情况），实现这个公式的数字解就变得很复杂。

如果可以异值分解（SVD）的方法，可以用下面的简单算法来计算最佳旋转矩阵$R$。

首先，计算协方差矩阵$H$的SVD

$$ H=U\Sigma V^T $$

然后，决定我们是否需要修正我们的旋转矩阵，以确保一个右旋的坐标系

$$ d=\text{sign}\left(\det(VU^T)\right) $$

最后，计算最优旋转矩阵$R$

$$ R=V\begin{pmatrix} 1&0&0\\ 0&1&0\\ 0&0&d \end{pmatrix} $$

最佳旋转矩阵也可以用四元数来表示。这种替代性描述最近被用于开发一种严格的方法，从柔性分子的分子动力学轨迹中去除刚体运动。在2002年，还提出了一个应用于概率分布（连续或不连续）的一般化方法。

总结

该算法是针对三维空间中的点而描述的。对D维的推广是直接的。

额外链接

SVD详细描述：http://cnx.org/content/m11608/latest/

MATLAB函数：http://www.mathworks.com/matlabcentral/fileexchange/25746-kabsch-algorithm

C++ Eigen实现：https://github.com/oleg-alexandrov/projects/blob/master/eigen/Kabsch.cpp

扩展

支持求平移的最小二乘方法：Least-Squares Rigid Motion Using SVD