只使用Eigen实现的并行ICP配准算法(SVD解法)

本文仅仅采用Eigen实现了并行的ICP算法（包括自己实现的一个KD树），其速度是PCL自带ICP算法的接近一倍，且精度更高。

1. ICP配准算法

1.1 ICP算法

ICP（Iterative Closest Point，迭代最近点）配准算法是一种用于将两个点云数据对齐的算法。它广泛应用于计算机视觉、机器人导航、3D建模等领域，目的是通过迭代优化过程使得一个点云与目标点云在形状和位置上尽可能吻合。ICP算法的核心思想是通过迭代的方式，不断寻找两个点云之间的最近点对，并最小化这些点对之间的距离，从而使得两个点云逐步对齐。下面来介绍下其核心部分的数学推导和代码实现：

1.2 SVD求解刚体变换

使用SVD求解三维空间中两组点云的刚体变换数学原理推导，见我以前发布的如下文章：
SVD求解两组点云(2D/3D)刚体变换-数学推导/C++ Eigen实现

1.3 KD树搜索最近邻

KD树基本原理见如下文章：
手写Kd树（C++模板非递归实现）

1.4 整体算法流程

1. 初始变换：给定源点云和目标点云，一个刚体变换的初始解（旋转和平移）。
2. 寻找最近点对：对于源点云中的每个点，找到目标点云中最近的点，形成点对集合。
3. 用对应点集求解刚体变换。
4. 用求解到的刚体变换更新点云。
5. 更新刚体变换（左乘初始变换）。
6. 判断是否收敛：如果收敛算法结束，不收敛继续执行2-5，直到最大迭代次数。

2. C++ Eigen代码实现

类声明（pimpl模式减少模块间的依赖）

#ifndef ICP3D_SVD_H
#define ICP3D_SVD_H
#include <Eigen/Dense>

class ICP3Dsvd
{
public:
    ICP3Dsvd();
    ~ICP3Dsvd() = default;
    bool setSourcePointCloud(const std::vector<Eigen::Vector3d> &source);
    bool setTargetPointCloud(const std::vector<Eigen::Vector3d> &target);
    bool registration(const double eps, const int iteratation,
                      Eigen::Matrix3d &R, Eigen::Vector3d &T);

private:
    class Impl;
    std::shared_ptr<Impl> m_pimpl;
};

#endif

类实现

#include <execution>

#include "icp3d_svd.h"
#include "../kdtree/kdtree.hpp"
#define USE_CPP17_MT
#include "../rigid_transform/rigid_transform.hpp"

class ICP3Dsvd::Impl
{
public:
    Impl()
    {
        m_p_Kdtree = std::make_unique<KdTree<3>>();
        m_p_estimator = std::make_unique<RigidTransform<3>>();
    };
    ~Impl() = default;
    bool setSourcePointCloud(const std::vector<Eigen::Vector3d> &source);
    bool setTargetPointCloud(const std::vector<Eigen::Vector3d> &target);
    bool registration(const double eps, const int iteration,
                      Eigen::Matrix3d &R, Eigen::Vector3d &T);

private:
    void transformPointCloud_(const Eigen::Matrix3d &R, const Eigen::Vector3d &T,
                              std::vector<Eigen::Vector3d> &point_cloud);

    double matchPointCloud_(std::vector<Eigen::Vector3d> &source);

private:
    std::unique_ptr<KdTree<3>> m_p_Kdtree;
    std::unique_ptr<RigidTransform<3>> m_p_estimator;
    std::vector<Eigen::Vector3d> m_source, m_target;
};

bool ICP3Dsvd::Impl::setSourcePointCloud(const std::vector<Eigen::Vector3d> &source)
{
    if (source.empty())
        return false;

    m_source = source;

    return true;
}

bool ICP3Dsvd::Impl::setTargetPointCloud(const std::vector<Eigen::Vector3d> &target)
{
    if (target.empty())
        return false;

    m_target = target;

    m_p_Kdtree->build(m_target);
    return true;
}

bool ICP3Dsvd::Impl::registration(const double eps, const int iteration,
                                  Eigen::Matrix3d &R, Eigen::Vector3d &T)
{
    // 1. Initialize RigidTransformation
    transformPointCloud_(R, T, m_source);
    Eigen::Matrix4d final_transformation = Eigen::Matrix4d::Identity();
    final_transformation.block<3, 3>(0, 0) = R;
    final_transformation.block<3, 1>(0, 3) = T;

    for (int i = 0; i < iteration; ++i)
    {
        // 2. Knn search
        std::vector<Eigen::Vector3d> source_;
        double err_ = matchPointCloud_(source_);
        // 3. Estimate RigidTransformation
        Eigen::Matrix4d cur_transformation = m_p_estimator->solve(source_, m_target);
        // 4. Update Point Cloud
        transformPointCloud_(cur_transformation.block<3, 3>(0, 0),
                             cur_transformation.block<3, 1>(0, 3), m_source);
        // 5. Update Transformation
        final_transformation = cur_transformation * final_transformation;

        // 6. Converage
        if (err_ < eps)
        {
            R = final_transformation.block<3, 3>(0, 0);
            T = final_transformation.block<3, 1>(0, 3);
            return true;
        }
    }

    return false;
}

void ICP3Dsvd::Impl::transformPointCloud_(const Eigen::Matrix3d &R, const Eigen::Vector3d &T,
                                          std::vector<Eigen::Vector3d> &point_cloud)
{
    std::transform(point_cloud.begin(), point_cloud.end(), point_cloud.begin(),
                   [&](const Eigen::Vector3d &p)
                   { return R * p + T; });
}

double ICP3Dsvd::Impl::matchPointCloud_(std::vector<Eigen::Vector3d> &source)
{
    std::vector<std::pair<size_t, size_t>> match_indexs;
    m_p_Kdtree->get_knn_points_mt(m_source, match_indexs, 1);

    source.resize(match_indexs.size());
    double err = 0.f;
    std::for_each(match_indexs.begin(), match_indexs.end(),
                  [&source, this, &err](const std::pair<size_t, size_t> &item)
                  {
                      source[item.first] = m_source[item.second];
                      err += (m_source[item.second] - m_target[item.first]).norm();
                  });

    return err / match_indexs.size();
}

ICP3Dsvd::ICP3Dsvd()
{
    m_pimpl = std::make_unique<Impl>();
}

bool ICP3Dsvd::setSourcePointCloud(const std::vector<Eigen::Vector3d> &source)
{
    return m_pimpl->setSourcePointCloud(source);
}

bool ICP3Dsvd::setTargetPointCloud(const std::vector<Eigen::Vector3d> &target)
{
    return m_pimpl->setTargetPointCloud(target);
}

bool ICP3Dsvd::registration(const double eps, const int iteration,
                            Eigen::Matrix3d &R, Eigen::Vector3d &T)
{
    return m_pimpl->registration(eps, iteration, R, T);
}

3. 与PCL库自带的ICP算法做对比

3.1 测试代码

#include "icp3d_svd.h"

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include "../common/common.h"

template <typename FuncT>
void evaluate_and_call(FuncT &&func, int run_num)
{
    double total_time = 0.f;
    for (int i = 0; i < run_num; ++i)
    {
        auto t1 = std::chrono::high_resolution_clock::now();
        func();
        auto t2 = std::chrono::high_resolution_clock::now();
        total_time +=  std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;;
    }

    std::cout << "run cost: " << total_time / 1000.f << std::endl;
}

void test_pcl_icp()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::string path = R"(.\testdata\rabbit.pcd)";
    pcl::io::loadPCDFile<pcl::PointXYZ>(path, *cloud);

    Eigen::Isometry3f SE3 = Eigen::Isometry3f::Identity();
    Eigen::AngleAxisf rvec(M_PI / 18, Eigen::Vector3f::UnitZ());
    Eigen::Vector3f T(0.005, 0.005, 0.005);
    SE3.pretranslate(T);
    SE3.rotate(rvec);

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud, *transformed_cloud, SE3.matrix());

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud);
    icp.setInputTarget(transformed_cloud);
    icp.setEuclideanFitnessEpsilon(0.0001);
    icp.setMaximumIterations(100);

    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

    std::cout << "SE3: " << std::endl
              << icp.getFinalTransformation() << std::endl;
    std::cout << "------------------------------" << std::endl;
    std::cout << "True: " << std::endl;
    std::cout << "R: " << rvec.matrix() << std::endl;
    std::cout << "T: " << T.transpose() << std::endl;
}

void test_icp_svd()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::string path = R"(.\testdata\rabbit.pcd)";
    pcl::io::loadPCDFile<pcl::PointXYZ>(path, *cloud);

    std::vector<Eigen::Vector3d> cloud_eigen;
    for (const auto &pt : cloud->points)
    {
        cloud_eigen.push_back(Eigen::Vector3d(pt.x, pt.y, pt.z));
    }

    Eigen::AngleAxisd rvec(M_PI / 18, Eigen::Vector3d::UnitZ());
    Eigen::Vector3d T(0.005, 0.005, 0.005);
    std::vector<Eigen::Vector3d> cloud_eigen2(cloud_eigen.size());
    std::transform(cloud_eigen.begin(), cloud_eigen.end(), cloud_eigen2.begin(), [&](const Eigen::Vector3d &p)
                   { return rvec * p + T; });

    ICP3Dsvd icp;
    icp.setSourcePointCloud(cloud_eigen);
    icp.setTargetPointCloud(cloud_eigen2);
    
    Eigen::Matrix3d R_ = Eigen::Matrix3d::Identity();
    Eigen::Vector3d T_ = Eigen::Vector3d::Zero();
    icp.registration(0.0001, 100, R_, T_);
    std::cout << "R: " << std::endl
              << R_ << std::endl
              << "T: " << T_.transpose() << std::endl;

    std::cout << "------------------------------" << std::endl;
    std::cout << "True: " << std::endl;
    std::cout << "R: " << rvec.matrix() << std::endl;
    std::cout << "T: " << T.transpose() << std::endl;
}

int main()
{
    std::cout << "PCL=================================" << std::endl;
    evaluate_and_call(test_pcl_icp, 1);
    std::cout << "ICP SVD=============================" << std::endl;
    evaluate_and_call(test_icp_svd, 1);

    return 0;
}

3.2 测试数据

斯坦福兔子：35947个点
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-l13iWoTT-1721910010605)(https://i-blog.csdnimg.cn/direct/a7415d91844b4f308b9f6e62725c6936.png#pic_center)]

3.3 测试结果

PCL=================================
SE3:
    0.984811    -0.173648 -6.71812e-08   0.00500015
    0.173648      0.98481 -1.95108e-07    0.0049998
-1.45029e-07   5.9595e-08            1   0.00499997
-1.45029e-07   5.9595e-08            1   0.00499997
           0            0            0            1
------------------------------
True:
R:  0.984808 -0.173648         0
R:  0.984808 -0.173648         0
 0.173648  0.984808         0
        0         0         1
T: 0.005 0.005 0.005
run cost: 0.525972
ICP SVD=============================
ICP SVD=============================
R:
R:
    0.984808    -0.173648  2.19244e-16
    0.984808    -0.173648  2.19244e-16
    0.173648     0.984808 -9.72026e-16
 2.32339e-16  1.79035e-15            1
T: 0.005 0.005 0.005
------------------------------
True:
True:
R:  0.984808 -0.173648         0
R:  0.984808 -0.173648         0
 0.173648  0.984808         0
        0         0         1
T: 0.005 0.005 0.005
run cost: 0.34996

• 本文实现的ICP算法相较于PCL速度快了接近一倍（如果点的数量更多效果更明显）；且精度更高(不确定是否是调参的问题)。
  Github完整测试数据和代码