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十四讲从理论到实践》 高翔,张涛