Open3D点云库 C++学习笔记

配准篇（一）前言一、RANSAC全局配准法二、快速全局配准法(FGR)三、参考资料总结

点云配准的方法主要可以分为两类，一类是全局(粗)配准方法，另一类为局部配准方法。
一般情况下，在对两幅点云进行配准时，如果点云之间的初始位姿差别比较大，就需要先使用全局配准方法，将两幅点云移动到一个比较接近的位置和姿态，然后再使用局部配准的方法进行精准匹配。如果直接使用局部配准的方法容易陷入局部最优解，使配准发生错误。只有在两幅点云的初始位姿已经非常接近的时候，可以直接使用局部配准的方法完成两幅点云的配准。
全局配准算法不需要初始化对齐。它们通常会得到一个不太紧密的配准结果，并常被用作局部配准方法的初始化位姿。本章节将主要介绍两种全局配准方法：RANSAC全局配准法和快速全局配准法。

此函数可以将转换后的源点云与目标点云在一起进行可视化：

void VisualizeRegistration(const open3d::geometry::PointCloud &source,
                           const open3d::geometry::PointCloud &target,
                           const Eigen::Matrix4d &Transformation) {
    std::shared_ptr source_transformed_ptr(
            new geometry::PointCloud);
    std::shared_ptr target_ptr(new geometry::PointCloud);
    *source_transformed_ptr = source;
    *target_ptr = target;
    source_transformed_ptr->Transform(Transformation);
    visualization::DrawGeometries({source_transformed_ptr, target_ptr},
                                  "Registration result");
}

首先需要对两幅点云分别进行下采样，估计法线，然后为每个点计算 FPFH 特征。FPFH 特征是一个 33 维向量，描述了一个点的局部几何特性。33 维空间中的最近邻查询可以返回具有相似局部几何结构的点。这些步骤可以由如下函数来完成：

std::tuple, //tuple是一个元组
           std::shared_ptr,
           std::shared_ptr>
PreprocessPointCloud(const char *file_name, const float voxel_size) {
    auto pcd = open3d::io::CreatePointCloudFromFile(file_name);
    auto pcd_down = pcd->VoxelDownSample(voxel_size); //下采样
    pcd_down->EstimateNormals(  //估计法线
            open3d::geometry::KDTreeSearchParamHybrid(2 * voxel_size, 30));
    auto pcd_fpfh = pipelines::registration::ComputeFPFHFeature(
            *pcd_down,          //计算FPFH 特征
            open3d::geometry::KDTreeSearchParamHybrid(5 * voxel_size, 100));
    return std::make_tuple(pcd, pcd_down, pcd_fpfh); //返回一个元组
}

RANSAC全局配准法主要程序如下，完整程序可以在我的github主页下载

    // Prepare input
    std::shared_ptr source, source_down, target,
            target_down;
    std::shared_ptr source_fpfh, target_fpfh;
    std::tie(source, source_down, source_fpfh) =
            PreprocessPointCloud("../data/fragment.ply", voxel_size);  //原点云
    std::tie(target, target_down, target_fpfh) =
            PreprocessPointCloud("../data/fragment1.ply", voxel_size); //目标点云

    visualization::DrawGeometries({source, target},"Initial state"); //展示两幅点云的初始状态

    pipelines::registration::RegistrationResult registration_result;

    // Prepare checkers
    std::vector>
            correspondence_checker;
    auto correspondence_checker_edge_length =
     //检查从源和目标对应关系中分别绘制的任意两条任意边（由两个顶点形成的线）的长度是否相似。
     //检查||edgesource||>0.9⋅||edgetarget||和||edgetarget||>0.9⋅||edgesource||是真的。
            pipelines::registration::CorrespondenceCheckerbasedOnEdgeLength(
                    0.9);
    auto correspondence_checker_distance =
            //检查对齐的点云是否接近（小于指定的阈值）
            pipelines::registration::CorrespondenceCheckerbasedOnDistance(distance_threshold);
    correspondence_checker.push_back(correspondence_checker_edge_length);
    correspondence_checker.push_back(correspondence_checker_distance);

        registration_result = pipelines::registration::
                RegistrationRANSACbasedOnFeatureMatching(
                        *source_down, *target_down, *source_fpfh, *target_fpfh,
                        mutual_filter, distance_threshold,
                        pipelines::registration::
                                TransformationEstimationPointToPoint(false),
                        3, correspondence_checker,
                        pipelines::registration::RANSACConvergenceCriteria(
                                max_iterations, confidence));

两幅点云的初始位姿展示：

两幅点云配准后结果展示：

考虑到基于 RANSAC 的全局配准方案可能需要消耗较长的时间。[Zhou2016]提出了一种更快的方法，它的优化过程不需要内部循环中的最近点查询，因此可以节省大量计算时间。

快速全局配准法的主程序如下：

int main(int argc, char *argv[]){
    float voxel_size =
            utility::GetProgramOptionAsDouble(argc, argv, "--voxel_size", 0.05);
    float distance_multiplier = utility::GetProgramOptionAsDouble(
            argc, argv, "--distance_multiplier", 1.5);
    float distance_threshold = voxel_size * distance_multiplier;
    int max_iterations =
            utility::GetProgramOptionAsInt(argc, argv, "--max_iterations", 64);
    int max_tuples =
            utility::GetProgramOptionAsInt(argc, argv, "--max_tuples", 1000);

    // Prepare input
    std::shared_ptr source, source_down, target,
            target_down;
    std::shared_ptr source_fpfh, target_fpfh;
    std::tie(source, source_down, source_fpfh) =
            PreprocessPointCloud("../data/fragment.ply", voxel_size);
    std::tie(target, target_down, target_fpfh) =
            PreprocessPointCloud("../data/fragment1.ply", voxel_size);

    pipelines::registration::RegistrationResult registration_result =
            pipelines::registration::
                    FastGlobalRegistrationbasedOnFeatureMatching(
                            *source_down, *target_down, *source_fpfh,
                            *target_fpfh,
                            pipelines::registration::
                                    FastGlobalRegistrationOption(
                                             1.4, true,
                                            true, distance_threshold,
                                            max_iterations,
                                             0.95,
                                            max_tuples));
    VisualizeRegistration(*source, *target,
                          registration_result.transformation_);
                          
    cout<<"RegistrationResult withfitness:"< 
 
 
两幅点云配准后结果展示：
 
 

 通过适当的配置，快速全局配准的准确性甚至可以与局部配准方法ICP相媲美。更多实验结果可以参考文献[Zhou2016]。
 
三、参考资料 
http://www.open3d.org/docs/latest/tutorial/pipelines/global_registration.html
 https://github.com/isl-org/Open3D/releases/tag/v0.15.0
 
 
总结 
以上就是配准篇（一）的全部内容，完整的可执行代码可以在我的github仓库进行下载，文章会持续更新，如果文章中有写的不对的地方，希望大家可以在评论区进行批评和指正，大家一起交流，共同进步！