ICP, Iterative Closest Points

发表于 |

1 基本实现

数据点云配准，最经典的方法就是ICP迭代最近点法。

  • 最近点：欧几里得意义上距离最近的点。
  • 迭代：迭代目标是通过不断更新运动参数，使得两个点云的重叠部分充分吻合。

ICP的求解分为两种方式：

  • 利用线性代数求解（SVD），在给定了匹配的情况下，最小二乘问题实际上具有解析解。
  • 利用非线性优化方式求解，类似于BA方法，适用于匹配未知的情况。

2 SVD方法求解

算法推导如下：

  1. 首先将点云文件进行粗匹配，如ORB特征点匹配。

  2. 从点集$P={\overrightarrow{p_1}, \overrightarrow{p_2}, …, \overrightarrow{p_n}}$中随机选取指定数量的点$\{\overrightarrow{p_t}\}$作为参考点，参考点的数量决定了ICP算法的计算效率和配准精度。

  3. 在另一个点集$Q={\overrightarrow{q_1}, \overrightarrow{q_2}, …, \overrightarrow{q_m}}$是待匹配的点query points，那么想要找到一个欧式变换$R, t$，使得$\forall i, p_i = Rq_i + t$。

  4. 求解欧式变换$T^k$，使得$E^k=\Sigma| \overrightarrow{p_t} - T^k \overrightarrow{q_t}|^2$最小化。 将空间变换分解为旋转和平移两部分，首先定义两个点云的质心：

    于是有目标函数：

    对目标函数展开，而且已知旋转矩阵是正交阵，$R^TR=I​$，所以目标函数的前两项都与$R​$无关：

    只有最后一项与$R$有关，于是得到关于$R$的目标函数：

    然后通过SVD奇异值分解求解上述问题的最优$R$，首先定义$W = \sum_1^n pq^T$，当$W$满秩时：

    然后间接得到平移$t$：

代码实现如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
void pose_estimation_3d3d(
        const vector<Point3f>& pts1,         // point cloud 1
        const vector<Point3f>& pts2,         // point cloud 2
        Mat& R, Mat& t,
        Eigen::Matrix3d& R_, Eigen::Vector3d& 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 COM
    for(int i=0; i<N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();       // calculate W matrix
    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();
    }

    // SVD decomposition
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);         // SVD
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // calculate R,t
    R_ = U * V.transpose();
    t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

3 非线性优化方法

另一种方式是通过迭代的方式来寻找最优值，误差项的表示与上一节相同，用李代数来表达位姿，旋转和平移不用再解耦表示，目标函数为：

单个误差项关于位姿的导数可以使用李代数扰动模型来描述：

其中$p_i$作为参考点，对扰动的导数为0，因此：

将最小二乘问题进行图描述：优化变量为李代数表达的位姿$\xi$，因此图中只有一个节点，误差项为一元边（从当前节点指向当前节点），对误差项做线性展开：

其中的雅可比矩阵也就是上面说的，单个误差项关于位姿的一阶导数。

4算法优化

  • 删除点云数据采集中产生的噪声及异常值。
  • 查找最近点的过程采用KD-Tree数据结构，减少时间复杂度。