视觉SLAM理论到实践系列（六）——特征点法视觉里程计（5）

视觉SLAM理论到实践系列文章

下面是《视觉SLAM十四讲》学习笔记的系列记录的总链接，本人发表这个系列的文章链接均收录于此

视觉SLAM理论到实践系列文章链接

---

下面是专栏地址：

视觉SLAM理论到实践专栏

---

---

前言

高翔博士的《视觉SLAM14讲》学习笔记的系列记录

---

视觉SLAM理论到实践系列（六）——特征点法视觉里程计（5）

3D-3D:ICP

理论部分

最后，我们来介绍 3D-3D 的位姿估计问题。假设我们有一组配对好的 3D 点 ( 例如我们对两幅 RGB-D 图像进行了匹配 ):

P = { p 1 , ⋯   , p n } , P ′ = { p 1 ′ , ⋯   , p n ′ } , \boldsymbol{P}=\{\boldsymbol{p}_{1},\cdots,\boldsymbol{p}_{n}\},\quad\boldsymbol{P}^{\prime}=\{\boldsymbol{p}_{1}^{\prime},\cdots,\boldsymbol{p}_{n}^{\prime}\}, P={p1​,⋯,pn​},P′={p1′​,⋯,pn′​},

现在，想要找一个欧氏变换 $R,t$, 使得

$$\forall i,{\mathbf{p}}_i=\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}.$$

这个问题可以用迭代最近点( Iterative Closest Point, ICP)求解。读者应该注意到了，3D-3D 位姿估计问题中并没有出现相机模型，也就是说，仅考虑两组 3D 点之间的变换时，和相机并没有关系。

因此，在激光 SLAM 中也会碰到 ICP, 不过由于激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。

而在视觉中，特征点为我们提供了较好的匹配关系，所以整个问题就变得更简单了。在RGB-D SLAM 中，可以用这种方式估计相机位姿。下文我们用 ICP 指代匹配好的两组点间的运动估计问题。

和 PnP 类似，ICP 的求解也分为两种方式； 利用线性代数的求解(主要是 SVD), 以及利用非线性优化方式的求解（类似于 BA）。下面分别进行介绍。

SVD之ICP

差项：

首先来看以 SVD 为代表的代数方法。根据前面描述的 ICP 问题，我们先定义第 $i$ 对点的误差项：

$$e_i=p_i-(R{p}_i^{\prime }+t).$$
然后，构建最小二乘问题，求使误差平方和达到极小的 $R,t:$

$$\underset{\mathbf{R},\mathbf{t}}{\min }\frac{1}{2}\sum _{i=1}^n\Vert ({\mathbf{p}}_i-({\mathbf{Rp}}_i{}^{\prime }+\mathbf{t})){\Vert }_2^2.$$

下面来推导它的求解方法。首先，定义两组点的质心：

$$\begin{array}{r}\mathbf{p}=\frac{1}{n}\sum _{i=1}^n({\mathbf{p}}_i),{\mathbf{p}}^{\prime }=\frac{1}{n}\sum _{i=1}^n({\mathbf{p}}_i^{\prime }).\end{array}$$
请注意，质心是没有下标的。随后，在误差函数中做如下处理：

$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_i-\left(\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}\right)\Vert }^2& =\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_i-{\mathbf{Rp}}_i{}^{\prime }-\mathbf{t}-\mathbf{p}+{\mathbf{Rp}}^{\prime }+\mathbf{p}-{\mathbf{Rp}}^{\prime }\Vert }^2\\ & =\frac{1}{2}\sum _{i=1}^n{\Vert \left({\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i^{\prime }-{\mathbf{p}}^{\prime }\right)\right)+\left(\mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\right)\Vert }^2\\ & {=\frac{1}{2}\sum _{i=1}^n(\Vert {\mathbf{p}}_i-\mathbf{p}-\mathbf{R}{({\mathbf{p}}_i}^{\prime }-{\mathbf{p}}^{\prime })\Vert }^2+{\Vert \mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\Vert }^2+\\ & 2({\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i^{\prime }-{\mathbf{p}}^{\prime }\right){)}^{\mathrm{T}}(\mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t})).\end{array}$$
注意到交叉项部分中 $(p_i-p-R(p_i^{\prime }-p^{\prime }))$ 在求和之后为零，因此优化目标函数可以简化为
$$\underset{\mathbf{R},\mathbf{t}}{\min }J=\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i^{\prime }-{\mathbf{p}}^{\prime }\right)\Vert }^2+{\Vert \mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\Vert }^2.$$
仔细观察左右两项，我们发现左边只和旋转矩阵 $R$ 相关，而右边既有 $R$ 也有 $t$，但只和质心相关。只要我们获得了 $R$，令第二项为零就能得到 $t$。

于是，ICP 可以分为以下三个步骤求解：

1. 计算两组点的质心位置 $p,p^{\prime }$,然后计算每个点的去质心坐标：

$$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }.$$

2. 根据以下优化问题计算旋转矩阵：

$${\mathbf{R}}^{\ast }=\arg \underset{\mathbf{R}}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{q}}_i-\mathbf{R}{\mathbf{q}}_i^{\prime }\Vert }^2.$$

3. 根据第2步的 $R$ 计算 $t$：

$$t^{\ast }=p-R{p}^{\prime }.$$

我们看到，只要求出了两组点之间的旋转，平移量是非常容易得到的。所以我们重点关注 $R$ 的计算。展开关于 $R$ 的误差项，得

$$\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{q}}_i-\mathbf{R}{\mathbf{q}}_i^{\prime }\Vert }^2=\frac{1}{2}\sum _{i=1}^n\left({\mathbf{q}}_i^{\mathrm{T}}{\mathbf{q}}_i+{\mathbf{q}}_i^{\prime \mathrm{T}}{\mathbf{R}}^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_i^{\prime }-2{\mathbf{q}}_i^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_i^{\prime }\right).$$

注意到第一项和 $R$ 无关，第二项由于 $R^{T}R=I$，亦与 $R$ 无关。因此，实际上优化目标函数变为

$$\sum _{i=1}^n-{\mathbf{q}}_i^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_i^{\prime }=\sum _{i=1}^n-\mathrm{tr}\left(\mathbf{R}{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}\right)=-\mathrm{tr}\left(\mathbf{R}\sum _{i=1}^n{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}\right).$$

因为有
$$\begin{array}{rl}{a}^{T}b& =tr(a{b}^T)\\ & \\ \therefore (q_i{}^{T}R){q_i}^{\prime }& =tr((R^T{q}_i){q_i^{\prime }}^T)=tr((R^T{q}_i{}^{\prime }){q_i}^T)\end{array}$$

接下来，我们介绍怎样通过 SVD 解出上述问题中最优的 $R$。关于最优性的证明较为复杂，感兴趣的读者请阅读参考文献[62,63]。为了解 $R$，先定义矩阵：
$$\mathbf{W}=\sum _{i=1}^n{\mathbf{q}}_i{\mathbf{q}}_i^{\prime \text{Т}}.$$
$W$ 是一个$3\times 3$ 的矩阵，对 $W$ 进行 SVD 分解，得

$$W=U\Sigma V^{\mathrm{T}}.$$
其中，$\Sigma$ 为奇异值组成的对角矩阵，对角线元素从大到小排列，而$U$ 和$V$为对角矩阵。当 $W$ 满秩时，$R$为
$$R=U{V}^{\mathrm{T}}.$$
解得 $R$ 后，按式 (7.54) 求解 $t$ 即可。如果此时 $R$ 的行列式为负，则取 $-R$ 作为最优值。

补充证明

书中（7.56）式证明，即
$$\sum _{i=1}^n-{\mathbf{q}}_i^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_i^{\prime }=\sum _{i=1}^n-\mathrm{tr}\left(\mathbf{R}{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}\right)=-\mathrm{tr}\left(\mathbf{R}\sum _{i=1}^n{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}\right).$$
证明如下：
$$\begin{array}{rl}{\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\min }-\sum _{i=1}^n\left({\mathbf{q}}_i^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_i^{\prime }\right)\\ & =\arg \underset{\mathbf{R}}{\max }\sum _{i=1}^n\left({\mathbf{q}}_i^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_i^{\prime }\right)\\ & =\arg \underset{\mathbf{R}}{\max }\sum _{i=1}^n\mathrm{tr}\left(\mathbf{R}{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}\right)\\ & =\arg \underset{\mathbf{R}}{\max }\mathrm{tr}\left(\mathbf{R}\sum _{i=1}^n{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}\right)\end{array}$$
令 ${\sum }_{i=1}^n{\mathbf{q}}_i^{\prime }{\mathbf{q}}_i^{\mathrm{T}}=\mathbf{W}$

$\because$ 现在的问题变成找到一个 ${\mathbf{R}}^{\ast }$ 使得 $\mathrm{tr}\left({\mathbf{R}}^{\ast }\mathbf{W}\right)$ 取得最大值

$\therefore$ 即 $\mathrm{tr}\left({\mathbf{R}}^{\ast }\mathbf{W}\right)\ge \mathrm{tr}\left(\mathbf{B}{\mathbf{R}}^{\ast }\mathbf{W}\right)$

这里的 $\mathbf{B}$ 为引入的辅助矩阵，表示 ${\mathbf{R}}^{\ast }$ 的领域（ $\mathbf{B}$ 为正交矩阵）

$\because$ 存在定理 $\mathrm{tr}\left(\mathbf{A}{\mathbf{A}}^T\right)\ge \mathrm{tr}\left(\mathbf{B}\mathbf{A}{\mathbf{A}}^T\right)$，其中 $\mathbf{B}{\mathbf{B}}^T=\mathbf{I}$

$\therefore$ 现在的问题提转化为找到一个 ${\mathbf{R}}^{\ast }$ 使得
$${\mathbf{R}}^{\ast }\mathbf{W}=\mathbf{A}{\mathbf{A}}^T$$
那么对 $\mathbf{W}$ 进行 SVD分解，有
$$\mathbf{W}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathrm{T}}$$
则当 ${\mathbf{R}}^{\ast }=\mathbf{V}{\mathbf{U}}^{\mathrm{T}}$ 时
$${\mathbf{R}}^{\ast }\mathbf{W}=\mathbf{V}{\mathbf{U}}^{\mathrm{T}}\cdot \mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathrm{T}}=\mathbf{V}\mathbf{\Sigma }{\mathbf{V}}^{\mathrm{T}}$$
令 $\mathbf{A}=\mathbf{V}{\mathbf{\Sigma }}^{\frac{1}{2}}$

则
$$\mathbf{A}{\mathbf{A}}^T=\mathbf{V}{\mathbf{\Sigma }}^{\frac{1}{2}}\cdot (\mathbf{V}{\mathbf{\Sigma }}^{\frac{1}{2}}{)}^T=\mathbf{V}\mathbf{\Sigma }{\mathbf{V}}^T={\mathbf{R}}^{\ast }\mathbf{W}$$
$\therefore$ 取 ${\mathbf{R}}^{\ast }=\mathbf{V}{\mathbf{U}}^{\mathrm{T}}$ 时为最优解

参考：

ICP 的 SVD 解法

矩阵的奇异值(SVD)分解及其简单应用

非线性优化之ICP

求解 ICP 的另一种方式是使用非线性优化，以迭代的方式去找最优值。该方法和我们前面讲述的 PnP 非常相似。以李代数表达位姿时，目标函数可以写成

$$\underset{\mathbf{\xi }}{\min }=\frac{1}{2}\sum _{i=1}^n\Vert ({\mathbf{p}}_i-\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{p}}_i^{\prime }){\Vert }_2^2.$$
单个误差项关于位姿的导数在前面已推导，使用李代数扰动模型即可：

$$\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=-{\left(\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{p}}_i{}^{\prime }\right)}^{\odot }.$$

前面已经讲过
$$\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right]\stackrel{\mathrm{def}}{=}(Tp{)}^{\odot }.$$

于是，在非线性优化中只需不断迭代，就能找到极小值。而且，可以证明${}^{[6]}$, ICP 问题存在唯一解或无穷多解的情况。

在唯一解的情况下，只要能找到极小值解，这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况。

这也意味着 ICP 求解可以任意选定初始值。这是已匹配点时求解 ICP 的一大好处。

需要说明的是，我们这里讲的 ICP 是指已由图像特征给定了匹配的情况下进行位姿估计的问题。在匹配已知的情况下，这个最小二乘问题实际上具有解析解${}^{[64-66]}$,所以并没有必要进行迭代优化。

ICP 的研究者们往往更加关心匹配未知的情况。那么，为什么我们要介绍基于优化的 ICP 呢？

这是因为，某些场合下，例如在 RGB-D SLAM 中，一个像素的深度数据可能有，也可能测量不到，所以我们可以混合着使用 PnP 和 ICP 优化： 对于深度已知的特征点，建模它们的 3D-3D 误差；对于深度未知的特征点，则建模 3D-2D 的重投影误差。

于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。

实践部分：求解ICP

SVD之ICP

十四讲7.10.1

代码在第7章 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 = Point3f(Vec3f(p1) / N);      // 计算质心
  p2 = Point3f(Vec3f(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        计算 W=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();
      // 这里 w+ = q1 * q2^T, R = V * U^T算出的是正变换，U * V^T 是逆变换
  }
  cout << "W=" << W << endl;

  // SVD on W       SVD 分解
  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;
  // 根据 U V 求解 R,是逆变换

  Eigen::Matrix3d R_ = U * (V.transpose());
  if (R_.determinant() < 0) {
    R_ = -R_;
  }
  // t = p1 - R * p2
  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=R{p}_i^{\prime }+t$ 进行的，这里的 $R,t$ 是第二帧到第一帧的变换，与前面 PnP 部分相反。

在输出结果中，我们同时打印了逆变换：

非线性优化之ICP

代码在第7章 pose_estimation_3d3d.cpp 中

// 定义优化节点，继承BaseVertex的模板基类，模板参数为 优化变量的维度，数据类型
/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  // Eigen库为了使用SSE加速，在内存上分配了128位的指针，涉及字节对齐问题，出问题时在输译时不会报错，
  // 只在运行时报错，写下这一句能够让编译器自动将数据对齐
  // SSE指令集是英特尔提供的基于SIMD(单指令多数据，也就是说同一时间内，对多个不同的数据执行同一条命令)的
  // 硬件加速指令，通过使用寄存器来进行并行加建

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();     // 初始化
                                    // 得一提的是，Sophus::SE3 内部是以 SO3 + trans存储的
                                    // SO3内部存储的是一个四元数
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {       // update增量
    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 {}
};

// 定义g2o优化的边        观测值维度，观测值类型，顶点类型
/// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;      // 内存对齐

  // 初始化
  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}

  virtual void computeError() override {        // 计算误差
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );      // 取出单边的节点--pose
    _error = _measurement - pose->estimate() * _point;      // e = p' - Tp
  }

  virtual void linearizeOplus() override {      // 计算雅可比矩阵
      /*
       *    J = - [  I    -(p')^
       *             0^T    0^T  ]    (4*6)
       */
    VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = pose->estimate();
    Eigen::Vector3d xyz_trans = T * _point;
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
  }

  bool read(istream &in) {}

  bool write(ostream &out) const {}

protected:
  Eigen::Vector3d _point;
};