上一篇博客中我们使用了四元数法计算ICP. 本篇我们使用SVD计算ICP. 下面是<视觉slam十四讲>中的计算方法: 计算步骤如下: 我们看到,只要求出了两组点之间的旋转,平移是非常容易得到的,所以我们重点关注R的计算.展开关于R的误差项,得: 注意到第一项和R无关,第二项由于R'R=I,亦与R无关.因此,实际上优化目标函数变为: 接下来,我们介绍怎样通过SVD解出上述问题中最优的R,但关于最优性的证明较为复杂,感兴趣的读者请参考[50,51],为了解R,先定义矩阵: W是一个3*3的矩阵,…