视觉SLAM ch7 计算相机运动(求位姿) |
您所在的位置:网站首页 › opencv光束法平差 › 视觉SLAM ch7 计算相机运动(求位姿) |
目录 计算相机运动 一、2D-2D:对极几何 二、三角测量 三、3D-2D:PnP 四、3D-3D:ICP 计算相机运动在已经有匹配好的点对基础上来估计相机的运动(其实就是求R和t) 根据相机不同的种类有以下几种情况: 1.相机为单目时,只有2D的像素坐标(缺少了深度信息),因此问题就是根据两组2D点估计相机运动,用对极几何解决。 2.相机为双目、RGB-D时,或者通过某种方式得到了距离信息,问题就是根据两组3D点估计相机运动,通常用ICP解决。 3.如果一组为3D,一组为2D,即我们得到了一些3D点和它们在相机的投影位置,也能估计相机运动,通过PnP解决。 一、2D-2D:对极几何对极约束的目的是估计相机运动(定位过程)。 1. 对极几何公式推导: 注意: x1 x2是归一化坐标 ,p1 p2是像素坐标。 总结:s可以理解为深度 结合书99页公式5.7, 深度Z * Puv = K P(相机坐标) 归一化的坐标= P / Z ,因此归一化坐标 = K^-1 Puv 也就是这里的 x1 = K^-1 p1. 根据公式相机位姿估计分为两步: 1.根据配对点的像素位置求E或F 2.根据E或F求出R和t。 E和F只差了相机内参,因此实际求解中求解形式简单的本质矩阵E。 2.本质矩阵的求解 本质矩阵是一个3x3 矩阵,有9个未知数。 本质矩阵的特殊结构: 1.本质矩阵E由对极约束定义而来,由于对极约束是等于0的约束,因此E乘以任意一个非零常数依然是E,即E在不同尺度下是等价的。 2.本质矩阵的内在性质:根据E = t^R,可以证明E的奇异值必定是[x,x,0]的形式 。 3.旋转平移各有三个自由度,t^R本来有6个自由度,但由于cE=E(尺度等价性),因此E只有5个自由度。 理论上可以用5对匹配点求解E,但是由于E的非线性强,估计比较困难,所以可以只用尺度等价性,用8对匹配点(3*3-1)来求解E,称为“八点法”。 单应矩阵: 所有的特征点落在同一个平面上(墙、地面),则可以通过单应进行运动估计。 问题讨论: 1.单目视觉的尺度不确定性 2.初始化的旋转问题:相机发生了纯选择会导致 t 为0,无法求解 R。因此,单目初始化不能只有旋转,必须有一定程度的平移。 3.多于8对点的情况 工程中给定的点数多于8对,可以计算一个最小二乘解。方程构成一个超定方程,可以通过最小化一个二次型来求。 4.在存在错误匹配的情况下使用随机抽样一致算法(Random Sample Concensus,RANSAC),可以处理带有错误匹配的数据。 RANSAC 随机采样一致,RANSAC主要解决样本中的外点问题,可以去除错误匹配,最多可处理50%的外点情况。 RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点,并用下述方法进行验证: N — 样本点个数, K — 求解模型需要最少的点的个数 1. 随机采样K个点 2. 对该K个点拟合模型 3. 计算其它点到拟合模型的距离,小于一定阈值,当作内点,统计内点个数 4. 重复M次,选择内点数最多的模型 5. 利用所有的内点重新估计模型(可选) 关于RANSAC算法的参考链接:(具体代码一般不用手写,OpenCV和PCL库中都有相应函数) 计算机视觉基本原理——RANSAC RANSAC算法介绍 2D-2D对极几何 二、三角测量对极几何只是求出相机在两个照片之间的运动,只是定位过程,而与建图无关。想要建图我们还要知道特征点的空间位置,得到深度信息,这就需要用到三角测量的方法。 问题讨论: 三角测量是通过平移得到的。 三角测量的矛盾可总结为:增大平移时,会导致匹配失败;减少平移量会导致三角化精度不够。这个问题称为“视差”。 三、3D-2D:PnPPnP是求解3D到2D点对运动的方法。 单目视觉里程计:必须先初始化,才能使用PnP; 双目或RGB-D视觉里程计:可以直接使用PnP估计相机运动。 问题描述:已知n个点的空间坐标和它们在相机像素平面的投影坐标,估计相机位姿。 在视觉SLAM中,PnP是最重要的姿态估计算法。因为对于双目相机与RGBD相机来说,可以直接得到特征点的空间坐标,进而直接使用PnP估计相机运动;对于单目相机来说,通过对极几何初始化之后,我们也得到了一些特征点的空间坐标,在之后的SLAM过程中,我们就可以持续使用PnP算法。 PnP求解方法:理论上最少可以用3对点求解PnP,称为P3P方法,除此以外还有DLT(直接线性变换)、EPnP(Efficient PnP)、UPnP。还可以使用非线性优化的方式,构建最小二乘问题并迭代求解,也就是光束法平差(BA)。 1.DTL直接线性变换 已知空间间坐标[X,Y,Z,1] 和归一化坐标(u1,v1,1),求相机的位姿R和t。 通过6对匹配点求解增广矩阵T = [ R | t ] ;多于6对时,使用SVD方法对超定方程求最小二乘解。 对于旋转矩阵R,必须针对DLT估计的T左边3x3的矩阵块,寻找一个最好的旋转矩阵对它进行近似(将3x3矩阵空间投影到SE(3)流形上),使用QR分解完成。 2.P3P 仅使用3对匹配点,当给定的匹配点对于3组时,难以利用更多的信息;如果3D或2D点受到噪声影响,或者存在误匹配,算法失效。 3.最小化重投影误差求解PnP DTL和P3P是线性方法,还可以把PnP问题构建成一个重投影误差的非线性最小二乘问题。 线性方法:先求相机位姿,再求空间点位置 非线性方法:把相机位姿和空间点位置看作优化变量,放在一起优化。 这一类把相机位姿和空间点放在一起进行最小化的问题,统称为BA。目标是最小化重投影误差,初值是可以是PnP或ICP给出的结果,如果相机是连续运动的(多数SLAM),可以不要初值,直接求解最优的相机位姿和路标点位置。 重投影误差定义:3D点投影到2D像素的位置 与 预测值的差。 使用BA求解,BA思想本身不难,难在应用在重投影中求导的地方(对误差项e求导)。直接对矩阵求导行不通,因为无论是变换矩阵还是旋转矩阵本身都有非常多的约束,再做求导只会越导越复杂。因此引入了李群与李代数,但是李群不满足加法,因此在李群上也没有导数的定义(ch4)故转而在李代数上利用(满足加法)导数的定义求矩阵的导数。使用李代数可以构建无约束的优化问题。所以难点在李代数求导(扰动模型:一般使用左扰动模型)。 总结:光束法平差BA,指把相机和三维点一起优化的方法。已知与3D点匹配的实际2D像素投影位置 关于重投影误差
ICP统指匹配好的两组点间运动估计问题。 ICP的求解过程和相机内参没有关系。 已知世界系下3D坐标、匹配的相机系下3D坐标 ---> 求解世界系、相机系间R,t(相机位姿) 问题描述: 第一帧中的特征点 书中这里是反过来看的,将第二帧的 得到公式: 误差 计算雅可比矩阵: 最后记得把算出的R和t 变回去(这里的R t是第二帧到第一帧) 线性求解SVD、非线性优化(BA)方法来计算: SVD。非线性优化。类似PnP的BA优化,存在唯一解(求极小值)或无穷多解的问题。ICP 是由图像特征给定了匹配的情况下进行位姿估计的问题。但现实中更注重匹配未知的情况,在RGD-D SLAM 中,一个像素的深度可能有,也可能测量不到,所以需要混合使用PnP和 ICP :对于深度已知的特征点,建模它们的3D-3D 误差;对于深度未知的特征点,建模它们的3D-2D重投影误差。将所有的误差放在同一个问题解决。 总结: ch7是这本书内容最多的一个章节,对视觉里程计进行了详细的介绍。重点介绍: 1.特征点是如何提取并匹配的 2.如何通过2D-2D的特征点估计相机运动(对极几何) 3.如果从2D-2D的匹配估计一个点的空间位置(三角测量) 4.3D-2D的PnP问题,线性解法(P3P、DLT、EPnP)和BA解法 5.3D-3D的ICP问题,线性解法(SVD)和BA解法 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |