视觉slam十四讲 pdf_视觉SLAM十四讲|第7讲 视觉里程计1之ICP问题

RGB-D相机可以获得深度信息，对于3D-3D的位姿估计问题可以转变成迭代最近点(Iterative Closest Point, ICP)问题。在RGB-D SLAM中用ICP问题指代匹配好的两组点间的运动估计问题。

1. SVD 方法

对于两个3D-3D匹配点，用其中一个点

来估算另一个点的坐标，有

，而该点的观测坐标为

，定义误差为

。构建最小二乘问题有：

在求解时，定义图像1和图像2中所有点的质心分别为：

可以得到如下的推导：

最终目标函数化简为：

目标简化成两个平方项之和，可以分别对两个平方项进行求解，先求解左边得到R，带入到右边得到t。右边项只有为0时才能让目标函数最小，当求解得到R时，可以根据

计算t。将左边的平方项展开有：

其中

，所以只有最后一项对结果有影响，整理得

使用SVD对

进行分解，得到

，当W满秩时

.

2. 非线性优化方法

使用李代数表达位姿，迭代求解。目标函数为：

使用扰动模型，得到导数：

在RGB-D SLAM中，有时可能测不到深度，所以可以混用PnP和ICP优化。

[1] 《视觉SLAM十四讲从理论到实践》 高翔，张涛