视觉SLAM（四）：视觉里程计——特征点法

最新推荐文章于 2024-04-02 11:08:52 发布

Starry丶

最新推荐文章于 2024-04-02 11:08:52 发布

阅读量1.4k

点赞数 3

分类专栏：控制理论文章标签： slam 自动驾驶

本文链接：https://blog.csdn.net/Starry__/article/details/119879988

版权

控制理论专栏收录该内容

11 篇文章 8 订阅

订阅专栏

1. 特征提取

此处特征点指图像中特别的区域，并且相邻间图像特征点在诸如旋转、光照、尺度缩放等等影响下不发生明显变化，可称为不变性。例如角点、边缘点、暗区域的亮点以及亮区域的暗点等等，可通过像素信息筛选出来。

例如下图长方体的角点，已由黄色圆圈圈出：
在这里插入图片描述

特征点包括关键点和描述子两部分：

● 关键点：特征点在图像中的位置、大小、朝向等信息。 例如Harris关键点、FAST关键点、GFTT关键点等等

● 描述子：用于描述关键点的信息，通常为向量。 描述子例如BRIEF等等。

还有既包含关键点又包含描述子的例如SIFT、SURF、ORB等等，有很多。

1.1. 尺度不变特征变换（Scale Invariant Feature Transform, SIFT）

SIFT是一种经典的图像特征，具有尺度不变性。SIFT在面对图像旋转、尺度缩放、亮度变化具有不变性，对于视角变化、仿射变换、噪声也具有一定的容忍度。

这样高不变性和高容忍度的SIFT代价就是需要较大的计算量。

可参考非常详细的sift算法原理解析
 SIFT算法原理

关键点：

描述子：

1.2. Oriented FAST and Rotated BRIEF（ORB）

ORB是一种非常简单的图像特征，其特征提取非常快速。关键点是从原FAST改进而来，描述子也是来源于BRIEF，其速度比SIFT高出近百倍，但同时牺牲了一些精度和准确性。

关键点：oriented FAST

是在原版的FAST中加入了方向性特征，以增加旋转不变性。

核心思想如下图所示，概括来说就是，对于图像中的任意一个像素，若该点像素亮度与周围像素亮度差别过大，则可认为是关键点or角点。
在这里插入图片描述
以像素 $p$ 为例，且该点的亮度记为 $I_p$ ，关键点的提取步骤如下，当然可以根据速度和精度要求适当减少、增加、修改步骤：

1. 预测试
检测 $p$ 周围半径为3像素的标为1、5、9、13四个点的亮度。

若至少有三个点亮度与 $I_p$ 差距 > 某个阈值 $T$ ，则继续，否则不是角点直接pass。

count = 0;
for(i = 1,5,9,13)
	if abs(Ip-Ii)>T 
		count++;
if count<3
	p is not corner.

2. 粗提取
继续判断 $p$ 与其他点的亮度差距是否超过阈值 $T$ 。

若 超过 $N$ 个点满足上述条件（则为FAST-N，常见的N可取9、11、12），则认为 $p$ 为角点。

for(i = 1:16 and i != 1,5,9,13)
	if abs(Ip-Ii)>T 
		count++;
		
if count < N
	p is not corner.

3. 非极大值抑制（Non-Maximal Suppression, NMS）

经过2. 粗提取之后可筛选出很多角点，但为了防止角点过于集中，需要NMS删除部分角点。效果可参考下图：

在这里插入图片描述

方法是先为每个角点定义响应值，可选为 $I_p$ 与周围16个点的亮度偏差的绝对值的和。

再设置一个另一个半径R，所有 $p$ 周围一定半径内只保留响应值最大的角点。

之后还可以简单地只选择有限的几个响应值最大的几个角点。

for(p in Keypoints)
	p.response = sum^16_{i=1}{abs(I_p - I_i)};		//求响应值
	
for(p in Keypoints and p_next is next corner)
	if abs(p - p_next) < R 
		p = (p.response > p_next.response)? p: p_next;
		Keypoints.erase(p_next);

4. 尺度不变性

首先尺度可以理解成分辨率的概念，尺度大与小指的是图像分辨率的增加与降低。例如下面这几张图，就是不同的尺度下的猫咪，每个猫咪的分辨率不同。而尺度不变性则是指，特征点在不同尺度下的特征保持不变，即放大or缩小这东西不变形。例如下面几个图中猫咪的脑袋都是圆的，即猫咪的脑袋形状具有尺度不变性。

在这里插入图片描述

在这里插入图片描述
而如上图这样将同一个图片在逐级降低分辨率（下采样）并呈现，构成的就是图像金字塔。

在这里插入图片描述

通过设定金字塔层数（OpenCV默认为8）和缩放比率（OpenCV默认为1.2），然后所有层图片的共同特征点，满足尺度不变性。

5. 旋转不变性

采用灰度质心法，将特征点到标号1-16构成圆的质心构成的向量作为特征点的方向，方向 $\theta$ 计算公式为

$\theta = \arctan(\frac{\sum_{x,y \in 圆} {y·I(x,y)} } { \sum_{x,y \in 圆} {x·I(x,y)} } )$

描述子：rotated BRIEF

在经典的BRIEF的基础上进一步地解决了噪声敏感问题，并加入了旋转不变性。

先使用积分图像进行平滑，再在特征点的31x31的邻域内产生256个or128个随机点对，构成2x256的坐标矩阵 $S$ ：

根据特征点的方向 $\theta$ 确定旋转矩阵 $R_{\theta}$ ，从而得到新的坐标矩阵 $S_{\theta}=R_{\theta}S$ ，即得到了新的随机点对。

$R_{\theta}=\left(\begin{array}{lcr} cos\theta & -sin\theta\\ sin\theta & cos\theta \\ \end{array}\right)$

每对的两个随机点再各产生5x5的邻域子窗口，通过比较两个子窗口的像素和大小形成该特征点的二进制向量

例如，若特征点 $p$ 在第 $i$ 次产生了一个随机点对 $u$ 和 $v$ ，则其子窗口的像素和分别为 $I (u)$ 和 $I (v)$ ，则特征点的二进制向量 $\tau$ 满足 $\tau(i)=(I(u)>I(v))?1:0$

相关参考：

Rublee E , Rabaud V , Konolige K , et al. ORB: an efficient alternative to SIFT or SURF[C]// IEEE International Conference on Computer Vision, ICCV 2011, Barcelona, Spain, November 6-13, 2011. IEEE, 2011.
FAST角点检测算法（二）- 非极大值抑制筛选fast特征点
ORB特征提取详解
特征，特征不变性，尺度空间与图像金字塔
ORB（FAST+BRIEF）特征提取与实现——特征点提取算法分析

2. 特征匹配

其实就是将当前看到的所有特征点 $X_t$ 与之前图像的所有特征点 $X_{t+1}$ 进行匹配。

但是需要注意图像局部特性导致的误匹配问题。
误匹配的话可采取多种策略，例如设定可接受的最高匹配汉明距离、汉明距离不得大于最小汉明距离的两倍等等。

下面介绍方法

2.1. 暴力匹配

很简单，依次取 $X_t$ 中的每个特征点与 $X_{t+1}$ 的特征点分别对比，可通过两个特征点的汉明距离测算。

根据BRIEF最终得到的描述子是256维的二进制向量，汉明距离就是计算两个向量中元素不相等的个数。

当然SLAM也是需要实时性的，而暴力匹配的 $O(n^2)$ 复杂度可能导致很大的运算量。

2.2. 近似最近邻（Approximate Nearest Neighbors, ANN）

ANN要解决的问题是从特定向量集合中寻找与目标向量最近的N个向量，是数据检索中的经典问题。

这个问题其实就是特征匹配的变种，当然也可以采用1.2.1.讲的暴力匹配法，显然暴力法耗时太长。

主要分为四类：

● 树方法，如 KD-tree，Ball-tree，Annoy

● 哈希方法，如 Local Sensitive Hashing (LSH)

● 矢量量化方法，如 Product Quantization (PQ)

● 近邻图方法，如 Hierarchical Navigable Small World (HNSW)

干货 | 一文读懂 ANN

3. 计算相机运动

在已知邻近的两幅图像的匹配特征点之后，就可以通过匹配点估算相机位姿的运动了。

下面给出几种场景和相应的求解方法

3.1. 2D-2D：对极几何

2D-2D问题指的是已知像素平面上的所有匹配点（均为2D），求解相机的运动。

如下图所示：

在这里插入图片描述
$P$ 为路标点， $O_1$ 为前一帧相机光心， $O_2$ 为后一帧相机光心， $I_1$ 与 $I_2$ 为两相机像素平面， $p_1$ 与 $p_2$ 为 $P$ 在两相机像素平面上的像素点。

已知： $P$ 像素坐标 $p_1(u_1,v_1)^T$ 和 $p_2(u_2,v_2)^T$ ，相机内参矩阵 $K$

求：相机从 $O_1$ 到 $O_2$ 的旋转矩阵 $R$ 和平移矩阵 $t$ ，以及 $P$ 的世界坐标

注意此处旋转矩阵 $R$ 和平移矩阵 $t$ 是省去了下标，应该是： $R_{21}$ 和 $t_{21}$

下面用对极几何法来求解该问题。

对极几何的思路是，将匹配点对的相机方程列写出来，之后进行推导改写成一个方程组的形式 $A x = b$ 或 $A x = 0$ ，其中的待求量 $x$ 就与 $R$ 和 $t$ 有关，最后再解 $R$ 与 $t$ 即可。

之后再根据相机内参求得目标点的世界三维坐标。

● 相机方程联立

设 $P$ 在 $O_1$ 的相机坐标为 $P_1(x_1,y_1,z_1)^T$ ，在 $O_2$ 的相机坐标为 $P_2(x_2,y_2,z_2)^T$ ，注意这两个相机坐标均未知。

则应该有如下关系：

参考视觉SLAM（二）：相机与图像中的从相机坐标系到像素坐标系的转换，有(1)和(2)成立：

$\left(\begin{array}{lcr} p_1 \\ 1 \end{array}\right) =K·\frac{1}{z_1}·P_1 \tag{1}$

$\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right) =K·\frac{1}{z_2}·P_2 =K·\frac{1}{z_2}·\left[ R·P_1 +t \right] \tag{2}$

将(1)算出 $P_1$ 代入(2)中，则有

$z_2·K^{-1}\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right) = R·z_1·K^{-1}·\left(\begin{array}{lcr} p_1 \\ 1 \end{array}\right) +t \tag{3}$

之后一般情况下可以进行变换得到本质矩阵，但如果很多特征点都在同一平面上，就可以得到单应矩阵，单应矩阵可使用更少的特征点组求解 $R$ 和 $t$ 。

本质矩阵

在(3)的基础上，左右同乘 $\left[ K^{-1}\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right)\right]^Tt^{-}$

此处 $t^{-}$ 为3维向量 $t$ 的反对称矩阵。对于任意3维向量 $v$ ，都有 $t^-v=t×v$ 成立。
并且
$t^{-}=\left(\begin{array}{lcr} 0 & -t(3) & t(2) \\ t(3) & 0 & -t(1) \\ -t(2) & t(1) & 0 \\ \end{array}\right)$

因此乘完之后式(3)左边为0，右边的 $+ t$ 也变成0，则有

$\left[ K^{-1}\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right)\right]^T·t^{-}· R·K^{-1} \left(\begin{array}{lcr} p_1 \\ 1 \end{array}\right) =c_2^TEc_1 \tag{4，重要}$

此处的 $E$ 就为本质矩阵，而式(4)就为对极约束，对其进行分解以算得 $t$ 和 $R$ 。

注意(4)中 $c_1$ 和 $c_2$ 为 $P$ 分别在 $O_1$ 和 $O_2$ 的 $Z$ 轴归一化相机坐标，即
$c_i=(\frac{x_i}{z_i},\frac{y_i}{z_i},1)^T=(c_{ix},c_{iy},1)^T,i=1,2$
以及(4)中的 $E=t^{-}·R$

单应矩阵

认为特征点都落于同一平面上。即对于相机 $O_1$ 满足

$n^TP_1+d=0 → -\frac{n^TP_1}{d}=1$

该式其实就是中学学过的 $A x + B y + C z + d = 0$ 的变体。

将平面方程代入(3)，并根据(1)则有

$\frac{z_2}{z_1} \left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right) = K· (R-\frac{tn^T}{d}) K^{-1}·\left(\begin{array}{lcr} p_1 \\ 1 \end{array}\right) =H·\left(\begin{array}{lcr} p_1 \\ 1 \end{array}\right) \tag{5，重要}$

此处的 $H$ 就为单应矩阵，对其进行分析以求得 $R$ 和 $t$ 。

● 求解本质矩阵 $E$

一般来讲采用线性变换法，将(4)中引入匹配好的特征点，并改写成方程组 $A x = 0$ 的形式，再求解 $E$ 。

直接线性变换法

$E$ 是一个3x3的矩阵，自由度为9。但注意到路标点 $P$ 为空间中任意一点，即式(4)中 $c_1$ 和 $c_2$ 为任意向量，因此 $E$ 的自由度为8。采用八点法求解，寻找八对匹配的特征点。

实际上，由 $E=t^{-}·R$ 可以算 $t$ 为平移矩阵自由度为3， $R$ 为旋转矩阵，自由度也为3，所以 $E$ 的自由度应为6，再根据(4)，自由度应为5。所以使用五个点就能算出 $E$ ，但是五点法需要用到非线性性质，比较复杂，此处用八点法。

重新写式(4)则有

$c_2^TEc_1=(c_{2x},c_{2y},1) \left(\begin{array}{lcr} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \\ \end{array}\right) \left(\begin{array}{lcr} c_{1x} \\ c_{1y}\\ 1 \end{array}\right) =(c_{2x}c_{1x},c_{2x}c_{1y},c_{2x},c_{2y}c_{1x},c_{2y}c_{1y},c_{2y},c_{1x},c_{1y},1) \left(\begin{array}{lcr} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \\ e_9 \\ \end{array}\right) =0 \tag{6}$

如(6)所示，需要将八对匹配特征点列写成(5)的矩阵形式，构成8x9的矩阵，即求解

$\left(\begin{array}{lcr} c^1_{2x}c^1_{1x},c^1_{2x}c^1_{1y},c^1_{2x},c^1_{2y}c^1_{1x},c^1_{2y}c^1_{1y},c^1_{2y},c^1_{1x},c^1_{1y},1 \\ c^2_{2x}c^2_{1x},c^2_{2x}c^2_{1y},c^2_{2x},c^2_{2y}c^2_{1x},c^2_{2y}c^2_{1y},c^2_{2y},c^2_{1x},c^2_{1y},1 \\ ...... \\ c^8_{2x}c^8_{1x},c^8_{2x}c^8_{1y},c^8_{2x},c^8_{2y}c^8_{1x},c^8_{2y}c^8_{1y},c^8_{2y},c^8_{1x},c^8_{1y},1 \end{array}\right) \left(\begin{array}{lcr} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \\ e_9 \\ \end{array}\right) =0 \tag{7}$

通过(7)即可解出 $E$ ，当然 $E$ 具有尺度等价性，就是说存在一个非零基。

别忘记(6)中各元素的含义！ $e_2$ 表示本质矩阵 $E$ 中的第一行第二列的元素。
而诸如 $c_{2y}^5$ 则表示第5组匹配特征点的 $O_2$ 相机坐标中的 $\frac{y}{z}$

一般来说相邻帧图像匹配的特征点组个数会远多于8，可以将所有的特征点组写成类似式(6)的样式，构成了一个方程个数>变量个数的超定方程。
例如匹配的组数为79，则有方程 $A_{79×9}·e=0$ ，这样的超定方程无解，可通过最小二乘求解，即 $min_{e}(Ae)^T(Ae)$

随机采样一致性（Random Sample Consensus, RANSAC）

RANSAC与最小二乘的区别在于，最小二乘是将所有的样本均纳入参考，如 $min_{A}||Y-AX||_2^2$ 是将所有的点代入 $X$ 与 $Y$ ，求 $A$ 。

而RANSAC的思想则是将数据点分为局内点和局外点，只有局内点参与求解，局外点被认为是不适用模型的样本点，同时会使用概率来衡量拟合的好坏。

在这里插入图片描述

1. 算法介绍

大致的思路是：先随机假设一些点为局内点，并构建一个模型，之后分析其他点是不是适合于这个模型，若是则将其纳入到局内点。直到遍历了所有的点，然后根据所得的局内点重新构建一个新模型，并估计局内点与该模型的错误率。之后根据该模型的局内点数目和错误率，与已有的最优模型比较，判断是否替换。将上述流程循环固定次数，得到结果。

下面给出详细的算法伪代码：

RANSAC(data, model, minNum, maxIterations, threshold){
// 输入：
// data：数据集
// model：要拟合的模型
// minNum：最小局内点数目
// maxIterations：迭代次数
// threshold：决定数据点是否适用于模型的阈值

best_model = NULL;		//最优模型参数
best_innerPoints = NULL;	//最优模型的局内点集合

while(k < maxIterations)
{
	innerPoints = RandomSelect(data, minNum);		//随机选局内点
	maybe_model = Match(model,innerPoints);			//根据所选的局内点拟合出一个模型
	
	for(each points from data and not innerPoints )		// 对于非局内点判断是否适用于maybe_model
	{
		if( isMatched(points,maybe_model) < threshold)
			innerPoints.add(points);
	}

	maybe_model = Match(model,innerPoints);			//重新拟合模型

	if(isBetter(maybe_model,innerPoints,best_model,best_innerPoints) == 1)	//判断是否比已有模型更优
	{
		best_model = maybe_model;
		best_innerPoints = innerPoints;
	}
	k++;
}

return best_model,best_innerPoints;
}

实际上RANSAC 优点在于，能够在很多局外点时准确估计模型。

但是缺点也比较明显，首先每次迭代都要随机选择一些点直接作为局内点，如果随机选择的过多就很容易偏离最优解，然后迭代次数没有上限，只能说迭代越多得到模型越接近最优，还有就是需要设定错误阈值。

参数选择方面minNum和threshold是根据具体问题和模型来确定的，只有迭代次数 $k$ 是只与算法有关的参数。
若存在最优模型参数和其对应的局内点集 $S$
定义 $p$ 为 $k$ 次迭代至少有一次随机选择的点均在 $S$ 中
定义 $w=\frac{局内点个数}{数据点个数}$
定义 $n$ 为每次迭代要随机选择的局内点个数
则有
$1-p=(1-w^n)^k → k=\frac{lg(1-p)}{lg(1-w^n)}$
根据上面这个式子，可以设定一个 $p$ ，例如0.995，估计一个 $w$ ，然后算出迭代的次数 $k$ 。
同时也可以在RANSAC的while循环中，每次更新一个新的k

2. 应用：计算基础矩阵、本质矩阵和单应矩阵

RANSAC也可用于滤除误匹配点，与分析特征点的描述子不同，RANSAC并不是去检查各特征点对所对应的图像是不是相同的，而是基于大部分的匹配点对是正确匹配的，只有小部分才是错误匹配的假设，这一点与RANSAC努力拟合出适合更多局内点的模型是一致的。

对于本质矩阵而言，就可先随机选取8个点对作为初始局内点，根据(7)计算出一个本质矩阵 $E$ 。

再根据投影误差测试其他的点对，扩充局内点。

之后满足阈值的就为局内点，匹配正确的点。局外点就为错误匹配的点。

之后将所有的局内点列写成(6)的最小二乘的形式，重新拟合一个更好的 $E$ 。
在这里插入图片描述

RANSAC算法讲解
 SLAM之解析RANSAC算法

● 求解单应矩阵 $H$

单应矩阵也存在相应的线性变化法，但若面对可能存在误匹配的点时，通常采用随机采样一致性的方法。

直接线性变换法

将(5)重新写成

$\left(\begin{array}{lcr} u_2 \\ v_2 \\ 1 \end{array}\right) =\frac{z_1}{z_2} \left(\begin{array}{lcr} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \\ \end{array}\right)· \left(\begin{array}{lcr} u_1 \\ v_1 \\ 1 \end{array}\right) =\frac{z_1·h_9}{z_2} \left(\begin{array}{lcr} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & 1 \\ \end{array}\right)· \left(\begin{array}{lcr} u_1 \\ v_1 \\ 1 \end{array}\right)$

上式将 $\frac{h_1}{h_9}$ 重新定义为 $h_1$ 。
是归一化步骤，就好比是原来是 $\left(\begin{array}{lcr} 1 & 2 \\ 4 & 5 \\ \end{array}\right)$ 然后变成 $\left(\begin{array}{lcr} 0.2 & 0.4 \\ 0.8 & 1 \\ \end{array}\right)$ 。

之后解出 $\frac{z_1·h_9}{z_2}$ 的表达式，并消除这个未知的非零因子，得到：

$h_1u_1+h_2v_1+h_3-h_7u_1u_2-h_8v_1v_2=u_2 \\ h_4u_1+h_5v_1+h_6-h_7u_1u_2-h_8v_1v_2=v_2 \\ \left(\begin{array}{lcr} u_1,v_1,1,0,0,0,-u_1u_2,-v_1v_2 \\ 0,0,0,u_1,v_1,1,-u_1u_2,-v_1v_2 \\ \end{array}\right) \left(\begin{array}{lcr} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \\ \end{array}\right) =\left(\begin{array}{lcr} u_2 \\ v_2 \\ \end{array}\right) \tag{8}$

从(8)可以看出，一对匹配点对就能构造出两个约束，所以解出 $H$ 只需要4对匹配点对即可解出，这比本质矩阵要效率高很多，即式(9)

$\left(\begin{array}{lcr} u_1^1,v_1^1,1,0,0,0,-u_1^1u_2^1,-v_1^1v_2^1 \\ 0,0,0,u_1^1,v_1^1,1,-u_1^1u_2^1,-v_1^1v_2^1 \\ u_1^2,v_1^2,1,0,0,0,-u_1^2u_2^2,-v_1^2v_2^2 \\ 0,0,0,u_1^2,v_1^2,1,-u_1^2u_2^2,-v_1^2v_2^2 \\ u_1^3,v_1^3,1,0,0,0,-u_1^3u_2^3,-v_1^3v_2^3 \\ 0,0,0,u_1^3,v_1^3,1,-u_1^3u_2^3,-v_1^3v_2^3 \\ u_1^4,v_1^4,1,0,0,0,-u_1^4u_2^4,-v_1^4v_2^4 \\ 0,0,0,u_1^4,v_1^4,1,-u_1^4u_2^4,-v_1^4v_2^4 \\ \end{array}\right) \left(\begin{array}{lcr} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \\ \end{array}\right) =\left(\begin{array}{lcr} u_2^1 \\ v_2^1 \\ u_2^2 \\ v_2^2 \\ u_2^3 \\ v_2^3 \\ u_2^4 \\ v_2^4 \\ \end{array}\right) \tag{9}$

诸如 $u_{2}^3$ 则表示第3组匹配特征点的 $O_2$ 像素坐标中的 $u$

RANSAC

应用方面与计算本质矩阵是一样的。

对于单应矩阵，就可先随机选取4个不共线的点对作为初始局内点，根据(9)计算出一个单应矩阵 $H$ 。再根据如下的投影误差测试其他的点对，其实就是(8)写成了差的形式。

$(u_2 - \frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+1})^2+ (v_2 - \frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+1})^2 < threshold$

之后满足阈值的就为局内点，匹配正确的点。局外点就为错误匹配的点。

● 本质矩阵 $E$ 与单应矩阵 $H$ 的分解——得出 $R$ 与 $t$

求得了本质矩阵 $E$ 与单应矩阵 $H$ ，就可以根据(4)和(5)对其进行分解以获得相机旋转矩阵 $R$ 和平移矩阵 $t$ 。

分解的方法包括数值法和解析法，并且最终会解得4组 $R$ 与 $t$ ，然后分别用于计算 $P_1$ 和 $P_2$ 的深度，正的深度就为正解

下面介绍一下：对本质矩阵 $E$ 进行奇异值分解来获取 $R$ 和 $t$ 的过程。

之后通过对 $E$ 进行奇异值分解得到 $t$ 与 $R$ 的可行解，若矩阵 $E$ 的奇异值分解为 $\Sigma V^T=t^{-}R$ ，则可能的解有：

$t^{-}=U R_Z(±\frac{\pi}{2})\Sigma U^T，R=U R^T_Z(±\frac{\pi}{2})\Sigma V^T$

之后需要解出可能的 $P$ 相机坐标，产生正深度的解才是可行的 $R$ 和 $t$ 。

还需要说明的是， $E$ 具有尺度不变性，即对于任意实数 $k$ 均有(4)中 $c_2^T(kE)c_1=0$ 。
而旋转矩阵 $\in SO(3)=\left\{ R \in \mathbb{R^{3×3}} | RR^T=I, det(R)=1 \right\}$ ，所以 $R$ 不具有尺度不变性。

所以 $t$ 具有尺度不变性，就是说 $\forall k \in \mathbb{R}， kt$ 都是可行解，单位与相机坐标单位一致
即单目相机中保证相同的旋转而改变尺度依旧会得到相同的图像，如下图所示

我们已知 $P$ 在两个相机的像素坐标，所以单目相机就会导致解出来的尺度是不确定的，即 $O_2$ 的平移是不确定的，这其实就是2D-2D解法的尺度不确定性问题，那该如何解决呢？
通常的方法是对t归一化，即令其模长为1，以给出一个确定值，但是肯定是不准确的，实际平移长度不可能恰好为1m，是存在一个未知的倍数关系的。

由此可见，如果只用2D-2D作视觉里程计，这种平移带来的尺度不确定性将永远存在，这时对 $t$ 的估计、 $P$ 相机坐标的获取偏差将越来越大。所以对极几何法一般仅仅是用于去初始化相机，以得到一个近似的P三维相机坐标，之后求解3D-2D问题，以消除尺度不确定性带来的累积误差。
.
或者是在初始化时，手动使相机平移1m使 $P$ 更加准确的确定。
.
还有一点就是 $t$ 不能为零，初始化时相机必须作一定的平移。

● 三角测量——得出 $P$ 世界坐标

实际上最后会解得到四组 $R$ 和 $t$ ，此时需要解出 $P$ 的相机坐标来判断深度是否为正值，再做确定，之后再求世界坐标。

在(3)左右同乘以一个 $\left[ K^{-1}\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right) \right]^-$ 则有

$=z_1· \left[ K^{-1}\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right) \right]^-·R·K^{-1}·\left(\begin{array}{lcr} p_1 \\ 1 \end{array}\right) +\left[ K^{-1}\left(\begin{array}{lcr} p_2 \\ 1 \end{array}\right) \right]^-t \tag{10}$

根据(10)即可求出 $z_1$ ，再根据(3)即可求出 $z_2$ 。

就是这个流程求出所有匹配点的深度，但是由于噪声的存在，(3)可能不是准确的等于，因此可以用最小二乘来求解。

当然别忘了四组 $R$ 和 $t$ ，选出全部都为正深度的那个解。

三角测量法能够算得匹配点的深度，但是相机的平移对于三角测量法的使用会产生影响，即三角测量的矛盾
如果平移距离较长，则可能导致相机中物体外观发生明显变化，使得特征提取和匹配变得困难，但是这种情况下的三角测量会比较准确。
如果平移距离较短，则视线角度稍微有点变化，就会导致偏离 $P$ 很远。

如上图所示，同样是偏转 $\delta\theta$ ， $P$ 的偏差显然是 $O_2$ 距离 $O_1$ 越远偏的越小，见 $P^{'}$ 和 $P^{''}$

3.2. 3D-3D：迭代最近点（Iterative Closest Point, ICP）

迭代最近点（Iterative Closest Point, ICP）

3.3. 3D-2D：n点透视（Perspective-n-Points, PnP）

n点透视（Perspective-n-Points, PnP）

Starry丶

关注

3
点赞
踩
24

收藏

觉得还不错? 一键收藏
打赏
0
评论
视觉SLAM（四）：视觉里程计——特征点法

目录1. 特征点法1.1. 特征提取1.2. 特征匹配1.3. 计算相机运动三级目录2. 直接法在SLAM中前端视觉里程计（Visual Odometry, VO）要完成的是通过摄像头相邻的两帧图像变化估计相机的运动，并恢复空间结构。有两大类算法，分别是特征点法和直接法。1. 特征点法特征点法的含义是：通过分析相邻图像中特征点的变化来估计相机的运动。1.1. 特征提取此处特征点指图像中特别的区域，并且相邻间图像特征点在诸如旋转、光照、尺度缩放等等影响下不发生明显变化。例如角点、边缘点、暗区域的亮
复制链接

扫一扫