[人工智能]12_视觉里程计1_ICP算法

前言

ICP的英文全称为Iterative Closest Point，即为迭代最近点。它在激光雷达应用频率很高，主要是在点云配准领域。ICP算法在是是视觉SLAM中应用也非常多，这个算法还是很重要。我们下面的讨论还是基于视觉SLAM，好了我们开始吧！

ICP算法流程

ICP算法顾名思义，就是找最近点。算法流程如下：

• step1：预处理点云
• step2：寻找对应点（最近点）
• step3：根据对应点，计算R和t
• step4：对点云进行转换，计算误差
• step5：不断迭代，直至误差小于某一个值

3D-3D: ICP

假设我们有一组配对好的3D点如下：

P = { p 1 , ? ? , p n } , P ′ = { p 1 ′ , ? ? , p n ′ } \boldsymbol{P}=\left\{\boldsymbol{p}_{1}, \cdots, \boldsymbol{p}_{n}\right\}, \quad \boldsymbol{P}^{\prime}=\left\{\boldsymbol{p}_{1}^{\prime}, \cdots, \boldsymbol{p}_{n}^{\prime}\right\} P={p1?,?,pn?},P′={p1′?,?,pn′?}

现在我们想要找到一个欧式变换，使得

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

上述的这个问题就可以使用ICP算法求解。这里主要需要注意下，如果仅考虑3D点之间的变换，此时和相机并没有关系。在RGB-D SLAM中用ICP问题指代匹配好的两组点间的运动估计问题（跟激光SLAM有区别，激光SLAM通常是未知的情况）。ICP求解主要是两种方式，一种是SVD，另一种是非线性优化方式求解。

SVD方法

根据前面描述的ICP问题，令第$i$对点的误差为：

$${\mathbf{e}}_i={\mathbf{p}}_i?\left(\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}\right).$$

这里解释一下，我们用一个点 ${\mathbf{p}}_i^{\prime }$（第二幅图中数据）来估算另一个点${\mathbf{p}}_i$时（在第一幅中数据，即为观测坐标），必然会产生误差。

我们构建最小二乘问题，求解使用平方和打到极小的$R$和$t$，则有

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

定义图像1和图像2中两组点的之心为：

$$\mathbf{p}=\frac{1}{n}\sum _{i=1}^n\left({\mathbf{p}}_i\right),{\mathbf{p}}^{\prime }=\frac{1}{n}\sum _{i=1}^n\left({\mathbf{p}}_i^{\prime }\right).$$

在误差函数做如下处理（去质心处理）：

$$\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{R}{\mathbf{p}}_i{}^{\prime }?\mathbf{t}?\mathbf{p}+\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{p}?\mathbf{R}{\mathbf{p}}^{\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}\left({\mathbf{p}}_i^{\prime }?{\mathbf{p}}^{\prime }\right)\Vert }^2+{\Vert \mathbf{p}?\mathbf{R}{\mathbf{p}}^{\prime }?\mathbf{t}\Vert }^2+\\ & 2{\left({\mathbf{p}}_i?\mathbf{p}?\mathbf{R}\left({\mathbf{p}}_i{}^{\prime }?{\mathbf{p}}^{\prime }\right)\right)}^{\mathrm{T}}\left(\mathbf{p}?\mathbf{R}{\mathbf{p}}^{\prime }?\mathbf{t}\right))\end{array}$$

最终优化目标函数简化为

$$\underset{\mathbf{R},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.$$

我们可以先求左边的第一项，我们记去质心坐标为：

$${\mathbf{q}}_i={\mathbf{p}}_i?\mathbf{p},{\mathbf{q}}_i^{\prime }={\mathbf{p}}_i^{\prime }?{\mathbf{p}}^{\prime }.$$

由于左边第一项只和$R$有关，因此我们先计算旋转矩阵$R$，则

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

我们将${\mathbf{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)$$

由于${\mathbf{R}}^{\mathrm{T}}\mathbf{R}=\mathbf{I}$，继续化简优化目标函数变为

$$\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)$$

为了解$R$，定义矩阵：

$$\mathbf{W}=\sum _{i=1}^n{\mathbf{q}}_i{\mathbf{q}}_i^{\prime \mathrm{T}}.$$

使用SVD分解，使得$\mathbf{W}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathrm{T}}$，当$\mathbf{W}$满秩时，则

$$\mathbf{R}=\mathbf{U}{\mathbf{V}}^{\mathrm{T}}$$

解得$R$以后，$t$即为

$$t^{?}=p?R{p}^{\prime }$$

非线性优化方法

李代数表达位姿，迭代方式找到最优值。目标函数为：

$$\underset{\mathbf{\xi }}{\min }=\frac{1}{2}\sum _{i=1}^n{\Vert \left({\mathbf{p}}_i?\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{p}}_i^{\prime }\right)\Vert }_2^2$$

实践：求解ICP

SVD方法

• 原理和前面介绍的一致，代码如下：

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
  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());
  if (R_.determinant() < 0) {
    R_ = -R_;
  }
  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));
}