ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。如下图所示，PR（红色点云）和RB（蓝色点云）是两个点集，该算法就是计算怎么把PB平移旋转，使PB和PR尽量重叠。

用数学语言描述如下，即ICP算法的实质是基于最小二乘法的最优匹配，它重复进行“确定对应关系的点集→计算最优刚体变换”的过程，直到某个表示正确匹配的收敛准则得到满足。

对于给定匹配点云集合：

\[ X = {x_1, x_2, \cdots , x_{nx}} \]

\[ P = {p_1, p_2, \cdots , p_{np}} \]

求解R和t，使得下式最小：

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - Rp_i - t ||^2 \]

基本ICP算法

已知对应点匹配求解方法

计算两个点集X和P的质心(平均)：

\[ \mu_x = \frac{1}{N_x} \sum_{i = 1}{N_x} x_i \]

\[ \mu_p = \frac{1}{N_p} \sum_{i = 1}{N_p} p_i \]

然后在两个点集中分别减去对应的质心:

\[ X^{`} = \{ x_i - \mu_x \} = {x_i^`} \]

\[ P^{`} = \{ p_i - \mu_p \} = {p_i^`} \]

SVD分解：

\[ W = \sum_{i = 1}^{N_p} x_i^` {p_i^`}^T = U \begin{bmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{bmatrix} V^T \]

则ICP解为：

\[ R = VU^T \]

\[ t = \mu_x - R\mu_p \]

推到如下：

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - Rp_i - t ||^2 \]

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - Rp_i - t -\mu_x + R \mu_{p} + \mu_x - R\mu_p ||^2 \]

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - \mu_x - R(p_i - \mu_p) + (\mu_x - R\mu_p - t) ||^2 \]

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - \mu_x - R(p_i - \mu_p) ||^2+ ||(\mu_x - R\mu_p - t) ||^2 + 2(x_i - \mu_x - R(p_i - \mu_p))^T(\mu_x - R\mu_p - t) \]

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - \mu_x - R(p_i - \mu_p) ||^2+ ||(\mu_x - R\mu_p - t) ||^2 \]

其中：

\(|| x_i - \mu_x - R(p_i - \mu_p) ||^2\)只与R有关，当已知R是可以根据\(\mu_x - R\mu_p - t\)求解t；

转换最小化函数：

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - \mu_x - R(p_i - \mu_p)||^2 \]

进一步的，

\[ E(R, t) = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i - \mu_x - R(p_i - \mu_p)||^2 \]

\[ = \frac{1}{N_p} \sum_{i = 1}^{N_p} || x_i^` - Rp_i^` ||^2 = \frac{1}{N_p} \sum_{i = 1}^{N_p} {x_i^`}^T x_i^1 + {p_i^`}^TR^TRp_i^` - 2x_i^`Rp_i^` = \sum_{i = 1}^{N-p} -2{x_i^`}^TRp_i^` \]

\[ \sum_{i = 1}^{N_p} {x_i^`}^TRp_i^` = \sum_{i = 1}^{N_p} Trace(Rx_i^`{p_i^`}^T) = Trace(RH) \]

\[ H=\sum_{i= 1}^{N_p}x_i^`{p_i^`}^T \]

由定理，假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有\(Trace(A) \geq Trace(BA)\)

\[ H = U \Lambda V^T \, \, \, X=VU^T \]

\[ XH = VU^TU \Lambda V^T = V \Lambda V^T \]

则：

\[ Trace(XH) \geq Trace(BXH) \]

因此：\(R = X = VU^T \, \, \, t = \mu_x - R\mu_p\)

Code

#include <iostream>
#include <cstdlib>
#include <ctime>
#include <Eigen/Eigen>

Eigen::Isometry3d icp_point2point(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &reference, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &current)
{
    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
    if (reference.size() != current.size()) {
        return transform;
    }
    int N = reference.size();
    Eigen::Map<Eigen::Matrix3Xd> ps(&reference[0].x(), 3, N); //maps vector<Vector3d>
    Eigen::Map<Eigen::Matrix3Xd> qs(&current[0].x(), 3, N); //to Matrix3Nf columnwise
    Eigen::Vector3d p_dash = ps.rowwise().mean();
    Eigen::Vector3d q_dash = qs.rowwise().mean();
    Eigen::Matrix3Xd ps_centered = ps.colwise() - p_dash;
    Eigen::Matrix3Xd qs_centered = qs.colwise() - q_dash;
    Eigen::Matrix3d K = qs_centered * ps_centered.transpose();
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(K, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d R = svd.matrixU()*svd.matrixV().transpose();
    if (R.determinant()<0) {
        R.col(2) *= -1;
    }
    transform.linear() = R;
    transform.translation() = q_dash - R*p_dash;
    return transform;
}

int main(int argc, const char **argv)
{
    const std::uint32_t points_size = 20;
    const float points_range = 20.;
    Eigen::AngleAxisd rotation = Eigen::AngleAxisd(3.141592, Eigen::Vector3d::UnitX());
    Eigen::Vector3d translation = Eigen::Vector3d(1., 2., 3.);
    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
    transform.linear() = rotation.toRotationMatrix();
    transform.translation() = translation;
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> reference, current;
    std::srand((unsigned int)(std::time(NULL)));
    for (int pos = 0; pos < points_size; pos++)
    {
        Eigen::Vector3d point;
        point.x() = (std::rand() % (int)(points_range * 2 * 1000.) - points_range * 1000.) / 1000.;
        point.y() = (std::rand() % (int)(points_range * 2 * 1000.) - points_range * 1000.) / 1000.;
        point.z() = (std::rand() % (int)(points_range * 2 * 1000.) - points_range * 1000.) / 1000.;
        std::printf("reference[%d]: %8.5f %8.5f %8.5f.\n", pos, point.x(), point.y(), point.z());
        reference.push_back(point);
    }
    for (int pos = 0; pos < reference.size(); pos++)
    {
        Eigen::Vector4d point;
        point.x() = reference.at(pos).x();
        point.y() = reference.at(pos).y();
        point.z() = reference.at(pos).z();
        point.w() = 1;
        point = transform * point;
        std::printf("current[%d]: %8.5f %8.5f %8.5f.\n", pos, point.x(), point.y(), point.z());
        current.push_back(Eigen::Vector3d(point.x(), point.y(), point.z()));
    }
    {
        auto estimate = icp_point2point(reference, current);
        std::printf("truth: translation: %8.5f %8.5f %8.5f.\n",
            transform.translation().x(), transform.translation().y(), transform.translation().z());
        std::printf("estimate: translation: %8.5f %8.5f %8.5f.\n",
            estimate.translation().x(), estimate.translation().y(), estimate.translation().z());
    }
    return 0;
}

未知匹配关系求解方法

对于未知匹配关系，不能一步到位计算出R和t，需要惊醒迭代运算。

算法流程：

1. 寻找对应点
2. 根据对应点，计算R和t
3. 对点云进行转换，计算误差
4. 不断迭代，直至误差小于阈值

Code

#include <iostream>
#include <random>
#include <cstdlib>
#include <ctime>
#include <Eigen/Eigen>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/icp/types_icp.h>

class Sample
{
    static std::default_random_engine gen_real;
    static std::default_random_engine gen_int;
public:
    static int uniform(int from, int to)
    {
        std::uniform_int_distribution<int> unif(from, to);
        int sam = unif(gen_int);
        return  sam;
    }
    static double uniform()
    {
        std::uniform_real_distribution<double> unif(0.0, 1.0);
        double sam = unif(gen_real);
        return  sam;
    }
    static double gaussian(double sigma)
    {
        std::normal_distribution<double> gauss(0.0, sigma);
        double sam = gauss(gen_real);
        return  sam;
    }
};
std::default_random_engine Sample::gen_real;
std::default_random_engine Sample::gen_int;

int main(int argc, const char **argv)
{
    double euc_noise = 0.01;       // noise in position, m
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(true);

    // variable-size block solver
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolverX>(g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));
    optimizer.setAlgorithm(solver);

    std::vector<Eigen::Vector3d> true_points;
    for (size_t i = 0; i<1000; ++i)
    {
        true_points.push_back(Eigen::Vector3d((Sample::uniform() - 0.5) * 3, Sample::uniform() - 0.5, Sample::uniform() + 10));
    }

    // set up two poses
    int vertex_id = 0;
    for (size_t i = 0; i<2; ++i)
    {
        // set up rotation and translation for this node
        Eigen::Vector3d t(0, 0, i);
        Eigen::Quaterniond q;
        q.setIdentity();

        Eigen::Isometry3d cam; // camera pose
        cam = q;
        cam.translation() = t;

        // set up node
        g2o::VertexSE3 *vc = new g2o::VertexSE3();
        vc->setEstimate(cam);

        vc->setId(vertex_id);      // vertex id

        std::cerr << t.transpose() << " | " << q.coeffs().transpose() << std::endl;

        // set first cam pose fixed
        if (i == 0)
            vc->setFixed(true);

        // add to optimizer
        optimizer.addVertex(vc);

        vertex_id++;
    }

    // set up point matches
    for (size_t i = 0; i<true_points.size(); ++i)
    {
        // get two poses
        g2o::VertexSE3* vp0 =
            dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(0)->second);
        g2o::VertexSE3* vp1 =
            dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(1)->second);

        // calculate the relative 3D position of the point
        Eigen::Vector3d pt0, pt1;
        pt0 = vp0->estimate().inverse() * true_points[i];
        pt1 = vp1->estimate().inverse() * true_points[i];

        // add in noise
        pt0 += Eigen::Vector3d(Sample::gaussian(euc_noise),
            Sample::gaussian(euc_noise),
            Sample::gaussian(euc_noise));

        pt1 += Eigen::Vector3d(Sample::gaussian(euc_noise),
            Sample::gaussian(euc_noise),
            Sample::gaussian(euc_noise));

        // form edge, with normals in varioius positions
        Eigen::Vector3d nm0, nm1;
        nm0 << 0, i, 1;
        nm1 << 0, i, 1;
        nm0.normalize();
        nm1.normalize();

        g2o::Edge_V_V_GICP * e           // new edge with correct cohort for caching
            = new g2o::Edge_V_V_GICP();

        e->setVertex(0, vp0);      // first viewpoint

        e->setVertex(1, vp1);      // second viewpoint

        g2o::EdgeGICP meas;
        meas.pos0 = pt0;
        meas.pos1 = pt1;
        meas.normal0 = nm0;
        meas.normal1 = nm1;

        e->setMeasurement(meas);
        //        e->inverseMeasurement().pos() = -kp;

        meas = e->measurement();
        // use this for point-plane
        e->information() = meas.prec0(0.01);

        // use this for point-point 
        //    e->information().setIdentity();

        //    e->setRobustKernel(true);
        //e->setHuberWidth(0.01);

        optimizer.addEdge(e);
    }

    // move second cam off of its true position
    g2o::VertexSE3* vc =
        dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(1)->second);
    Eigen::Isometry3d cam = vc->estimate();
    cam.translation() = Eigen::Vector3d(0, 0, 0.2);
    vc->setEstimate(cam);

    optimizer.initializeOptimization();
    optimizer.computeActiveErrors();
    std::cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << std::endl;

    optimizer.setVerbose(true);

    optimizer.optimize(5);

    std::cout << std::endl << "Second vertex should be near 0,0,1" << std::endl;
    std::cout << dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(0)->second)
        ->estimate().translation().transpose() << std::endl;
    std::cout << dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(1)->second)
        ->estimate().translation().transpose() << std::endl;
    return 0;
}

改进ICP算法

PL-ICP

对于激光雷达前端匹配，使用ICP具有一定的缺陷，激光雷达的点并不是配对的，使用ICP计算出来的变换矩阵具有偏差。

对于PP-ICP，误差是以点对点之间的距离作为误差。

对于PL-ICP误差计算并不是点之间的距离，计算的是两点组成直线，计算点到直线的距离作为误差。

参考：https://censi.science/pub/research/2008-icra-plicp.pdf

The main contribution of this paper is the use of a point-to-line metric instead of the point-to-point metric used by vanilla ICP. Call n i the normal to the surface at the projected point. Then, the point-to-line metric is written as:

\[ \min{q_{k + 1}} \sum_{i}(n_i^T[p_i \oplus q_{k + 1} - II{S^{ref}, p_i \oplus q_k}])^2 \]

实现：https://github.com/AndreaCensi/csm

---

Reference

http://graphics.stanford.edu/~smr/ICP/comparison/eggert_comparison_mva97.pdf

http://www.cs.princeton.edu/~smr/papers/icpstability.pdf

https://www.cnblogs.com/21207-iHome/p/6038853.html

https://github.com/adrelino/mv-lm-icp

https://www.cnblogs.com/yhlx125/p/4955337.html

Censi, A. (2008). "An ICP variant using a point-to-line metric." IEEE International Conference on Robotics & Automation. IEEE,: 19-25.

https://censi.science/pub/research/2008-icra-plicp-slides.pdf

https://censi.science/software/csm/

https://github.com/AndreaCensi/csm

http://jlyang.org/go-icp/

https://github.com/yangjiaolong/Go-ICP