Open3D点云库 C++学习笔记

配准篇（二）

---

---

前言

点云配准的方法主要可以分为两类，一类是全局(粗)配准方法，另一类为局部配准方法。
一般情况下，在对两幅点云进行配准时，如果点云之间的初始位姿差别比较大，就需要先使用全局配准方法，将两幅点云移动到一个比较接近的位置和姿态，然后再使用局部配准的方法进行精准匹配。如果直接使用局部配准的方法容易陷入局部最优解，使配准发生错误。只有在两幅点云的初始位姿已经非常接近的时候，可以直接使用局部配准的方法完成两幅点云的配准。
ICP（迭代最近点）配准算法是一种最为常见的局部配准算法，因此本文主要展示了三种不同类型的 ICP 配准算法，其中包括：点对点 ICP 、点对面 ICP [Rusinkiewicz2001]和融合了颜色信息的 Colored ICP算法[Park2017]。

---

一、点到点 ICP

此方法通过直接搜索一片点云中的点在另一片点云中的最近点来确定对应点集。这种以最近点为标准的方法虽然计算简便直观,但其所确立的对应点集中存在大量错误对应点对,算法迭代容易陷入局部极值。

点到点ICP代码如下：

int main(int argc, char *argv[]) {
    using namespace open3d;

    // Prepare input
    std::shared_ptr<geometry::PointCloud> source =
            open3d::io::CreatePointCloudFromFile("../data/fragment.ply");
    std::shared_ptr<geometry::PointCloud> target =
            open3d::io::CreatePointCloudFromFile("../data/fragment2.ply");
    if (source == nullptr || target == nullptr) {
        utility::LogWarning("Unable to load source or target file.");
        return -1;
    }

    visualization::DrawGeometries({source, target},
                                  "initial state");

    std::vector<double> voxel_sizes = {0.05, 0.05 / 4}; //下采样体素栅格的边长
    std::vector<int> iterations = {50, 14}; //最大迭代次数
    Eigen::Matrix4d trans_point2point= Eigen::Matrix4d::Identity();
    for (int i = 0; i < 2; ++i) {
        float voxel_size = voxel_sizes[i];
        auto source_down = source->VoxelDownSample(voxel_size);
        source_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        auto target_down = target->VoxelDownSample(voxel_size);
        target_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        double max_correspondence_distance=0.07;  //对应点对的最大距离，这个值对于结果影响很大

        auto result_point2point = pipelines::registration::RegistrationICP( //点到点的ICP
                *source_down, *target_down, max_correspondence_distance, trans_point2point,
                pipelines::registration::
                        TransformationEstimationPointToPoint(),
                pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6,
                                                                iterations[i]));

        trans_point2point = result_point2point.transformation_;

        cout<<"匹配方式：点对点"<<" "<<"inlier_rmse:"<<result_point2point.inlier_rmse_<<endl;
        VisualizeRegistration(*source, *target, trans_point2point); //显示配准结果
    }
      return 0;
}

max_correspondence_distance为对应点对的最大距离，这个值对于结果影响很大。如果设置太大容易搜索到错误的对应点，如果设置太小又容易搜索不到对应点。

两片点云的初始位置：

点到点ICP配准结果：

点对点方案输出结果：inlier_rmse:0.0229718，这个值越小说明效果越好。

二、点到面 ICP

点到面ICP将点点间的距离用点到面间的距离来代替,迭代次数少,且不易陷入局部极值。应用最为广泛的点到面算法为点到切平面算法。

点到面ICP代码如下：

int main(int argc, char *argv[]) {
    using namespace open3d;

    // Prepare input
    std::shared_ptr<geometry::PointCloud> source =
            open3d::io::CreatePointCloudFromFile("../data/fragment.ply");
    std::shared_ptr<geometry::PointCloud> target =
            open3d::io::CreatePointCloudFromFile("../data/fragment2.ply");
    if (source == nullptr || target == nullptr) {
        utility::LogWarning("Unable to load source or target file.");
        return -1;
    }

    visualization::DrawGeometries({source, target},
                                  "initial state");

    std::vector<double> voxel_sizes = {0.05, 0.05 / 4}; //下采样体素栅格的边长
    std::vector<int> iterations = {50, 14}; //最大迭代次数
    Eigen::Matrix4d trans_point2plane = Eigen::Matrix4d::Identity();
    for (int i = 0; i < 2; ++i) {
        float voxel_size = voxel_sizes[i];

        auto source_down = source->VoxelDownSample(voxel_size);
        source_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        auto target_down = target->VoxelDownSample(voxel_size);
        target_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        double max_correspondence_distance=0.07;  //对应点对的最大距离，这个值对于结果影响很大
        auto result_point2plane = pipelines::registration::RegistrationICP( //点到面的ICP
                *source_down, *target_down, max_correspondence_distance, trans_point2plane,
                pipelines::registration::
                        TransformationEstimationPointToPlane(),
                pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6,
                                                                iterations[i]));

        trans_point2plane = result_point2plane.transformation_;

        cout<<"匹配方式：点对面"<<" "<<"inlier_rmse:"<<result_point2plane.inlier_rmse_<<endl;;
        VisualizeRegistration(*source, *target, trans_point2plane); //显示配准结果
    }
      return 0;
}

点到面ICP配准结果：

点对面方案输出结果：inlier_rmse:0.0145821，比点到点ICP的配准效果更好。

三、Colored ICP算法

Colored ICP算法是融合了颜色信息的ICP改进算法，彩色点云配准算法的核心函数是RegistrationColoredICP( )。在[Park2017]一文中有具体的流程和原理的讲述，其主要思想就是在原本点到点ICP的基础上融合了颜色信息来加以联合优化，在不同的场景下赋予几何信息和颜色信息不同的优化权重，以达到最优的配准效果。简单来说，就是使用颜色信息来辅助配准，因此这种算法也只有在待配准点云之间的颜色信息差别很小时，能发挥较大的优势。为了进一步提高效率，[Park2017]还提出了多尺度的配准方案。

Colored ICP配准算法程序：

int main(int argc, char *argv[]) {
    using namespace open3d;

    // Prepare input
    std::shared_ptr<geometry::PointCloud> source =
            open3d::io::CreatePointCloudFromFile("../data/frag_115.ply");
    std::shared_ptr<geometry::PointCloud> target =
            open3d::io::CreatePointCloudFromFile("../data/frag_116.ply");
    if (source == nullptr || target == nullptr) {
        utility::LogWarning("Unable to load source or target file.");
        return -1;
    }
    visualization::DrawGeometries({source, target},"initial state");

    std::vector<double> voxel_sizes = {0.05, 0.05 / 4};
    std::vector<int> iterations = {50,14};
    Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();
    for (int i = 0; i < 2; ++i) {
        float voxel_size = voxel_sizes[i];

        auto source_down = source->VoxelDownSample(voxel_size);
        source_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        auto target_down = target->VoxelDownSample(voxel_size);
        target_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));


        auto result = pipelines::registration::RegistrationColoredICP(
                *source_down, *target_down, 0.07, trans,
                pipelines::registration::
                        TransformationEstimationForColoredICP(),
                pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6,
                                                                iterations[i]));
        trans = result.transformation_;

            VisualizeRegistration(*source, *target, trans);
            std::cout<<"匹配方式：Colored_ICP"<<" "<<"inlier_rmse:"<<result.inlier_rmse_<<std::endl;
    }
    std::stringstream ss;
    ss << trans;
    utility::LogInfo("Final transformation = \n{}", ss.str());

    return 0;
}

两片点云的初始位置：

点对面ICP配准结果：

点对点ICP配准结果：

Colored_ICP配准结果：

从三种方法的配准结果对比可以发现，只有Colored_ICP算法的配准效果比较好，因为基于点到点和点到面的ICP只有几何信息约束，不能防止两个平面之间的滑动，因此只有加入颜色信息进行辅助配准才能达到比较好的配准效果。

四、参考资料

https://github.com/isl-org/Open3D/releases/tag/v0.15.0
http://www.open3d.org/docs/latest/tutorial/pipelines/icp_registration.html
http://www.open3d.org/docs/latest/tutorial/pipelines/colored_pointcloud_registration.html

---

总结

以上就是配准篇（二）的全部内容，完整的可执行代码可以在我的github仓库进行下载，文章会持续更新，如果文章中有写的不对的地方，希望大家可以在评论区进行批评和指正，大家一起交流，共同进步！