slam特征点深度 svd_单目SLAM的本质矩阵与尺度

本文按照以下顺序讨论：从本质矩阵恢复运动与尺度问题的来源

进一步讨论尺度

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》