求解空间两个三维坐标系之间的变换矩阵

三维刚体变换模型（即旋转/平移矩阵，RT矩阵）的估计方法

原理简单阐述：
只要算出变换矩阵，就可以算出A坐标系的一个点P在坐标系B里的对应点坐标，即
T为3x3的转换矩阵， t 为3x1的位移变换向量，这里点坐标均为3x1的列向量（非齐次形式，齐次形式下为4x1列向量，多出的一个元素值补1而已）。
理论上只要给定至少3对点，就能计算出 T 和 t 。自然的，点对越多，计算出来的转换就越精确。
详细的原理请参考《Estimating 3-D Rigid Body Transformations: A Comparison of Four Major Algorithms》，它使用SVD方法计算 T 和 t 。

下面就话不多说，上代码。亲写亲测有效。

cv