视觉SLAM十四讲笔记-7-4

四夕小一冰

已于 2022-05-22 16:39:53 修改

阅读量378

点赞数

分类专栏：视觉slam十四讲笔记文章标签：经验分享

于 2022-05-18 20:10:57 首次发布

本文链接：https://blog.csdn.net/qq_35588369/article/details/124823863

版权

视觉slam十四讲笔记专栏收录该内容

34 篇文章 10 订阅

订阅专栏

视觉SLAM十四讲笔记-7-4

文章目录

视觉SLAM十四讲笔记-7-4

7.7 3D-2D:PnP

PnP(Perspective-n-point) 是求解 3D 到 2D 点对运动的方法。它描述了当知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。2D-2D的对极几何方法需要 8 个或者 8 个以上的点对，且存在初始化、纯旋转和尺度的问题。然而，如果两张图像中的一张特征点的 3D 位置已知，那么最少需要 3 个点对(以及一个额外点验证结果)就可以估计相机运动。特征点的 3D 位置可以有三角化或者 RGB-D 相机的深度图像确定。
在双目相机或者 RGB-D 的视觉里程计中，可以直接使用 PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，才能使用 PnP。
3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计。
PnP 问题有很多种求解方法。例如，用 3 对点估计位姿的 P3P，直接线性变换(DLT)，EPnP(Efficient PnP)，UPnP等。此外，还能用非线性优化的方法，构建最小二乘问题并迭代求解，也就是万金油的光束法平差(Bundle Adjustment, BA)。
下面先来介绍直接线性变换法(DLT)。

7.7.1 直接线性变换

已知一组 3D 点的位置，以及它们在某个相机中的投影位置，求该相机的位姿。这个问题也可以用于求解给定地图和图像时的相机状态问题。如果把 3D 点看成在另一个相机坐标系中的点的话，则也可以用于求解两个相机的相对运动问题。
考虑某个空间点 $P$ ，它的齐次坐标为 $P=[X,Y,Z,1]^T$ 。在图像 $I_1$ 中，投影到特征点 $x_1 = (u_1,v_1,1)^T$ (以归一化平面齐次坐标表示)。此时，相机的位姿 $R, t$ 是未知的。与单应矩阵的求解类似，定义增广矩阵 $[R ∣ t]$ 为一个 3 * 4 的矩阵，包含了旋转和平移的信息。将其展开形式列些如下：
$s\begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} = \begin{pmatrix} t_1 & t_2 & t_3& t_4\\ t_5 & t_6 & t_7& t_8\\ t_9 & t_{10} & t_{11} &t_{12} \end{pmatrix}\begin{bmatrix} X\\ Y\\ Z\\ 1 \end{bmatrix}$
用最后一行把 $s$ 消去，得到两个向量：
$u_1 = \frac{t_1X+t_2Y+t_3Z+t_4}{t_9X+t_{10}Y+t_{11}Z+t_{12}}$
$v_1 = \frac{t_5X+t_6Y+t_7Z+t_8}{t_9X+t_{10}Y+t_{11}Z+t_{12}}$
为了简化表示，定义 $T$ 的行向量：
$t_1 = (t_1,t_2,t_3,t_4)^T，t_2 = (t_5,t_6,t_7,t_8)^T，t_3 = (t_9,t_{10},t_{11},t_{12})^T，$
于是有：
$t_1^TP - t_3^TPu_1 = 0，t_2^TP - t_3^TPv_1 = 0$
这里的 $t$ 为待求变量，可以看到，每个特征点提供了两个关于 $t$ 的线性约束。假设一共有
$N$ 个特征点，则可以列出如下线性方程组：
$\left(\begin{array}{ccc} \boldsymbol{P}_{1}^{T} & 0 & -u_{1} \boldsymbol{P}_{1}^{T} \\ 0 & \boldsymbol{P}_{1}^{T} & -v_{1} \boldsymbol{P}_{1}^{T} \\ \vdots & \vdots & \vdots \\ \boldsymbol{P}_{N}^{T} & 0 & -u_{N} \boldsymbol{P}_{N}^{T} \\ 0 & \boldsymbol{P}_{N}^{T} & -v_{N} \boldsymbol{P}_{N}^{T} \end{array}\right)\left(\begin{array}{l} \boldsymbol{t}_{1} \\ \boldsymbol{t}_{2} \\ \boldsymbol{t}_{3} \end{array}\right)=0$
$t$ 一共 12 维，因此可以通过 6 对匹配点即可实现矩阵 $T$ 的线性求解。这种方法称为 DLT。当匹配点大于 6 时，也可以用 SVD 等方法对超定方程求最小二乘解。
在DLT求解中，我们直接将 $T$ 矩阵看成12个未知数，忽略了它们之间的联系。因为旋转矩阵 R \in SO(3) ，用 DLT求出的解不一定满足该约束，它是一个一般矩阵。平移向量比较好办，它属于向量空间。对于旋转矩阵 $R$ ，我们必须针对 DLT 估计的 $T$ 左边的 3 * 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成，也可以像这样来计算：
$R\longleftarrow (RR^T)^{-\frac{1}{2} }R$
需要解释的是，我们这里的 $x_1$ 使用了归一化平面坐标，去掉了内参矩阵 $K$ 的影响，这是因为内参 $K$ 在 SLAM 中通常假设为已知的。即使内参未知，也能用 PnP 去估计 $K, R, t$ 三个量。然而由于未知量增多，效果会差一些。

7.7.2 P3P

P3P 仅使用三对匹配点，对数据要求较少，它的输入数据为三对 3D-2D 匹配点，此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 $D - d$ ，相机光心为 $O$ 。
记 $3 D$ 点为 $A, B, C$ ， $2 D$ 点为 $a, b, c$ ，(这里的 $A, B, C$ 是在世界坐标系中的坐标，而不是在相机坐标系下的坐标。)
在这里插入图片描述
首先，显然三角形之间存在对应关系：
$\triangle Oab - \triangle OAB，\triangle Obc - \triangle OBC，\triangle Oac - \triangle OAC$
来考虑 $O a b$ 和 $O A B$ 的关系。利用余弦定理，有：
$OA^2 + OB^2 - 2OA.OB.cos<a,b> = AB^2$
对于其他两个三角形也有类似的性质，于是有：
$OA^2 + OB^2 - 2OA.OB.cos<a,b> = AB^2 \\ OB^2 + OC^2 - 2OB.OC.cos<b,c> = BC^2 \\ OA^2 + OC^2 - 2OA.OC.cos<a,b> = AC^2$
对以上三式全体除以 $OC^2$ ，并且记 $x = O A / O C ， y = O B / O C$ ，得：
$x^2+y^2 - 2xycos<a,b> = AB^2 / OC^2 \\ y^2+1 - 2ycos<b,c> = BC^2 / OC^2\\ x^2+1 - 2ycos<a,c> = AC^2 / OC^2$
记 $v = AB^2 / OC^2，uv = BC^2 / OC^2，wv = AC^2/OC^2$ ，有：
$x^2+y^2 - 2xycos<a,b> - v = 0 \\ y^2+1 - 2ycos<b,c> - uv = 0\\ x^2+1 - 2ycos<a,c> - wv = 0$
把第一个式子中的 $v$ 放到等式一边，并代入其后两式，得：
$\begin{aligned} &(1-u) y^{2}-u x^{2}-\cos \langle b, c\rangle y+2 u x y \cos \langle a, b\rangle+1=0 \\ &(1-w) x^{2}-w y^{2}-\cos \langle a, c\rangle x+2 w x y \cos \langle a, b\rangle+1=0 \end{aligned}$
注意这些方程中的已知量和未知量。知道 2D 点的图像位置，3 个余弦角 $c o s < a, b >$ ， $c o s < b, c >$ ， $c o s < a, c >$ 是已知的。同时 $u = BC^2 / AB^2$ ， $w = AC^2 / AB^2$ 可以通过 $A, B, C$ 在世界坐标系下的坐标算出，坐标变到像极坐标系下之后，这个比值是不变的。该式中 $x, y$ 是未知的，随着相机的移动会发生变化。因此，该方程组是关于 $x, y$ 的一个二元二次方程(多项式方程)。求该方程组是一个很复杂的过程，需要用吴消元法。利用该方程得到 $A, B, C$ 在相机坐标系下的 3D 坐标。然后，根据 3D-3D 的点对，计算相机的运动 $R, t$ 。
从 P3P 的原理可以看出，为了求解 PnP，利用了三角形相似性质，求解投影点 $a, b, c$ 在相机坐标系下的 3D 坐标，最后将问题转换成一个 3D 到 3D 的位姿估计问题。
P3P也存在着一些问题：
1.P3P 只利用 3 个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
2.如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。

7.7.3 最小化重投影误差求解 PnP

除了使用线性方法，还可以把 PnP 问题构建成一个关于重投影误差的非线性最小二乘问题。前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。这是一种非常通用的方式，可以用它对 PnP 或 ICP 给出的结果进行优化。
这一类把相机位姿和三维点放在一起进行最小化的问题，统称为 Bundle Adjustment。
完全可以在 PnP 中构建一个 Bundle Adjustment 问题对相机位姿进行优化。如果相机是连续运动的，也可以直接用 BA 求解相机位姿。在本节给出此问题在两个视图下的基本形式，然后在后续章节中讨论较大规模的 BA 问题。
考虑 $n$ 个三维空间点 $P$ 及其投影 $p$ ，希望计算相机的位姿 $R, t$ ，它的李群表示为 $T$ 。
设某空间点坐标为 $P_i = [X_i, Y_i, Z_i]^T$ ，其投影点为 $\boldsymbol{u}_i = [u_i, v_i]^T$ 。根据第五章的内容，像素位置与空间点位置的关系如下:
$s_{i}\left[\begin{array}{c} u_{i} \\ v_{i} \\ 1 \end{array}\right]=K T\left[\begin{array}{c} X_{i} \\ Y_{i} \\ Z_{i} \\ 1 \end{array}\right]$
写成矩阵的形式就是：
$s_i \boldsymbol{u}_i = KTP_i$
这个式子隐含了一次从齐次坐标到非其次的转换。否则按照矩阵的乘法来说，维度是不对的。现在，由于相机位姿未知及观测点的噪声，该等式存在一个误差。因此，把误差求和，构建最小二乘问题，然后寻求最好的相机位姿，使其最小化：
$T^{*}=\arg \min _{T} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{u}_{i}-\frac{1}{s_{i}} K T P_{i}\right\|_{2}^{2}$
该问题的误差项，是将 3D 点的投影位置与观测位置作差，所以称为重投影误差。使用齐次坐标时，这个误差有 3 维。不过，由于 $\boldsymbol{u}$ 最后一维为 1，该维度的误差一直为零，因而更多时候使用非齐次坐标，于是误差就只有 2 维了。
图片来源：link
在这里插入图片描述
如上图所示，通过特征匹配知道了 $p_1$ 和 $p_2$ 是同一个空间点 $P$ 的投影，但是不知道相机的位姿。在初始值中， $P$ 的投影 $\hat{p_2}$ 与实际的 $p_2$ 之间有一定的距离。于是，通过调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后的效果也是整体误差的缩小，而每个点的误差通常都不会精确为零。
最小二乘问题在第六章介绍过了。使用李代数，可以构建无约束的优化问题。很方便地通过高斯牛顿法、列文伯格－马夸尔特方法等优化算法进行求解。不过，在使用高斯牛顿法和列文伯格-马夸尔特方法之前，需要知道每个误差项关于优化变量的导数，也就是线性化：
$e(x+\Delta x) \approx e(x) + J^T \Delta x$
当 $e$ 为像素坐标误差时(2维)， $x$ 为相机位姿(6维)时(变换矩阵的李代数形式)， $J^T$ 将是一个 2 * 6 的矩阵。下面来推导 $J^T$ 的形式：
回忆李代数的内容，介绍了如何使用扰动模型来求李代数的导数。首先，记变换到相机坐标系下的空间点坐标为 $P^{'}$ ，并且将其前 3 维取出来：
$P' = (TP)_{1:3} = [X',Y',Z']^T$
那么，相机投影模型相对于 $P^{'}$ 为：
$s\boldsymbol{u} = KP'$
展开：
$\left[\begin{array}{c} s u \\ s v \\ s \end{array}\right]=\left[\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} X^{\prime} \\ Y^{\prime} \\ Z^{\prime} \end{array}\right]$
利用第三行消去 $s$ (实际上就是 $P^{'}$ 的深度)，得：
$\begin{aligned} &u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x} \\ &v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y} \end{aligned}$
当求误差时，可以把这里的 $u$ , $v$ 与实际的测量值比较，求差。在定义了中间变量后，对 $T$ 左乘扰动量 $\delta \xi$ ，然后考虑 $e$ 的变化关于扰动量的导数。根据链式法则，可以列写如下：
$\frac{\partial \delta e}{\partial \delta \xi}=\lim _{\delta \xi \rightarrow 0} \frac{e(\delta \xi \oplus \xi)-e(\xi)}{\delta \xi}=\frac{\partial e}{\partial P^{\prime}} \frac{\partial P^{\prime}}{\partial \delta \xi}$
这里的 $\oplus$ 指李代数上的左乘扰动。第一项误差关于投影点的导数，根据上述像素坐标系下坐标与相机坐标系下坐标的对应关系可以看出变量之间的关系，易得：
$\frac{\partial e}{\partial P^{\prime}}=-\left[\begin{array}{lll} \frac{\partial u}{\partial X^{\prime}} & \frac{\partial u}{\partial Y^{\prime}} & \frac{\partial u}{\partial Z^{\prime}} \\ \frac{\partial v}{\partial X^{\prime}} & \frac{\partial v}{\partial Y^{\prime}} & \frac{\partial v}{\partial Z^{\prime}} \end{array}\right]=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right]$
(这里的负号并不是真正意义上的负号，为了代表观测值减去预测值这个意义而加上的)
而第二项为变换后关于李代数的导数。
$\frac{\partial(T P)}{\partial \delta \xi}=(T P)^{\odot}=\left[\begin{array}{cc} I & -P^{\prime \wedge} \\ \mathbf{0}^{T} & \mathbf{0}^{T} \end{array}\right]$
而在 $P^{'}$ 的定义中，取出了前 3 维，于是得：
$\frac{\partial P^{\prime}}{\partial \delta \xi}=\left[I,-P^{\prime \wedge}\right]=\left[\begin{array}{cccccc} 1 & 0 & 0 & 0 & Z^{\prime} & -Y^{\prime} \\ 0 & 1 & 0 & -Z^{\prime} & 0 & X^{\prime} \\ 0 & 0 & 1 & Y^{\prime} & -X^{\prime} & 0 \end{array}\right]$
将其两项相乘，就得到了 2 * 6 的雅克比矩阵：
$\begin{aligned} &\frac{\partial e}{\partial \delta \xi}\\ &=\frac{\partial e}{\partial P^{\prime}} \frac{\partial P^{\prime}}{\partial \delta \xi}\\ &=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right]\left[\begin{array}{cccccc} 1 & 0 & 0 & 0 & Z^{\prime} & -Y^{\prime} \\ 0 & 1 & 0 & -Z^{\prime} & 0 & X^{\prime} \\ 0 & 0 & 1 & Y^{\prime} & -X^{\prime} & 0 \end{array}\right]\\ &=-\left[\begin{array}{cccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} & -\frac{f_{x} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_{x}+\frac{f_{x} X^{\prime 2}}{Z^{\prime 2}} & -\frac{f_{x} Y^{\prime}}{Z^{\prime}} \\ \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} & -f_{y}-\frac{f_{y} Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime}}{Z^{\prime}} \end{array}\right] \end{aligned}$
这个雅克比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。保留前面的负号，这时因为误差是由观测值减预测值定义的。
如果 $s e (3)$ 的定义方式是旋转在前，平移在后，则只要将这个矩阵的前 3 列和后 3 列对调即可。
除了优化位姿，还希望优化特征点的空间位置。因此，需要讨论一下 $e$ 关于空间点 $P$ 的导数，仍然使用链式法则：
$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{\prime}} \frac{\partial P^{\prime}}{\partial P}$
第一项等于：
$\frac{\partial e}{\partial P^{\prime}}=-\left[\begin{array}{lll} \frac{\partial u}{\partial X^{\prime}} & \frac{\partial u}{\partial Y^{\prime}} & \frac{\partial u}{\partial Z^{\prime}} \\ \frac{\partial v}{\partial X^{\prime}} & \frac{\partial v}{\partial Y^{\prime}} & \frac{\partial v}{\partial Z^{\prime}} \end{array}\right]=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right]$
第二项等于：
$P' = (TP)_{1:3} = RP+t$
发现 $P^{'}$ 对 $P$ 求导将只剩下旋转矩阵 $R$ 。于是就有：
$\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}}=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] \boldsymbol{R}$
上述推到了观测相机方程关于相机位姿与特征点的两个导数矩阵。这两个矩阵十分重要，能够在优化问题中提供重要的梯度方向，指导优化的迭代。

7.8 实践：求解PnP

7.8.1 使用 OpenCV 提供的 PnP 函数求解位姿

下面通过实验理解 PnP 的过程。首先演示如何使用 OpenCV 的 EPnP 求解 PnP 问题，然后通过非线性优化再次求解。
由于 PnP 需要使用 3D 点，为了避免初始化带来的麻烦，使用了 RGB-D 相机中的深度图 (1_depth.png) 作为特征点的 3D 位置。

mkdir pose_estimation_3d2d
cd pose_estimation_3d2d/
code .

//launch.json
{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "g++ - 生成和调试活动文件",
            "type": "cppdbg",
            "request":"launch",
            "program":"${workspaceFolder}/build/pose_estimation_3d2d",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启动整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "Build",
            "miDebuggerPath": "/usr/bin/gdb"
        }
    ]
}

//tasks.json
{
	"version": "2.0.0",
	"options":{
		"cwd": "${workspaceFolder}/build"   //指明在哪个文件夹下做下面这些指令
	},
	"tasks": [
		{
			"type": "shell",
			"label": "cmake",   //label就是这个task的名字,这个task的名字叫cmake
			"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
			"args":[
				".."
			]
		},
		{
			"label": "make",  //这个task的名字叫make
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"command": "make",  //这个task要执行的任务是make
			"args": [

			]
		},
		{
			"label": "Build",
			"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
			"dependsOn":[				//这个label依赖于上面两个label
				"cmake",
				"make"
			]
		}
	]
}

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(G2OCURVEFITTING)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++14")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

list( APPEND CMAKE_MODULE_PATH /home/lss/Downloads/g2o/cmake_modules )
set(G2O_ROOT /usr/local/include/g2o)

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )

# Eigen
include_directories("/usr/include/eigen3")

find_package (glog 0.6.0 REQUIRED)

#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(pose_estimation_3d2d pose_estimation_3d2d.cpp)

#链接OpenCV库,Ceres库，
target_link_libraries(pose_estimation_3d2d ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} glog::glog)
target_link_libraries(pose_estimation_3d2d Sophus::Sophus)

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <algorithm>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keypoints_1,
    std::vector<KeyPoint> &keypoints_2,
    std::vector<DMatch> &matches);

// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv)
{
    //
    // if(argc != 3)
    // {
    //     cout << "usage: feature_extraction img1 img2" << endl;
    //     return 1;
    // }
    // //---读取图像
    // Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    // Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    // assert(img_1.data != nullptr && img_2.data != nullptr);
    //
    //---读取图像
    Mat img_1 = imread("./1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("./2.png", CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;

    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;

    // 建立3D点
    Mat d1 = imread("./1_depth.png", CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts_3d; // 3d路标点
    vector<Point2f> pts_2d; // 2d像素点
    for (DMatch m : matches)
    {
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if (d == 0) // bad depth
            continue;
        float dd = d / 5000.0;
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd)); // img_1中提取的特征点对应的相机坐标系下3D点坐标
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);        // img_2中提取的特征点坐标
    }
    cout << "3d-2d pairs:" << pts_3d.size() << endl; //输出3d-2d pairs

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Mat r, t;                                                                                      // r表示旋转向量 t表示平移向量
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);                                               // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;                                                                                         //旋转矩阵R
    cv::Rodrigues(r, R);                                                                           //由罗德里格斯公式将旋转向量转换为旋转矩阵
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();                             //计时结束
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1); //计算耗时
    cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;         //输出使用opencv中使用pnp所花费的时间
    cout << "R=" << endl
         << R << endl;
    cout << "t=" << endl
         << t << endl;
    return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches)
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist)
            min_dist = dist;
        if (dist > max_dist)
            max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }
}

Point2f pixel2cam(const Point2d &p, const Mat &K)
{
    return Point2f(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

运行结果：
请添加图片描述
在该例子中，首先在第一幅图像中寻求深度，并求出它们在相机坐标系下的 3D 位置，以此作为 3D 空间位置。再以第二个图像的像素位置为 2D 点，然后调用 OpenCV 中的 PnP函数求解 PnP 问题。

7.8.2 手写位姿估计

在上面的 pose_estimation_3d2d.cpp 中继续实验，更改后的代码如下：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <algorithm>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keypoints_1,
    std::vector<KeyPoint> &keypoints_2,
    std::vector<DMatch> &matches);

// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundleAdjustmentGaussNewton(
    const VecVector3d &points_3d,
    const VecVector2d &points_2d,
    const Mat &K,
    Sophus::SE3d &pose);

int main(int argc, char **argv)
{
    //
    // if(argc != 3)
    // {
    //     cout << "usage: feature_extraction img1 img2" << endl;
    //     return 1;
    // }
    // //---读取图像
    // Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    // Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    // assert(img_1.data != nullptr && img_2.data != nullptr);
    //
    //---读取图像
    Mat img_1 = imread("./1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("./2.png", CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;

    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;

    // 建立3D点
    Mat d1 = imread("./1_depth.png", CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts_3d; // 3d路标点
    vector<Point2f> pts_2d; // 2d像素点
    for (DMatch m : matches)
    {
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if (d == 0) // bad depth
            continue;
        float dd = d / 5000.0;
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd)); // img_1中提取的特征点对应的相机坐标系下3D点坐标
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);        // img_2中提取的特征点坐标
    }
    cout << "3d-2d pairs:" << pts_3d.size() << endl; //输出3d-2d pairs

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Mat r, t;                                                                                      // r表示旋转向量 t表示平移向量
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);                                               // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;                                                                                         //旋转矩阵R
    cv::Rodrigues(r, R);                                                                           //由罗德里格斯公式将旋转向量转换为旋转矩阵
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();                             //计时结束
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1); //计算耗时
    cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;         //输出使用opencv中使用pnp所花费的时间
    cout << "R=" << endl
         << R << endl;
    cout << "t=" << endl
         << t << endl;

    //接下来演示如何使用非线性优化的方式计算相机位姿，首先手写一个高斯牛顿法的PnP，然后演示如何调用g2o来求解
    VecVector3d pts_3d_eigen;
    VecVector2d pts_2d_eigen;
    for (size_t i = 0; i < pts_3d.size(); ++i)
    {
        pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z)); // img_1中提取的特征点对应的相机坐标系下3D点坐标
        pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y)); // img_2中提取的特征点坐标
    }
    cout << "calling bundle adjustment by gauss newton" << endl;
    Sophus::SE3d pose_gn;
    t1 = chrono::steady_clock::now();
    bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

    return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches)
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist)
            min_dist = dist;
        if (dist > max_dist)
            max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }
}

Point2f pixel2cam(const Point2d &p, const Mat &K)
{
    return Point2f(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

void bundleAdjustmentGaussNewton(
    const VecVector3d &points_3d,
    const VecVector2d &points_2d,
    const Mat &K,
    Sophus::SE3d &pose)
{
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    const int iterations = 10;
    double cost = 0, lastcost = 0;
    double fx = K.at<double>(0, 0);
    double fy = K.at<double>(1, 1);
    double cx = K.at<double>(0, 2);
    double cy = K.at<double>(1, 2);

    for(int iter = 0; iter < iterations; ++iter)
    {
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost = 0;
        //compute cost
        for(int i=0; i<points_3d.size(); ++i)
        {
            Eigen::Vector3d pc = pose * points_3d[i];
            double inv_z = 1.0 / pc[2];
            double inv_z2 = inv_z * inv_z;
            //将相机坐标系下的点投影到像素坐标系下与观测位置做差
            Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
            Eigen::Vector2d e = points_2d[i] - proj;
            cost += e.squaredNorm();
            Eigen::Matrix<double, 2, 6> J;
            J << -fx * inv_z,
            0,
            fx * pc[0] * inv_z2,
            fx * pc[0] * pc[1] * inv_z2,
            -fx - fx * pc[0] * pc[0] * inv_z2,
            fx * pc[1] * inv_z,
            0,
            -fy * inv_z,
            fy * pc[1] * inv_z2,
            fy + fy * pc[1] * pc[1] * inv_z2,
            -fy * pc[0] * pc[1] * inv_z2,
            -fy * pc[0] * inv_z;

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

        Vector6d dx;
        dx = H.ldlt().solve(b);

        if(isnan(dx[0]))
        {
            cout << "result is nan!" << endl;
            break;
        }

        if(iter > 0 && cost >= lastcost)
        {
            //cost increase, update is not good
            cout << "cost: " << cost << ",last cost:" << lastcost << endl;
            break;
        }

        //update your estimation
        pose = Sophus::SE3d::exp(dx) * pose;
        lastcost = cost;

        cout << "iteration " << iter << "cost=" << cout.precision(12) << cost << endl;
        if(dx.norm() < 1e-6)
        {
            //converge
            break;
        }
    }
    cout << "pose by g-n: \n" << pose.matrix() << endl;
}

运行结果：
请添加图片描述

7.8.3 使用 g2o 进行 BA 优化

在手写了一遍优化流程之后，接下来看如何用 g2o 实现同样的操作。
在使用 g2o 之前，要把问题建模成一个图优化问题。
在这里插入图片描述
在这个图优化问题中，节点和边的旋转如下：
1.节点：第二个相机的位姿节点 $\in SE(3)$
2.边：每个 $3 D$ 点在第二个相机中的投影，以观测方程来描述：
$z_j = h(T,P_j)$
由于第一个相机位姿固定为零，所以没有把它写到优化变量中去，但是在更多的场合里，会考虑更多相机的估计。现在，根据一组 3D 点和第二个图像中的 2D 投影，估计第二个相机的位姿。
g2o 中提供了许多关于 BA 的节点和边，例如 g2o/types/sba/types_six_dof_expmap.h 中提供了李代数表达的节点和边。在本节中，自己实现一个 VertexPose 顶点和 EdgeProjection 边。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <algorithm>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keypoints_1,
    std::vector<KeyPoint> &keypoints_2,
    std::vector<DMatch> &matches);

// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundleAdjustmentGaussNewton(
    const VecVector3d &points_3d,
    const VecVector2d &points_2d,
    const Mat &K,
    Sophus::SE3d &pose);

void bundleAdjustmentG2O(
    const VecVector3d &points_3d,
    const VecVector2d &points_2d,
    const Mat &K,
    Sophus::SE3d &pose);

int main(int argc, char **argv)
{
    //
    // if(argc != 3)
    // {
    //     cout << "usage: feature_extraction img1 img2" << endl;
    //     return 1;
    // }
    // //---读取图像
    // Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    // Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    // assert(img_1.data != nullptr && img_2.data != nullptr);
    //
    //---读取图像
    Mat img_1 = imread("./1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("./2.png", CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;

    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;

    // 建立3D点
    Mat d1 = imread("./1_depth.png", CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts_3d; // 3d路标点
    vector<Point2f> pts_2d; // 2d像素点
    for (DMatch m : matches)
    {
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if (d == 0) // bad depth
            continue;
        float dd = d / 5000.0;
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd)); // img_1中提取的特征点对应的相机坐标系下3D点坐标
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);        // img_2中提取的特征点坐标
    }
    cout << "3d-2d pairs:" << pts_3d.size() << endl; //输出3d-2d pairs

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Mat r, t;                                                                                      // r表示旋转向量 t表示平移向量
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);                                               // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;                                                                                         //旋转矩阵R
    cv::Rodrigues(r, R);                                                                           //由罗德里格斯公式将旋转向量转换为旋转矩阵
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();                             //计时结束
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1); //计算耗时
    cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;         //输出使用opencv中使用pnp所花费的时间
    cout << "R=" << endl
         << R << endl;
    cout << "t=" << endl
         << t << endl;

    //接下来演示如何使用非线性优化的方式计算相机位姿，首先手写一个高斯牛顿法的PnP，然后演示如何调用g2o来求解
    VecVector3d pts_3d_eigen;
    VecVector2d pts_2d_eigen;
    for (size_t i = 0; i < pts_3d.size(); ++i)
    {
        pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z)); // img_1中提取的特征点对应的相机坐标系下3D点坐标
        pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));              // img_2中提取的特征点坐标
    }
    cout << "calling bundle adjustment by gauss newton" << endl;
    Sophus::SE3d pose_gn;
    t1 = chrono::steady_clock::now();
    bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

    cout << "calling bundle adjustment by g2o" << endl;
    Sophus::SE3d pose_g2o;
    t1 = chrono::steady_clock::now();
    bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;

    return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches)
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist)
            min_dist = dist;
        if (dist > max_dist)
            max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }
}

Point2f pixel2cam(const Point2d &p, const Mat &K)
{
    return Point2f(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

void bundleAdjustmentGaussNewton(
    const VecVector3d &points_3d,
    const VecVector2d &points_2d,
    const Mat &K,
    Sophus::SE3d &pose)
{
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    const int iterations = 10;
    double cost = 0, lastcost = 0;
    double fx = K.at<double>(0, 0);
    double fy = K.at<double>(1, 1);
    double cx = K.at<double>(0, 2);
    double cy = K.at<double>(1, 2);

    for (int iter = 0; iter < iterations; ++iter)
    {
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost = 0;
        // compute cost
        for (int i = 0; i < points_3d.size(); ++i)
        {
            Eigen::Vector3d pc = pose * points_3d[i];
            double inv_z = 1.0 / pc[2];
            double inv_z2 = inv_z * inv_z;
            //将相机坐标系下的点投影到像素坐标系下与观测位置做差
            Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
            Eigen::Vector2d e = points_2d[i] - proj;
            cost += e.squaredNorm();
            Eigen::Matrix<double, 2, 6> J;
            J << -fx * inv_z,
                0,
                fx * pc[0] * inv_z2,
                fx * pc[0] * pc[1] * inv_z2,
                -fx - fx * pc[0] * pc[0] * inv_z2,
                fx * pc[1] * inv_z,
                0,
                -fy * inv_z,
                fy * pc[1] * inv_z2,
                fy + fy * pc[1] * pc[1] * inv_z2,
                -fy * pc[0] * pc[1] * inv_z2,
                -fy * pc[0] * inv_z;

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

        Vector6d dx;
        dx = H.ldlt().solve(b);

        if (isnan(dx[0]))
        {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastcost)
        {
            // cost increase, update is not good
            cout << "cost: " << cost << ",last cost:" << lastcost << endl;
            break;
        }

        // update your estimation
        pose = Sophus::SE3d::exp(dx) * pose;
        lastcost = cost;

        cout << "iteration " << iter << "cost=" << cout.precision(12) << cost << endl;
        if (dx.norm() < 1e-6)
        {
            // converge
            break;
        }
    }
    cout << "pose by g-n: \n"
         << pose.matrix() << endl;
}

// vertex and edges used in g2o
//曲线模型的顶点，模板参数：优化变量维度和数据类型
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> //:表示继承，public表示公有继承；CurveFittingVertex是派生类，:BaseVertex<6, Sophus::SE3d>是基类
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //解决Eigen库数据结构内存对齐问题

    virtual void setToOriginImpl() override // virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
    {
        _estimate = Sophus::SE3d();
    }

    //更新，left multiplication on SE3
    virtual void oplusImpl(const double *update) override
    {
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
    }
    //存盘和读盘：留空
    virtual bool read(istream &in) override {}
    virtual bool write(ostream &out) const override {}
};

//误差模型　模板模型:观测值维度，类型，连接顶点类型
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {} //使用列表赋初值

    //计算曲线模型误差
    virtual void computeError() override
    {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]); //创建指针v
        Sophus::SE3d T = v->estimate();                                //将estimate()值赋给V
        Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();
    }
    //计算雅克比矩阵
    virtual void linearizeOplus() override
    {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Eigen::Vector3d pos_cam = T * _pos3d;
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Z2 = Z * Z;
        _jacobianOplusXi
            << -fx / Z,
            0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
            0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
    }

    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}

private:
    Eigen::Vector3d _pos3d;
    Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(
    const VecVector3d &points_3d,
    const VecVector2d &points_2d,
    const Mat &K,
    Sophus::SE3d &pose)
{
    //构建图优化，先设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;           // pose is 6, landmark is 3
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; //线性求解器类型
    //梯度下降方法，可以从GN,LM,Dogleg中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer; //图模型
    optimizer.setAlgorithm(solver); //设置求解器
    optimizer.setVerbose(true);     //打开调试输出

    // vertex,往图中添加顶点
    VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
    vertex_pose->setId(0);
    vertex_pose->setEstimate(Sophus::SE3d());
    optimizer.addVertex(vertex_pose);

    // K
    Eigen::Matrix3d K_eigen;
    K_eigen << K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
        K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
        K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

    // edges
    int index = 1;
    for (size_t i = 0; i < points_2d.size(); ++i)
    {
        auto p2d = points_2d[i];
        auto p3d = points_3d[i];
        EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
        edge->setId(index);
        edge->setVertex(0, vertex_pose); //设置连接的顶点
        edge->setMeasurement(p2d); //观测数据
        edge->setInformation(Eigen::Matrix2d::Identity()); //信息矩阵：协方差矩阵之逆
        optimizer.addEdge(edge);
        index++;
    }
    //执行优化
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
    cout << "pose estimated by g2o =\n"
         << vertex_pose->estimate().matrix() << endl;
    pose = vertex_pose->estimate();
}

运行结果：
请添加图片描述
首先，声明了 g2o 图优化器，并配置优化求解器和梯度下降方法。然后，根据估计到的特征点，将位姿和空间点放到图中。最后，调用优化函数进行求解。
BA是一种通用的做法，它可以不限于两幅图像，完全可以放入多幅图像匹配到的位姿和空间点进行迭代优化，甚至可以把整个SLAM过程放进来。那种做法规模较大，主要在后端使用，会在第10讲中再次遇到这个问题。在前端通常考虑局部相机位姿和特征点的小型BA问题，希望对它进行实时求解和优化。

四夕小一冰

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
视觉SLAM十四讲笔记-7-4

视觉SLAM十四讲笔记-7-47.7 3D-2D:PnPPnP(Perspective-n-point) 是求解 3D 到 2D 点对运动的方法。它描述了当知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。2D-2D的对极几何方法需要 8 个或者 8 个以上的点对，且存在初始化、纯旋转和尺度的问题。然而，如果两张图像中的一张特征点的 3D 位置已知，那么最少需要 3 个点对(以及一个额外点验证结果)就可以估计相机运动。特征点的 3D 位置可以有三角化或者 RGB-D 相机的深度图像
复制链接

扫一扫

专栏目录