视觉 SLAM 十四讲 - 7 (下)

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/mobius_strip/article/details/84591881

7.7 3D-2D: PnP

PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。前面已经说了， 2D-2D 的对极几何方法需要八个或八个以上的点对（以八点法为例），且存在着初始化、纯旋转和尺度的问题。然而，如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的 3D 位置可以由三角化，或者由 RGB-D 相机的深度图确定。因此，在双目或 RGB-D 的视觉里程计中，我们可以直接使用 PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP。 3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。

PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P[45]，直接线性变换（DLT），EPnP（Efcient PnP） [46]， UPnP[47] 等等）。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment。我们先来看 DLT，然后再讲 Bundle Adjustment。

7.7.1 直接线性变换

考虑某个空间点 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{bmatrix} 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{bmatrix} \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^T_1 P − t^T_3 P u_1 = 0,$
和
$t^T_2 P − t^T_3 P v_1 = 0.$
请注意 t 是待求的变量，可以看到每个特征点提供了两个关于 t 的线性约束。假设一共有 N 个特征点，可以列出线性方程组：
$\begin{bmatrix} P^T_1 & 0 & −u_1P_1^T \\ 0 & P_1^T −v_1P_1^T \\ . &. &. \\ . &. &. \\ P^T_N & 0 & −u_NP_N^T \\ 0 & P^T_N & −v_NP_N^T \end{bmatrix} \begin{bmatrix} t_1 \\ t_2 \\ t_3 \end{bmatrix} = 0.$
由于 t 一共有 12 维，因此最少通过六对匹配点，即可实现矩阵 T 的线性求解，这种方法（也）称为直接线性变换（Direct Linear Transform， DLT）。当匹配点大于六对时，（又）可以使用 SVD 等方法对超定方程求最小二乘解。

在 DLT 求解中，我们直接将 T 矩阵看成了 12 个未知数，忽略了它们之间的联系。因为旋转矩阵 $R \in SO(3)$，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。平移向量比较好办，它属于向量空间。对于旋转矩阵 R，我们必须针对 DLT 估计的 T 的左边 3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成 [3, 48]，相当于把结果从矩阵空间重新投影到 $SE(3)$ 流形上，转换成旋转和平移两部分。

需要解释的是，我们这里的 x1 使用了归一化平面坐标，去掉了内参矩阵 K 的影响——这是因为内参 K 在 SLAM 中通常假设为已知。如果内参未知，那么我们也能用 PnP 去估计 K, R, t 三个量。然而由于未知量的增多，效果会差一些。

7.7.2 P3P

下面讲的 P3P 是另一种解 PnP 的方法。它仅使用三对匹配点，对数据要求较少，因此这里也简单介绍一下（这部分推导借鉴了 [49]）。

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D 点为 A, B, C， 2D 点为 a, b, c，其中小写字母代表的点为大写字母在相机成像平面上的投影，如图 7-11 所示。此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 D − d，相机光心为 O。请注意，我们知道的是 A, B, C 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。一旦 3D 点在相机坐标系下的坐标能够算出，我们就得到了 3D-3D 的对应点，把 PnP 问题转换为了 ICP 问题。

首先，显然，三角形之间存在对应关系：
$∆Oab − ∆OAB, ∆Obc − ∆OBC, ∆Oac − ∆OAC.$
来考虑 Oab 和 OAB 的关系。利用余弦定理，有：
$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, c⟩ = AC^2.$
对上面三式全体除以 $OC^2$，并且记 $x = OA/OC, y = OB/OC$，得：
$x^2 + y^2 − 2xy cos ⟨a, b⟩ = AB^2/OC^2 \\ y^2 + 1^2 − 2y cos ⟨b, c⟩ = BC^2/OC^2 \\ x^2 + 1^2 − 2x cos ⟨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 − 2xy cos ⟨a, b⟩ − v = 0 \\ y^2 + 1^2 − 2y cos ⟨b, c⟩ − uv = 0 \\ x^2 + 1^2 − 2x cos ⟨a, c⟩ − wv = 0.$
我们可以把第一个式子中的 v 放到等式一边，并代入第 2， 3 两式，得：
$(1 − u) y^2 − ux^2 − cos ⟨b, c⟩ y + 2uxy cos ⟨a, b⟩ + 1 = 0 \\ (1 − w) x^2 − wy^2 − cos ⟨a, c⟩ x + 2wxy cos ⟨a, b⟩ + 1 = 0.$
(7.33)
注意这些方程中的已知量和未知量。由于我们知道 2D 点的图像位置，三个余弦角 $cos ⟨a, b⟩ , cos ⟨b, c⟩ , cos ⟨a, c⟩$ 是已知的。同时， $u = BC^2/AB^2, w = AC^2/AB^2$ 可以通过 A, B, C 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的 x, y 是未知的，随着相机移动会发生变化。因此，该方程组是关于 x, y 的一个二元二次方程（多项式方程）。解析地求解该方程组是一个复杂的过程，需要用吴消元法。这里不展开对该方程解法的介绍，感兴趣的读者请参照 [45]。类似于分解 E 的情况，该方程最多
可能得到四个解，但我们可以用验证点来计算最可能的解，得到 A, B, C 在相机坐标系下的 3D 坐标。然后，根据 3D-3D 的点对，计算相机的运动 R, t。这部分将在 7.9 小结介绍。

从 P3P 的原理上可以看出，为了求解 PnP，我们利用了三角形相似性质，求解投影点 a, b, c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。后文将看到，带有匹配信息的 3D-3D 位姿求解非常容易，所以这种思路是非常有效的。其他的一些方法，例如 EPnP，亦采用了这种思路。然而， P3P 也存在着一些问题：

1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。

所以后续人们还提出了许多别的方法，如 EPnP、 UPnP 等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。不过，相对于 P3P 来说，原理会更加复杂一些，所以我们建议读者阅读原始的论文，或通过实践来理解 PnP 过程。在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。接下来我们从非线性优化角度来看一下 PnP 问题。

7.7.3 Bundle Adjustment

除了使用线性方法之外，我们可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。这将用到本书第四章和第五章的知识。前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。这是一种非常通用的求解方式，我们可以用它对 PnP 或 ICP 给出的结果进行优化。在 PnP 中，这个 Bundle Adjustment 问题，是一个最小化重投影误差（Reprojection error） 的问题。我们在本节给出此问题在两个视图下的基本形式，然后在第十讲讨论较大规模的 BA 问题。

考虑 n 个三维空间点 P 和它们的投影 p，我们希望计算相机的位姿 R, t，它的李代数表示为 ξ。假设某空间点坐标为 $P_i = [X_i, Y_i, Z_i]^T$ ，其投影的像素坐标为 $u_i = [u_i, v_i]^T$ 。
根据第五章的内容，像素位置与空间点位置的关系如下：
$s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = K exp (ξ^\wedge) \begin{bmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{bmatrix}.$
除了用 ξ 为李代数表示的相机姿态之外，别的都和前面的定义保持一致。写成矩阵形式就是：
$s_iu_i = K exp (ξ^\wedge) P_i.$
请读者脑补中间隐含着的齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的。现在，由于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：
$ξ^∗ = arg \min_ξ \frac{1}{2}\sum_{i=1}^{n} ||u_i − \frac{1}{s_i}K exp (ξ^\wedge) P_i||_2^2.$
该问题的误差项，是将像素坐标（观测到的投影位置）与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称之为重投影误差。使用齐次坐标时，这个误差有 3 维。不过，由于 u 最后一维为 1，该维度的误差一直为零，因而我们更多时候使用非齐次坐标，于是误差就只有 2 维了。如图 7-12 所示，我们通过特征匹配，知道了 $p_1$ 和 $p_2$ 是同一个空间点 P 的投影，但是我们不知道相机的位姿。在初始值中， P 的投影 $\hat{p}_2$
与实际的 $p_2$ 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。

最小二乘优化问题已经在第六讲介绍过了。使用李代数，可以构建无约束的优化问题，很方便地通过 G-N, L-M 等优化算法进行求解。不过，在使用 G-N 和 L-M 之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化：
$e(x + ∆x) ≈ e(x) + J∆x.$
这里的 J 的形式是值得讨论的，甚至可以说是关键所在。我们固然可以使用数值导数，但如果能够推导解析形式时，我们会优先考虑解析导数。现在，当 e 为像素坐标误差（2 维）， x 为相机位姿（6 维）时， J 将是一个 2 × 6 的矩阵。我们来推导 J 的形式。

回忆李代数的内容，我们介绍了如何使用扰动模型来求李代数的导数。首先，记变换到相机坐标系下的空间点坐标为 $P^′$，并且把它前三维取出来：
$P ^′ = (exp (ξ^\wedge) P )_{1:3} = [X^′, Y ^′, Z^′]^T.$
那么，相机投影模型相对于 $P^′$ 则为：
$s_u = KP^ ′.$
展开之：
$\begin{bmatrix} su \\ sv \\ s \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X^′ \\ Y^′ \\ Z^′ \end{bmatrix}.$
利用第 3 行消去 s（实际上就是 $P^′$ 的距离），得：
$u = f_x \frac{X^′}{Z^′} + c_x, v = f_y \frac{Y^′}{Z^′} + c_y.$
这与之前讲的相机模型是一致的。当我们求误差时，可以把这里的 u, v 与实际的测量值比较，求差。在定义了中间变量后，我们对 ξ^ 左乘扰动量 δξ，然后考虑 e 的变化关于扰动量的导数。利用链式法则，可以列写如下：
$\frac{\partial e}{\partial δξ} = \lim_{δξ \rightarrow 0} \frac{e(δξ⊕ ξ)}{δξ} = \frac{\partial e}{\partial P^′}\frac{\partial P^′}{\partial δξ} .$
这里的 ⊕ 指李代数上的左乘扰动。第一项是误差关于投影点的导数，在式（7.40）已经列出了变量之间的关系，易得：
$\frac{\partial e}{\partial P^′} = − \begin{bmatrix} \frac{\partial u}{\partial X^′} & \frac{\partial u}{\partial Y^′} & \frac{\partial u}{\partial Z^′} \\ \frac{\partial v}{\partial X^′} & \frac{\partial v}{\partial Y^′} & \frac{\partial v}{\partial Z^′} \end{bmatrix} = − \begin{bmatrix} \frac{f_x}{Z^′} & 0 & − \frac{f_xX^′}{Z^′} \\ 0 & \frac{f_y}{Z^′} & − \frac{f_yY^′}{Z^′} \end{bmatrix}.$
而第二项为变换后的点关于李代数的导数，根据在4.3.5中的推导，得：
$\frac{\partial TP}{\partial δξ} = (T P )^⊙ = \begin{bmatrix} I & −P^{′\wedge} \\ 0^T & 0^T \end{bmatrix}.$
而在 P′ 的定义中，我们取出了前三维，于是得：
$\frac{\partial P^′}{\partial δξ} = [I, −P ^{′\wedge}].$
将这两项相乘，就得到了 2 × 6 的雅可比矩阵：
$\frac{\partial P^′}{\partial δξ} = − \begin{bmatrix} \frac{f_x}{Z^′} & 0 & −\frac{f_xX^′}{Z^{′2}} & −\frac{f_xX^′Y^′}{Z^{′2}} & f_x + \frac{f_xX^2}{Z^{′2}} & − \frac{f_xY^′}{Z′} \\ 0 & \frac{f_y}{Z^′} & − \frac{f_yY^′}{Z^{′2}} & −f_y − \frac{f_yY^{′2}}{Z^{′2}} & \frac{f_yX^′Y^′}{Z^{′2}} & \frac{f_yX^′}{Z^′} \end{bmatrix} .$
这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。我们保留了前面的负号，因为这是由于误差是由观测值减预测值定义的。它当然也可反过来，定义成“预测减观测”的形式。在那种情况下，只要去掉前面的负号即可。此外，如果 $se(3)$ 的定义方式是旋转在前，平移在后时，只要把这个矩阵的前三列与后三列对调即可。

另一方面，除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 e 关于空间点 P 的导数。所幸这个导数矩阵相对来说容易一些。仍利用链式法则，有：
$\frac{\partial e}{\partial P} = \frac{\partial e}{\partial P^′}\frac{\partial P^′}{\partial P} .$
第一项已在前面推导了，第二项，按照定义
$P^′ = exp(ξ^\wedge)P = RP + t.$
我们发现 P ′ 对 P 求导后只剩下 R。于是
$\frac{\partial e}{\partial P} = − \begin{bmatrix} \frac{f_x}{Z^′} & 0 & − \frac{f_xX^′}{Z^{′2}} \\ 0 & \frac{f_y}{Z^′} & − \frac{f_yY^′}{Z^{′2}} \end{bmatrix} R.$
于是，我们推导了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。

7.8 实践：求解 PnP

7.8.1 使用 EPnP 求解位姿

下面，我们通过实验理解一下 PnP 的过程。首先，我们用 OpenCV 提供的 EPnP 求解 PnP 问题，然后通过 g2o 对结果进行优化。由于 PnP 需要使用 3D 点，为了避免初始化带来的麻烦，我们使用了 RGB-D 相机中的深度图（1_depth.png），作为特征点的 3D 位置。首先来看 OpenCV 提供的 PnP 函数：

slambook/ch7/pose_estimation_3d2d.cpp（片段）

int main( int argc, char** argv )
{
    ......
    // 建立 3D 点
    Mat d1 = imread( argv[3], 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;
    vector<Point2f> pts_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/1000.0;
        Point2d p1 = pixel2cam( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back( Point3f(p1.x*dd, p1.y*dd, dd) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }
 
    cout<<"3d-2d pairs: "<<pts_3d.size()<<endl;
   
    Mat r, t;
    // 调用 OpenCV 的 PnP 求解，可选择 EPNP ， DLS 等方法
    solvePnP( pts_3d, pts_2d, K, Mat(), r, t, false, cv::SOLVEPNP_EPNP );
    Mat R;
    cv::Rodrigues(r, R); // r 为旋转向量形式，用 Rodrigues 公式转换为矩阵
 
    cout << "R=" << endl << R << endl;
    cout << "t=" << endl << t << endl;
}

空间位置。以此空间位置为 3D 点，再以第二个图像的像素位置为 2D 点，调用 EPnP 求解 PnP 问题。程序输出如下：

% build/pose_estimation_3d2d 1.png 2.png d1.png d2.png
-- Max dist : 95.000000
-- Min dist : 4.000000
一共找到了79组匹配点
3d-2d pairs: 78
R=
[0.9977970937403702, -0.05195299069131867, 0.04125344205637558;
0.05073872610592159, 0.9982626103770279, 0.02995567385972873;
-0.04273805559942161, -0.02779653722084675, 0.9986995599889442]
t=
[-0.6455324432075111;
-0.05776758294184359;
0.2844565219506077]

读者可以对比先前 2D-2D 情况下求解的 R, t 有什么不同。可以看到，在有 3D 信息时，估计的 R 几乎是相同的，而 t 相差的较多。这是由于我们引入了新的深度信息所致。不过，由于 Kinect 采集的深度图本身会有一些误差，所以这里的 3D 点也不是准确的。我们会希望把位姿 ξ 和所有三维特征点 P 同时优化。

7.8.2 使用 BA 优化

下面，我们来演示如何进行 Bundle Adjustment。我们将使用前一步的估计值作为初始值。优化可以使用前面讲的 Ceres 或 g2o 库实现，这里采用 g2o 作为例子。

g2o 的基本知识在第六讲中已经介绍过了。在使用 g2o 之前，我们要把问题建模成一个最小二乘的图优化问题，如图 7-13 所示。在这个图优化中，节点和边的选择为：

1. 节点：第二个相机的位姿节点 $ξ \in se(3)$，以及所有特征点的空间位置 $P \in R^3$。
2. 边：每个 3D 点在第二个相机中的投影，以观测方程来描述：
   $z_j = h(ξ, P_j).$

由于第一个相机位姿固定为零，我们没有把它写到优化变量里，但在习题中，我希望你能够把第一个相机的位姿与观测也考虑进来。现在我们根据一组 3D 点和第二个图像中的 2D 投影，估计第二个相机的位姿。所以我们把第一个相机画成虚线，表明我们不希望考虑它。

g2o 提供了许多关于 BA 的节点和边，我们不必自己从头实现所有的计算。在
g2o/types/sba/types_six_dof_expmap.h 中则提供了李代数表达的节点和边。请读者打开这个文件，找到 VertexSE3Expmap（李代数位姿）、 VertexSBAPointXYZ（空间点位置）和 EdgeProjectXYZ2UV（投影方程边）这三个类。我们来简单看一下它们的类定义，例如 VertexSE3Expmap：

class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    VertexSE3Expmap();
 
    bool read(std::istream& is);
 
    bool write(std::ostream& os) const;
 
    virtual void setToOriginImpl() {
        _estimate = SE3Quat();
    }
 
    virtual void oplusImpl(const double* update_) {
        Eigen::Map<const Vector6d> update(update_);
        setEstimate( SE3Quat::exp(update)*estimate());
    }
};

请注意它的模板参数。第一个参数 6 表示它内部存储的优化变量维度，可以看到这是一个 6 维的李代数。第二参数是优化变量的类型，这里使用了 g2o 定义的相机位姿： SE3Quat。这个类内部使用了四元数加位移向量来存储位姿，但同时也支持李代数上的运算，例如对数映射（log 函数）和李代数上增量（update 函数）等操作。我们可以对照它的实现代码，看看 g2o 对李代数是如何操作的：

class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D>
{
    ......
};
 
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ,
VertexSE3Expmap>
{
    ......
    void computeError() {
        const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
        const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
        const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));
        Vector2D obs(_measurement);
        _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }
};

我就不把整个类定义都搬过来了。从模板参数可以看到，空间点位置类的维度为 3，类型是 Eigen 的 Vector3D。另一方面，边 EdgeProjectXYZ2UV 连接了两个前面说的两个顶点，它的观测值为 2 维，由 Vector2D 表示，实际上就是空间点的像素坐标。它的误差计算函数表达了投影方程的误差计算方法，也就是我们前面提到的 z − h(ξ; P ) 的方式。

现在，进一步观察 EdgeProjectXYZ2UV 的 linearizeOplus 函数的实现。这里用到了我们前面推导的雅可比矩阵：

void EdgeProjectXYZ2UV::linearizeOplus() {
    VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
    SE3Quat T(vj->estimate());
    VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
    Vector3D xyz = vi->estimate();
    Vector3D xyz_trans = T.map(xyz);
 
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    double z_2 = z*z;
 
    const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));
 
    Matrix<double,2,3,Eigen::ColMajor> tmp;
    tmp(0,0) = cam->focal_length;
    tmp(0,1) = 0;
    tmp(0,2) = -x/z*cam->focal_length;
 
    tmp(1,0) = 0;
    tmp(1,1) = cam->focal_length;
    tmp(1,2) = -y/z*cam->focal_length;
 
    _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
 
    _jacobianOplusXj(0,0) = x*y/z_2 *cam->focal_length;
    _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
    _jacobianOplusXj(0,2) = y/z *cam->focal_length;
    _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
    _jacobianOplusXj(0,4) = 0;
    _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;
 
    _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
    _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
    _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
    _jacobianOplusXj(1,3) = 0;
    _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
    _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

仔细研究此段代码，我们会发现它与式（7.45）和（7.47）是一致的。成员变量“_-jacobianOplusXi”是误差到空间点的导数，“_jacobianOplusXj”是误差到相机位姿的导数，以李代数的左乘扰动表达。稍有差别的是， g2o 的相机里用 f 统一描述 $f_x, f_y$，并且李代数定义顺序不同（g2o 是旋转在前，平移在后；我们是平移在前，旋转在后），所以矩阵前三列和后三列与我们的定义是颠倒的。此外都是一致的。

值得一提的是，我们亦可自己实现相机位姿节点，并使用 Sophus::SE3 来表达位姿，提供类似的求导过程。然而，既然 g2o 已经提供了这样的类，在没有额外要求的情况下，自己重新实现就没有必要了。现在，我们在上一个 PnP 例程的基础上，加上 g2o 提供的 Bundle Adjustment。

slambook/ch7/pose_estimation_3d2d.cpp（片段）

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
    Block* solver_ptr = new Block( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );
 
    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
    R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
    R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
    R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2);
    pose->setId(0);
    pose->setEstimate( g2o::SE3Quat(
        R_mat,
        Eigen::Vector3d( t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0))
    ) );
    optimizer.addVertex( pose );
   
    int index = 1;
    for ( const Point3f p:points_3d ) // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId( index++ );
        point->setEstimate( Eigen::Vector3d(p.x, p.y, p.z) );
        point->setMarginalized( true );
        optimizer.addVertex( point );
    }
 
    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters(
    K.at<double>(0,0), Eigen::Vector2d(K.at<double>(0,2), K.at<double>(1,2)), 0
42 );
    camera->setId(0);
    optimizer.addParameter( camera );
 
    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId( index );
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(index)) );
        edge->setVertex( 1, pose );
        edge->setMeasurement( Eigen::Vector2d( p.x, p.y ) );
        edge->setParameterId(0,0);
        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(100);
    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 << endl << "after optimization:" << endl;
    cout << "T=" << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}

程序大体上和第六章的 g2o 类似。我们首先声明了 g2o 图优化，配置优化求解器和梯度下降方法，然后根据估计到的特征点，将位姿和空间点放到图中。最后调用优化函数进行求解。读者可以看到优化的结果：

calling bundle adjustment
iteration= 0 chi2= 1.083180 time= 0.000107183 cumTime= 0.000107183 edges= 76 schur= 1 lambda=
78.907222 levenbergIter= 1
iteration= 1 chi2= 0.000798 time= 5.8615e-05 cumTime= 0.000165798 edges= 76 schur= 1 lambda= 26.302407
levenbergIter= 1
iteration= 2 chi2= 0.000000 time= 3.0203e-05 cumTime= 0.000196001 edges= 76 schur= 1 lambda= 17.534938
levenbergIter= 1
...... 中间过程略
iteration= 11 chi2= 0.000000 time= 2.8394e-05 cumTime= 0.000525203 edges= 76 schur= 1 lambda=
11209.703029 levenbergIter= 1
optimization costs time: 0.00132938 seconds.

after optimization:
T=
0.997776 -0.0519476 0.0417755 -0.649778
0.050735 0.998274 0.0295806 -0.0545231
-0.0432401 -0.0273953 0.998689 0.295564
0 0 0 1

迭代 11 轮后， LM 发现优化目标函数接近不变，于是停止了优化。我们输出了最后得到位姿变换矩阵 T，对比之前直接做 PnP 的结果，大约在小数点后第三位发生了一些变化。这主要是由于我们同时优化了特征点和相机位姿导致的。
Bundle Adjustment 是一种通用的做法。它可以不限于两个图像。我们完全可以放入多个图像匹配到的位姿和空间点进行迭代优化，甚至可以把整个 SLAM 过程放进来。那种做法规模较大，主要在后端使用，我们会在第十章重新遇到这个问题。在前端，我们通常考虑局部相机位姿和特征点的小型 Bundle Adjustment 问题，希望实时对它进行求解和优化。

7.9 3D-3D: ICP

最后，我们来介绍 3D-3D 的位姿估计问题。假设我们有一组配对好的 3D 点（比如我们对两个 RGB-D 图像进行了匹配）：
$P = \{p_1, ..., p_n\}, P ′ = \{p^′ _1, ..., p^′ _n\},$
现在，想要找一个欧氏变换 R, t，使得：
KaTeX parse error: Expected 'EOF', got '\all' at position 2: \̲a̲l̲l̲ ̲i, p_i = Rp^′_i…
这个问题可以用迭代最近点（Iterative Closest Point, ICP）求解。读者应该注意到，3D-3D 位姿估计问题中，并没有出现相机模型，也就是说，仅考虑两组 3D 点之间的变换时，和相机并没有关系。因此，在激光 SLAM 中也会碰到 ICP，不过由于激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。而在视觉中，特征点为我们提供了较好的匹配关系，所以整个问题就变得更简单了。在 RGB-D SLAM 中，可以用这种方式估计相机位姿。下文我们用 ICP 指代匹配好的两组点间运动估计问题。

和 PnP 类似， ICP 的求解也分为两种方式：利用线性代数的求解（主要是 SVD），以及利用非线性优化方式的求解（类似于 Bundle Adjustment）。下面分别来介绍它们。

7.9.1 SVD 方法

首先我们看以 SVD 为代表的代数方法。根据前面描述的 ICP 问题，我们先定义第 i 对点的误差项：
$e_i = p_i − (Rp^′ _i + t).$
然后，构建最小二乘问题，求使误差平方和达到极小的 R, t：
$\min_{R,t} J = \frac{1}{2}\sum_{i=1}^{n} ∥(p_i − (Rp_i^′ + t))∥_2 ^2.$
下面我们来推导它的求解方法。首先，定义两组点的质心：
$p = \frac{1}{n}∑ _{i=1}^{n}(p_i), p^′ = \frac{1}{n}∑_{i=1}^{n}(p^′ _i).$
请注意质心是没有下标的。随后，在误差函数中，我们作如下的处理：
$\begin{aligned} &\frac{1}{2}∑_{i=1}^{n}∥p_i − (Rp_i^′ + t)∥^2 \\ = &\frac{1}{2} ∑^n_{i=1}∥p_i − Rp_i^′ − t − p + Rp^′ + p − Rp^′∥^2 \\ = &\frac{1}{2}∑_{i=1}^n∥(p_i − p − R (p_i^′ − p′)) + (p − Rp^′ − t)∥^2 \\= &\frac{1}{2}∑_{i=1}^n(∥p_i − p − R (p_i^′ − p^′)∥^2 + ∥p − Rp^′ − t∥^2+ 2(p_i − p − R (p_i^′ − p^′))T (p − Rp^′ − t)). \end{aligned}$
注意到交叉项部分中， $(p_i − p − R (p_i^′ − p^′))$ 在求和之后是为零的，因此优化目标函数可以简化为：
$\min_{R,t}J = \frac{1}{2}∑ _{i=1}^n∥p_i − p − R (p_i^′ − p^′)∥^2 + ∥p − Rp^′ − t∥^2.$
仔细观察左右两项，我们发现左边只和旋转矩阵 R 相关，而右边既有 R 也有 t，但只和质心相关。只要我们获得了 R，令第二项为零就能得到 t。于是， ICP 可以分为以下三个步骤求解：

1. 计算两组点的质心位置 $p, p^′$，然后计算每个点的去质心坐标：
   $q_i = p_i − p, q_i^′ = p^′ _i − p^′.$
2. 根据以下优化问题计算旋转矩阵：
   $R^∗ = arg \min_R frac{1}{2}∑ _{i=1}^n∥q_i − Rq_i^′∥^2.$
3. 根据第二步的 R，计算 t：
   $t^∗ = p − Rp^′.$

我们看到，只要求出了两组点之间的旋转，平移量是非常容易得到的。所以我们重点关注 R 的计算。展开关于 R 的误差项，得：
$\frac{1}{2}∑ _{i=1}^n∥q_i − Rq_i^′∥^2 = \frac{1}{2}∑ _{i=1}^n q_i^T q_i + q_i^{′T} R^T Rq_i^′ − 2q_i^T Rq_i^′.$
注意到第一项和 R 无关，第二项由于 $R^T R = I$，亦与 R 无关。因此，实际上优化目标函数变为：
$∑ _{i=1}^n−q_i^T Rq_i^′ = ∑ _{i=1}^n−tr(Rq_i^′q_i^T) = −tr(R∑_{i=1}^n q_i^′q_i^T ) .$
接下来，我们介绍怎样通过 SVD 解出上述问题中最优的 R，但是关于最优性的证明较为复杂，感兴趣的读者请参考 [50, 51]。为了解 R，先定义矩阵：
$W =∑ _{i=1}^nq_iq_i^{′T}.$
W 是一个 3 × 3 的矩阵，对 W 进行 SVD 分解，得：
$W = UΣV^T.$
其中， Σ 为奇异值组成的对角矩阵，对角线元素从大到小排列，而 U 和 V 为正交矩阵。当 W 满秩时， R 为：
$R = UV ^T$
解得 R 后，按式（7.53）求解 t 即可。

7.9.2 非线性优化方法

求解 ICP 的另一种方式是使用非线性优化，以迭代的方式去找最优值。该方法和我们前面讲述的 PnP 非常相似。以李代数表达位姿时，目标函数可以写成：
KaTeX parse error: Expected group after '^' at position 44: …n∥(p_i − exp (ξ^̲^) p^′ _i)∥_2 ^…
单个误差项关于位姿导数已经在前面推导过了，使用李代数扰动模型即可：
$\frac{\partial e}{\partial δξ} = −(exp (ξ^\wedge) p_i^′)^⊙.$
于是，在非线性优化中只需不断迭代，我们就能找到极小值。而且，可以证明 [6]， ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要我们能找到极小值解，那么这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况。这也意味着ICP 求解可以任意选定初始值。这是已经匹配点时求解 ICP 的一大好处。

需要说明的是，我们这里讲的 ICP，是指已经由图像特征给定了匹配的情况下，进行位姿估计的问题。在匹配已知的情况下，这个最小二乘问题实际上具有解析解 [52, 53, 54]，所以并没有必要进行迭代优化。 ICP 的研究者们往往更加关心匹配未知的情况。不过，在RGB-D SLAM 中，由于一个像素的深度数据可能测量不到，所以我们可以混合着使用 PnP 和 ICP 优化：对于深度已知的特征点，用建模它们的 3D-3D 误差；对于深度未知的特征点，则建模 3D-2D 的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。

7.10 实践：求解 ICP

7.10.1 SVD 方法

下面，我们来演示一下如何使用 SVD 以及非线性优化来求解 ICP。本节我们使用两个 RGB-D 图像，通过特征匹配获取两组 3D 点，最后用 ICP 计算它们的位姿变换。由于OpenCV 目前还没有计算两组带匹配点的 ICP 的方法，而且它的原理也并不复杂，所以我们自己来实现一个 ICP。

slambook/ch7/pose_estimation_3d3d.cpp（片段）

void pose_estimation_3d3d(
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t)
{
    Point3f p1, p2; // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 /= N; p2 /= N;
    vector<Point3f> q1(N), q2(N); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }
    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d( q2[i].x, q2[i].y, q2[i].z )
.transpose();
    }
    cout << "W=" << W << endl;
 
    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;
 
    Eigen::Matrix3d R_ = U*(V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d( p2.x, p2.y, p2.z );
 
    // convert to cv::Mat
    R = ( Mat_<double>(3,3) <<
        R_(0,0), R_(0,1), R_(0,2),
        R_(1,0), R_(1,1), R_(1,2),
        R_(2,0), R_(2,1), R_(2,2));
    t = ( Mat_<double>(3,1) << t_(0,0), t_(1,0), t_(2,0) );
}

ICP 的实现方式和前文讲述的是一致的。我们调用 Eigen 进行 SVD，然后计算 R, t矩阵。我们输出了匹配后的结果，不过请注意，由于前面的推导是按照 $p_i = Rp^′ _i + t$ 进行的，这里的 R, t 是第二帧到第一帧的变换，与前面 PnP 部分是相反的。所以在输出结果中，我们同时打印了逆变换：

% build/pose_estimation_3d3d 1.png 2.png 1_depth.png 2_depth.png
-- Max dist : 95.000000
-- Min dist : 4.000000
一共找到了 79 组匹配点
3d-3d pairs: 74
W= 298.51 -14.1815 41.0456
-44.8208 107.825 -164.404
78.1978 -163.954 271.439
U= 0.474143 -0.880373 -0.0114952
-0.460275 -0.258979 0.849163
0.750556 0.397334 0.528006
V= 0.535211 -0.844064 -0.0332488
-0.434767 -0.309001 0.84587
0.724242 0.438263 0.532352
ICP via SVD results:
R = [0.9972395976914055, 0.05617039049497474, -0.04855998381307948;
-0.05598344580804095, 0.9984181433274515, 0.005202390798842771;
0.04877538920134394, -0.002469474885032297, 0.998806719591959]
t = [0.7086246277241892;
-0.2775515782948791;
-0.1559573762377209]
R_inv = [0.9972395976914055, -0.05598344580804095, 0.04877538920134394;
0.05617039049497474, 0.9984181433274515, -0.002469474885032297;
-0.04855998381307948, 0.005202390798842771, 0.998806719591959]
t_inv = [-0.7145999506834847;
0.2369236766013986;
0.1916260075851286]

读者可以比较一下 ICP 与 PnP, 对极几何的运动估计结果之间的差异。可以认为，在这个过程中我们使用了越来越多的信息（没有深度——有一个图的深度——有两个图的深度），因此，在深度准确的情况下，得到的估计也将越来越准确。但是，由于 Kinect 的深度图存在噪声，而且有可能存在数据丢失的情况，使得我们不得不丢弃一些没有深度数据的特征点。这可能导致 ICP 的估计不够准确，并且，如果特征点丢弃得太多，可能引起由于特征点太少，无法进行运动估计的情况。

7.10.2 非线性优化方法

下面我们考虑用非线性优化来计算 ICP。我们依然使用李代数来表达相机位姿。与SVD 思路不同的地方在于，在优化中我们不仅考虑相机的位姿，同时会优化 3D 点的空间位置。对我们来说， RGB-D 相机每次可以观测到路标点的三维位置，从而产生一个 3D 观测数据。不过，由于 g2o/sba 中没有提供 3D 到 3D 的边，而我们又想使用 g2o/sba 中李代数实现的位姿节点，所以最好的方式是自定义一种这样的边，并向 g2o 提供解析求导方式。

slambook/ch7/pose_estimation_3d3d.cpp

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) :
    _point(point) {}
 
    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
        // measurement is p, point is p
 _error = _measurement - pose->estimate().map( _point );
    }
 
    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
 
        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;
 
        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;
 
        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }
 
    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

这是一个一元边，写法类似于前面提到的 g2o::EdgeSE3ProjectXYZ，不过观测量从 2 维变成了 3 维，内部没有相机模型，并且只关联到一个节点。请读者注意这里雅可比矩阵的书写，它必须与我们前面的推导一致。雅可比矩阵给出了关于相机位姿的导数，是一个 3 × 6 的矩阵。

调用 g2o 进行优化的代码是相似的，我们设定好图优化的节点和边即可。这部分代码请读者查看源文件，我们就不在书中列出了。现在，来看看优化的结果：

calling bundle adjustment
iteration= 0 chi2= 452884.696837 time= 3.8443e-05 cumTime= 3.8443e-05 edges= 74 schur= 0
iteration= 1 chi2= 452762.638918 time= 1.436e-05 cumTime= 5.2803e-05 edges= 74 schur= 0
iteration= 2 chi2= 452762.618632 time= 1.1943e-05
...... 中间略
iteration= 9 chi2= 452762.618615 time= 1.0772e-05 cumTime= 0.000140108 edges= 74 schur= 0
optimization costs time: 0.000528066 seconds.

after optimization:
T=
0.99724 0.0561704 -0.04856 0.708625
-0.0559834 0.998418 0.00520239 -0.277551
0.0487754 -0.00246948 0.998807 -0.155957
0 0 0 1

我们发现只迭代一次后，总体误差就已经稳定不变，说明仅在一次迭代之后算法即已收敛。从位姿求解的结果可以看出，它和前面 SVD 给出的位姿结果几乎一模一样，这说明 SVD 已经给出了优化问题的解析解。所以，本实验中可以认为 SVD 给出的结果是相机位姿的最优值。

需要说明的是，在本例的 ICP 中，我们使用了在两个图都有深度读数的特征点。然而，事实上，只要其中一个图深度确定，我们就能用类似于 PnP 的误差方式，把它们也加到优化中来。同时，除了相机位姿之外，将空间点也作为优化变量考虑，亦是一种解决问题的方式。我们应当清楚，实际的求解是非常灵活的，不必拘泥于某种固定的形式。如果同时考虑点和相机，整个问题就变得更自由了，你可能会得到其他的解。比如，可以让相机少转一些角度，而把点多移动一些。这从另一侧面反映出，在 Bundle Adjustment 里面，我
们会希望有尽可能多的约束，因为多次观测会带来更多的信息，使我们能够更准确地估计每个变量。

7.11 小结

本节介绍了基于特征点的视觉里程计中的几个重要的问题。包括：

1. 特征点是如何提取并匹配的；
2. 如何通过 2D-2D 的特征点估计相机运动；
3. 如何从 2D-2D 的匹配估计一个点的空间位置；
4. 3D-2D 的 PnP 问题，它的线性解法和 Bundle Adjustment 解法；
5. 3D-3D 的 ICP 问题，其线性解法和 Bundle Adjustment 解法。

本章内容较为丰富，且结合应用了前几章的基本知识。读者若觉得理解有困难，可以对前面知识稍加回顾。最好亲自做一遍实验，以理解整个运动估计的内容。

需要解释的是，为保证行文流畅，我们省略了大量的，关于某些特殊情况的讨论。例如，如果在对极几何求解过程中，给定的特征点共面，会发生什么情况（这在单应矩阵 H中提到）？共线又会发生什么情况？在 PnP 和 ICP 中若给定这样的解，又会导致什么情况？求解算法能否识别这些特殊的情况，并报告所得的解可能不可靠？——尽管它们都是值得研究和探索的，然而对它们的讨论势必让本书变得特别繁琐。而且在工程实现中，这些情况甚少出现，所以本书介绍的方法，是指在实际工程中能够有效运行的方法，我们假定了那些少见的情况并不发生。如果你关心这些少见的情况，可以阅读 [3] 等论文，在文献中我们会经常研究一些特殊情况下的解决方案。

习题

1. 除了本书介绍的 ORB 特征点外，你还能找到哪些其他的特征点？请说说 SIFT 或 SURF 的原理，对比它们与 ORB 之间的优劣。
2. 设计程序，调用 OpenCV 中的其他种类特征点。统计在提取 1000 个特征点时，在你的机器上所用的时间。
3. • 我们发现 OpenCV 提供的 ORB 特征点，在图像当中分布不够均匀。你是否能够找到或提出让特征点分布更加均匀的方法？
4. 研究 FLANN 为何能够快速处理匹配问题。除了 FLANN 之外，还能哪些可以加速匹配的手段？
5. 把演示程序使用的 EPnP 改成其他 PnP 方法，并研究它们的工作原理。
6. 在 PnP 优化中，将第一个相机的观测也考虑进来，程序应如何书写？最后结果会有何变化？
7. 在 ICP 程序中，将空间点也作为优化变量考虑进来，程序应如何书写？最后结果会有何变化？
8. * 在特征点匹配过程中，不可避免地会遇到误匹配的情况。如果我们把错误匹配输入到 PnP 或 ICP 中，会发生怎样的情况？你能想到哪些避免误匹配的方法？
9. * 使用 Sophus 的 SE3 类，自己设计 g2o 的节点与边，实现 PnP 和 ICP 的优化。
10. * 在 Ceres 中实现 PnP 和 ICP 的优化。