视觉SLAM笔记(38) 3D-3D: ICP


1. 3D-3D 的位姿估计问题

假设有一组配对好的 3D 点
比如对两个 RGB-D 图像进行了匹配:P = {p1, …, pn},P’ = {p’1, …, p’n}
现在,想要找一个欧氏变换 R, t,使得:
在这里插入图片描述
这个问题可以用 迭代最近点(Iterative Closest Point, ICP)求解

在 3D-3D 位姿估计问题中,并没有出现相机模型
也就是说,仅考虑两组 3D 点之间的变换时,和相机并没有关系

因此,在激光 SLAM 中也会碰到 ICP
不过由于激光数据特征不够丰富,无从知道两个点集之间的匹配关系
只能认为距离最近的两个点为同一个,所以这个方法称为迭代最近点
而在视觉中,特征点提供了较好的匹配关系,所以整个问题就变得更简单了

在 RGB-D SLAM 中,可以用这种方式估计相机位姿
和 PnP 类似, ICP 的求解也分为两种方式:
利用线性代数的求解(主要是 SVD),以及利用非线性优化方式的求解(类似于 Bundle Adjustment)


2. SVD 方法

以 SVD 为代表的线性代数方法
根据前面描述的 ICP 问题,先定义第 ii 对点的误差项:
在这里插入图片描述
然后,构建最小二乘问题,求使误差平方和达到极小的 R, t:
在这里插入图片描述
下面来推导它的求解方法
首先,定义两组点的质心:
在这里插入图片描述
请注意质心是没有下标的
随后,在误差函数中,作如下的处理:
在这里插入图片描述

注意到交叉项部分中, (p(pi pR(p− p − R (pip))′ − p′)) 在求和之后是为零的

因此优化目标函数可以简化为:
在这里插入图片描述
仔细观察左右两项,发现左边只和旋转矩阵 R 相关,而右边既有 R 也有 t,但只和质心相关
只要获得了 R,令第二项为零就能得到 t

于是, ICP 可以分为以下三个步骤求解:

在这里插入图片描述
看到,只要求出了两组点之间的旋转,平移量是非常容易得到的
所以重点关注 R 的计算,展开关于 R 的误差项,得:
在这里插入图片描述
注意到第一项和 RR 无关,第二项由于 RRT R=IR = I,亦与 RR 无关
因此,实际上优化目标函数变为:
在这里插入图片描述
接下来,就是怎样通过 SVD 解出上述问题中最优的 R
但是关于最优性的证明较为复杂,就不详细叙述

为了解 R,先定义矩阵:
在这里插入图片描述

WW 是一个 3 × 3 的矩阵,对 WW 进行 SVD 分解,得:
在这里插入图片描述

其中, Σ 为奇异值组成的对角矩阵,对角线元素从大到小排列,而 U 和 V 为正交矩阵

当 W 满秩时, R 为:
在这里插入图片描述
解得 R 后,按下式求解 t 即可:
在这里插入图片描述


3. 非线性优化方法

求解 ICP 的另一种方式是使用非线性优化,以迭代的方式去找最优值
该方法和 视觉SLAM笔记(36) 3D-2D: PnP 非常相似
以李代数表达位姿时,目标函数可以写成:
在这里插入图片描述
单个误差项关于位姿导数已经在前面推导过了,使用李代数扰动模型即可:

在这里插入图片描述
于是,在非线性优化中只需不断迭代,就能找到极小值
而且, ICP问题存在唯一解或无穷多解的情况
在唯一解的情况下,只要能找到极小值解
那么这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况
这也意味着 ICP 求解可以任意选定初始值
这是已经匹配点时求解 ICP 的一大好处

需要说明的是,这里讲的 ICP,是指已经由图像特征给定了匹配的情况下,进行位姿估计的问题
在匹配已知的情况下,这个最小二乘问题实际上具有解析解,所以并没有必要进行迭代优化
ICP 的研究者们往往更加关心匹配未知的情况
不过,在 RGB-D SLAM 中,由于一个像素的深度数据可能测量不到
所以我们可以混合着使用 PnP 和 ICP 优化:对于深度已知的特征点,用建模它们的 3D-3D 误差
对于深度未知的特征点,则建模 3D-2D 的重投影误差
于是,可以将所有的误差放在同一个问题中考虑,使得求解更加方便


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(37) 求解 PnP
视觉SLAM笔记(36) 3D-2D: PnP
视觉SLAM笔记(35) 三角化求特征点的空间位置
视觉SLAM笔记(34) 三角测量
视觉SLAM笔记(33) 对极约束求解相机运动


发布了217 篇原创文章 · 获赞 292 · 访问量 289万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览