两组三维对应点/坐标系转换公式及简要推导

  需要实现的是将源点集变换到目标点集坐标系下。设目标点集共包含n个点，该点集为P={p₁,…,p_n}；源点集为P’={p₁’,…,p_n’}。设将目标点集变换到源点集的坐标系下的旋转矩阵为R，平移向量为t。使用最小二乘的方法来求其最优解R^*和t^*，其求解过程可以分为四步，如下所示：

1. 计算两组点的质心位置p，p’：
   
2. 计算每个点的去质心坐标：
   
3. 根据以下优化问题计算旋转矩阵：
   
4. 根据第3步的R^*计算t^*：