视觉SLAM十四讲学习笔记-第七讲-视觉里程计-PnP和实践

专栏汇总

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（2）---非线性优化应用_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点发和特征提取和匹配实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-单应矩阵和实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践_goldqiu的博客-CSDN博客

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，直接线性变换（DLT）， EPnP（Efficient PnP），UPnP等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment。

7.7.1 直接线性变换

考虑某个空间点 P，它的齐次坐标为 P = (X, Y, Z, 1)T。在图像 I1 中，投影到特征点 x1 = (u1, v1, 1)T（以归一化平面齐次坐标表示）。此时相机的位姿 R, t 是未知的。与单应矩阵的求解类似，定义增广矩阵 [R|t] 为一个 3 × 4 的矩阵，包含了旋转与平移信息。展开形式列写如下：

用最后一行把 s 消去，得到两个约束：

定义 T 的行向量： t1 = (t1, t2, t3, t4) T , t2 = (t5, t6, t7, t8) T , t3 = (t9, t10, t11, t12) T , 于是有：

t 是待求的变量，可以看到每个特征点提供了两个关于 t 的线性约束。假设一共有 N 个特征点，可以列出线性方程组：

由于 t 一共有 12 维，因此最少通过六对匹配点，即可实现矩阵 T 的线性求解，这种方法称为直接线性变换（Direct Linear Transform，DLT）。当匹配点大于六对时，可以使用 SVD 等方法对超定方程求最小二乘解。在 DLT 求解中，直接将 T 矩阵看成了 12 个未知数，忽略了它们之间的联系。因为旋转矩阵 R ∈ SO(3)，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。平移向量属于向量空间，对于旋转矩阵 R，必须针对 DLT 估计的 T 的左边寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成，相当于把结果从矩阵空间重新投影到 SE(3) 流形上，转换成旋转和平移两部分。这里的 x1 使用了归一化平面坐标，去掉了内参矩阵 K 的影响 ——这是因为内参 K 在 SLAM 中通常假设为已知。如果内参未知，也能用 PnP 去估计 K, R, t 三个量。然而由于未知量的增多，效果会差一些。

7.7.2 P3P

P3P仅使用三对匹配点，对数据要求较少，需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D 点为 A, B, C，2D 点为 a, b, c，其中小写字母代表的点为大写字母在相机成像平面上的投影，如图所示。

此外，P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 D − d，相机光心为 O。注意，预知的是 A, B, C 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。一旦3D 点在相机坐标系下的坐标能够算出，就得到了 3D-3D 的对应点，把 PnP 问题转换为了 ICP 问题。三角形之间存在对应关系：

利用余弦定理，有：

对上面三式全体除以

并且记 x = OA/OC, y = OB/OC，得：

记

有：

化为：

由于知道 2D 点的图像位置，三个余弦角 cos〈a, b〉, cos〈b, c〉, cos〈a, c〉 是已知的。同时，

可以通过 A, B, C 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的 x, y 是未知的，随着相机移动会发生变化。因此，该方程组是关于 x, y 的一个二元二次方程（多项式方程）。解析地求解该方程组是一个复杂的过程，需要用吴消元法。类似于分解 E 的情况，该方程最多可能得到四个解，但可以用验证点来计算最可能的解，得到 A, B, C 在相机坐标系下的 3D 坐标。然后，根据 3D-3D 的点对，计算相机的运动 R, t。

从 P3P 的原理上可以看出，为了求解 PnP，利用了三角形相似性质，求解投影点 a, b, c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。

P3P 也存在着一些问题：

1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。

2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。所以后续人们还提出了许多别的方法，如 EPnP、UPnP 等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。

7.7.3 Bundle Adjustment

除了使用线性方法之外，可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。这是一种非常通用的求解方式，可以用它对 PnP 或 ICP 给出的结果进行优化。在 PnP 中，这个 Bundle Adjustment 问题，是一个最小化重投影误差（Reprojection error）的问题。

考虑 n 个三维空间点 P 和它们的投影 p，希望计算相机的位姿 R, t，它的李代数表示为 ξ。假设某空间点坐标为 Pi = [Xi , Yi , Zi ]T，其投影的像素坐标为 ui = [ui , vi ]T。像素位置与空间点位置的关系如下：

写成矩阵形式是：

中间隐含着齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的。因为exp (ξ∧) Pi 结果是 4 × 1 的，而它左侧的 K 是 3 × 3 的，所以必须把 exp (ξ ∧) Pi 的前三维取出来，变成三维的非齐次坐标。现在，由于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

该问题的误差项，是将像素坐标（观测到的投影位置）与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称之为重投影误差。使用齐次坐标时，这个误差有 3 维。不过，由于u最后一维为 1，该维度的误差一直为零，因而更多时候使用非齐次坐标，于是误差就只有 2 维了。

如图所示，通过特征匹配，知道了 p1 和 p2 是同一个空间点 P 的投影，但是不知道相机的位姿。在初始值中，P 的投影 pˆ2 与实际的 p2 之间有一定的距离。于是调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。

使用李代数，可以构建无约束的优化问题，很方便地通过 G-N, L-M 等优化算法进行求解。不过，在使用 G-N 和 L-M 之前，需要知道每个误差项关于优化变量的导数，也就是线性化： e(x + ∆x) ≈ e(x) + J∆x.

这里的 J 的形式是关键所在。固然可以使用数值导数，但如果能够推导解析形式时，会优先考虑解析导数。现在，当 e 为像素坐标误差（2 维），x 为相机位姿（6 维）时，J 将是一个 2 × 6 的矩阵。

使用扰动模型来求李代数的导数：

首先，记变换到相机坐标系下的空间点坐标为 P′，并且将其前三维取出来： P′ = (exp (ξ∧)P)1:3 = [X′,Y′,Z′]T

相机投影模型相对于P′则为： su = KP′

展开：

利用第 3 行消去 s（实际上就是 P′ 的距离），得：

这与相机模型是一致的。当求误差时，可以把这里的 u, v 与实际的测量值比较，求差。定义了中间变量后对 ξ∧左乘扰动量δξ，然后考虑 e 的变化关于扰动量的导数。利用链式法则，可以列写如下：

这里的⊕指李代数上的左乘扰动。第一项是误差关于投影点的导数，得：

第二项为变换后的点关于李代数的导数，得：

而在 P′ 的定义中，取出了前三维，于是得：

将这两项相乘，就得到了 2 × 6 的雅可比矩阵：

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。保留了前面的负号，因为这是由于误差是由观测值减预测值定义的。它当然也可反过来，定义成 “预测减观测”的形式。在这种情况下，只要去掉前面的负号即可。此外，如果 se(3) 的定义方式是旋转在前，平移在后时，只要把这个矩阵的前三列与后三列对调即可。

另一方面，除了优化位姿，还希望优化特征点的空间位置。因此，需要讨论 e 关于空间点 P 的导数。仍利用链式法则，有：

第二项，按照定义 P′ = exp(ξ∧)P = RP + t.

发现 P′ 对 P 求导后只剩下 R。于是:

以上是观测相机方程关于相机位姿与特征点的两个导数矩阵。它们能够在优化过程中提供重要的梯度方向，指导优化的迭代。

7.8 实践：求解 PnP

7.8.1 使用 EPnP 求解位姿

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

代码为: slambook/ch7/pose_estimation_3d2d.cpp

在例程中，得到配对特征点后，在第一个图的深度图中寻找它们的深度，并求出空间位置。以此空间位置为 3D 点，再以第二个图像的像素位置为 2D 点，调用 EPnP 求解 PnP 问题。程序输出后可以看到，在有3D信息时，估计的 R 几乎是相同的，而 t 相差的较多。这是由于引入了新的深度信息所致。不过，由于 Kinect 采集的深度图本身会有一些误差，所以这里的 3D 点也不是准确的。后面会希望把位姿 ξ 和所有三维特征点 P 同时优化。

7.8.2 使用 BA 优化

使用前一步的估计值作为初始值，把问题建模成一个最小二乘的图优化问题，如图所示。

在这个图优化中，节点和边的选择为：

节点：第二个相机的位姿节点 ξ ∈ se(3)，以及所有特征点的空间位置 P ∈ R3。
边：每个 3D 点在第二个相机中的投影，以观测方程来描述： zj = h(ξ, Pj ).

由于第一个相机位姿固定为零，没有把它写到优化变量里。现在根据一组 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 函数）等操作。
空间点位置类的维度为 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;}

它与公式是一致的。成员变量“- jacobianOplusXi”是误差到空间点的导数，“jacobianOplusXj”是误差到相机位姿的导数，以李代数的左乘扰动表达。稍有差别的是，g2o 的相机里用 f 统一描述 fx, fy，并且李代数定义顺序不同（g2o 是旋转在前，平移在后），所以矩阵前三列和后三列与上面的定义是颠倒的。

们在上一个 PnP 例程的基础上，加上 g2o 提供的 Bundle Adjustment：

首先声明了 g2o 图优化，配置优化求解器和梯度下降方法，然后根据估计到的特征点，将位姿和空间点放到图中。最后调用优化函数进行求解。

优化结果：迭代 11 轮后，LM 发现优化目标函数接近不变，于是停止了优化。输出了最后得到位姿变换矩阵 T，对比之前直接做 PnP 的结果，大约在小数点后第三位发生了一些变化。这主要是由于同时优化了特征点和相机位姿导致的。

Bundle Adjustment 是一种通用的做法。它可以不限于两个图像。可以放入多个图像匹配到的位姿和空间点进行迭代优化，甚至可以把整个 SLAM 过程放进来。那种做法规模较大，主要在后端使用。在前端，我们通常考虑局部相机位姿和特征点的小型 Bundle Adjustment 问题，希望实时对它进行求解和优化。