本文按照以下顺序讨论:从本质矩阵恢复运动与尺度问题的来源
进一步讨论尺度
1.从本质矩阵恢复运动与尺度问题的来源
设
和
是两帧之间的匹配像素点的归一化平面坐标,
是本质矩阵(Essential Matrix)。则有
将(1)展开得
将矩阵相乘的形式拆开得到
可以发现,对方程左边进行任意缩放都不会影响方程的解,这就是尺度问题的来由。所以
虽然有9个未知数,但是有一个变量可以看做是缩放因子,因此实际只有8个未知量(5个自由度),所以可以用八点法(五点不好算)构建方程组(4)来求解。
求解完线性方程组(4)后,我们需要从
中恢复相机的运动
和
。这个过程可以用奇异值分解(SVD)完成。设
的SVD为
对于任意一个
,可以得到两个可能的
和
与之对应
其中,
是U的最后一列,如果尺度不定时将模长归一化为1,即
。而
。图1 从本质矩阵恢复得到的四组解
这样一共会产生四组解(如图1所示),需要经过两步筛选得到正确解:
1.筛选旋转矩阵,剔除反射矩阵。
这一步可以计算分解得到的旋转矩阵的行列式实现,因为旋转矩阵的行列式为1,而反射矩阵的行列式为-1。
在ORB-SLAM实现如下:
R1 = u*W*vt;
if(cv::determinant(R1)<0)
R1=-R1;
R2 = u*W.t()*vt;
if(cv::determinant(R2)<0)
R2=-R2;
在VINS-Mono中实现如下:
/* VINS-Mono实现 */
//本质矩阵svd分解得到4组Rt解,这里的R是R_21 decomposeE(E, R1, R2, t1, t2);
// 旋转矩阵的行列式为1,反射矩阵行列式为-1 // 将E改成-E后,SVD分解得到的结果中,U=-U,其余两项不变 if (determinant(R1) + 1.0 < 1e-09)
{
E = -E;
decomposeE(E, R1, R2, t1, t2);
}
2.筛选出路标点在两个相机中都具有正深度的解。
在VINS-Mono中实现如下:
//通过三角化得到的正深度比例选择Rt解 double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2;
2.进一步讨论尺度
从(3)可知,尺度问题来源于单目SLAM中路标点的深度具有不确定性(所以才反投影到归一化平面上),也就是说尺度缩放实际作用的是两个相机之间的平移缩放。图2 深度不确定引起相机之间的平移缩放从图中也可以看出,由于尺度scale的关系,不同的t,决定了以后计算点P的深度也是不同的,所以恢复的物体深度也是跟尺度scale有关的,这就是论文中常说的结构恢复structure reconstruction,只是恢复了物体的结构框架,而不是实际意义的物体尺寸。并且要十分注意,每两对图像计算E并恢复R,T时,他们的尺度都不是一样的,本来是同一点,在不同尺寸下,深度不一样了,这时候地图map它最头痛了,所以这个尺度需要统一。
那怎么统一尺度呢?
思路1:缩放不同的尺度进行统一。
思路2:因为尺度不一致是计算本质矩阵
引起的,另一种常用的思路就是避开本质矩阵的计算。
思路1容易理解,但思路2具体怎么实现呢?这就涉及到PnP(Perspective-n-Point)方法。Pnp是求解3D-2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时,如何估计相机的位姿
考虑投影方程
可以用最后一行将尺度因子
消去,这也是后续尺度与初始尺度统一的原因。得到两个约束:
为了简化表示,定义:
则有:
假设有N个特征点,则可以列出如下线性方程:
一共12维,因此最少可以通过6对匹配点求解(9)。当匹配点大于6时,可以使用SVD等方法对超定方程求最小二乘解。
但由于匹配误差等原因,两帧之间的位姿变换总会出现误差,当这些误差积累之后,便产生了尺度漂移。
在ORB-SLAM中,作者就是使用了PnP方法求解后续帧的位姿变换。
参考^《视觉SLAM十四讲 从理论到实践》
^《Multiple View Geometry in Computer Vision.2nd Edition》