【点云配准】刚性物体的鲁棒姿态估计

什么是刚性物体的鲁棒姿态估计？

刚性物体的姿态估计时可能会遇到的问题及解决方法：

采样一致性（SAC）算法的原理

预配准采样一致性（Prerejective Sample Consensus, PRSAC）算法的原理

SAC算法与PRSAC算法的不同点

代码

什么是刚性物体的鲁棒姿态估计？

刚性物体是指在三维空间中保持形状和尺寸不变的物体，例如物体中的一个刚性工具或一个稳定的物体。姿态估计是确定物体在三维空间中的旋转和平移变换的过程，即确定物体的位置和方向。

刚性物体的鲁棒性姿态估计是指对刚性物体进行姿态估计的时候，考虑到噪声、局部变形或不完整点云等因素，通过采用鲁棒性较强的方法来提高准确性。

刚性物体的姿态估计时可能会遇到的问题及解决方法：

问题：

噪声：输入的点云数据可能包含噪声，干扰姿态估计的准确性。
不完整点云：点云数据可能不完整，可能存在缺失或遮挡的部分。
局部变形：由于物体的几何形状可能存在变形或形状扭曲，局部区域的点云可能与整体形状不一致。

为了解决这些问题，鲁棒姿态估计方法通常使用一些技术，包括但不限于以下内容：

采样一致性（Sample Consensus, SAC）算法：通过随机采样和一致性判断来估计刚性变换矩阵，可以优化对噪声和异常值的鲁棒性。
迭代最近点（Iterative Closest Point, ICP）算法：通过迭代优化匹配点云之间的最小平方距离，来估计刚性变换矩阵，可用于对齐点云和估计刚性物体的姿态。
特征描述子：使用几何或局部特征描述子，例如法线、曲率、SHOT等，帮助提高在噪声和不完整点云情况下的匹配和姿态估计准确性。
鲁棒对应关系：通过过滤不正确的匹配对、剔除噪声和异常值等，建立可靠的对应关系，提高姿态估计的鲁棒性。

采样一致性（SAC）算法的原理

当我们想要找到一组数据中的某种模式或规律时，采样一致性（Sample Consensus, SAC）算法可以帮助我们。

想象一下，我们手上有一堆点的数据，并且我们认为这些点可以被拟合成一条直线或者一个平面。但是，数据中可能会有一些异常值或者噪声，它们可能偏离了我们期望的模式。

SAC算法的任务就是找到最适合我们数据的模型。它的工作原理是这样的：

首先，我们从数据中随机选择一些点，作为我们的候选点。我们假设这些点符合我们期望的模式。
接下来，我们根据这些选定的点来计算出模型的参数，也就是拟合这些点的最佳直线或平面。
然后，我们计算其他的点与估计出来的模型之间的距离。如果某个点与模型的距离小于某个阈值，我们认为这个点是符合模型的点，也就是内点。
如果我们找到了足够多的内点，我们可以通过优化参数来进一步提高模型的准确性。
最后，我们根据内点的数目或者一致性标准来评估模型的好坏。如果我们达到了预设的一致性标准，我们就认为我们找到了合适的模型。

通过反复迭代这个过程，SAC算法能够帮助我们找到一个最能够描述数据的模型，即使在有些数据点可能是异常值或者噪声的情况下也可以做到。我们可以通过调整阈值和一致性标准等参数来控制算法的准确性和效率。

用通俗的话来说，SAC算法就像是用一颗懂得计算和评估的大脑，帮我们从一堆点的数据中找到最合适的模式或规律，即使有些点离谱也能做到。这个算法在很多领域中都有应用，特别在点云处理方面，帮助我们处理和分析复杂的数据。

预配准采样一致性（Prerejective Sample Consensus, PRSAC）算法的原理

SampleConsensusPrerejective是PCL库中的一种用于点云配准的算法，它基于Sample Consensus（SAC）算法进行改进和扩展，并引入了两个阶段的策略。

该算法的原理如下：

Prerejection阶段：根据参考点云，使用特征描述子的比较快速筛选出可能的匹配点。特征描述子可以帮助进行点云的局部特征匹配，提高配准的准确性。
Rejection阶段：在Prerejection阶段基础上，采用采样一致性的迭代优化算法，通过估计刚性变换参数来寻找最佳的配准结果。算法会尝试不同的变换，并通过评估变换的一致性得分来选择最佳的匹配。

通过这两个阶段的策略，SampleConsensusPrerejective算法可以更准确地找到点云间的最佳刚性变换，实现更精确的点云配准。

SAC算法与PRSAC算法的不同点

SampleConsensusPrerejective是一种基于点模型配准的方法，而SAC算法则是用于估计点云的模型参数，如平面、直线等。
SampleConsensusPrerejective采用了两个阶段的策略：Prerejection和Rejection，先筛选匹配点，然后通过迭代和优化达到最佳配准结果。而SAC算法通常是单一阶段的，直接根据采样的点和数据对模型参数进行估计。
SampleConsensusPrerejective在配准过程中会使用到特征描述子，通过比较特征描述子来确定匹配点。而SAC算法通常不涉及特征描述子，主要依靠点之间的距离和几何一致性。

代码

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/common/time.h>
#include <pcl/console/print.h>

#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>

#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>

#include <pcl/io/pcd_io.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointNormal PointNT; //点类型， 位置信息和法线信息
typedef pcl::PointCloud<PointNT> PointCloudT; // 点云类型

typedef pcl::FPFHSignature33 FeatureT; // 特征类型，用来表示点云的局部特征
typedef pcl::PointCloud<FeatureT> FeatureCloudT; // 特征点云类型

typedef pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT> FeatureEstimationT; // 估计特征的类，在此实例化；使用OMP（Open Multi-Processing）进行并行计算；输入点云为PointNT,输出特征为FeatureT
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;// 点云颜色处理器

int main(){
    // 实例化必要的数据容器
    PointCloudT::Ptr object(new PointCloudT);
    PointCloudT::Ptr object_aligned(new PointCloudT);
    PointCloudT::Ptr scene(new PointCloudT);

    FeatureCloudT::Ptr object_features(new FeatureCloudT);
    FeatureCloudT::Ptr scene_features(new FeatureCloudT);

    // Load obejct and scene
    std::string path1 = "/home/jason/file/pcl-learning/14registration配准/5刚性物体的鲁棒姿态估计/chef.pcd"; // 源点云
    std::string path2 = "/home/jason/file/pcl-learning/14registration配准/5刚性物体的鲁棒姿态估计/rs1.pcd"; // 目标点云，没有法线字段信息；所以后续需要计算法线
    if (pcl::io::loadPCDFile<PointNT>(path1,*object) < 0 || pcl::io::loadPCDFile<PointNT>(path2, *scene) < 0) {
        pcl::console::print_error("Error loading obejct/scene file!\n");
    }

    // 下采样
    pcl::console::print_highlight("Downsampling....\n");
    pcl::VoxelGrid<PointNT> grid;
    const float leaf = 0.005f;
    grid.setLeafSize(leaf, leaf, leaf);
    grid.setInputCloud(object);
    grid.filter(*object);

    grid.setInputCloud(scene);
    grid.filter(*scene);

    // Estimate normals for scene
    // 对目标点云计算法线
    pcl::console::print_highlight("Estimate scene normals...\n");
    pcl::NormalEstimationOMP<PointNT, PointNT> nest;
    nest.setRadiusSearch(0.01); // 法线的搜索半径为0.01,即估计场景点云的每个点的法线时，仅仅考虑该点半径范围内的邻居点
    nest.setInputCloud(scene);
    nest.compute(*scene);

    // Estimate features
    // 计算目标点云、源点云的特征描述符
    // 对于下采样点云中每个点，使用PCL的pcl::FPFHEstimationOMP<>类来计算用于对齐过程中，用于匹配的快速点特征直方图（FPFH）描述符
    pcl::console::print_highlight("Estimating features...\n");
    FeatureEstimationT fest;
    fest.setRadiusSearch(0.025);
    fest.setInputCloud(object);
    fest.setInputNormals(object);
    fest.compute(*object_features);

    fest.setInputCloud(scene);
    fest.setInputNormals(scene);
    fest.compute(*scene_features);

    // peform alignment
    // 对齐配准对象创建于配置
    pcl::console::print_highlight("Starting alignment...\n");
    pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align; // 预配准采样一致性（Prerejective Sample Consensus, PRSAC）算法
    align.setInputSource(object); // 待配准点云为object，即物体点云
    align.setSourceFeatures(object_features); // 设置源点云的特征

    align.setInputTarget(scene); // 设置配准的目标点云为scene，即场景点云
    align.setTargetFeatures(scene_features); // 设置目标点云的特征

    // 下面设置的其实都是默认值！即可以省略
    align.setMaximumIterations(50000); // 设置RPSAC算法的最大迭代次数
    align.setNumberOfSamples(3); // 设置PRSAC算法每次采样的点数
    align.setCorrespondenceRandomness(5); // 设置PRSAC算法中用于计算变换矩阵的匹配对数

    align.setSimilarityThreshold(0.9f); // 设置相似度阈值，用于过滤不相关的匹配对
    align.setMaxCorrespondenceDistance(2.5f * leaf); // 设置最大对应距离，用于拒绝远离目标点的匹配对i
    align.setInlierFraction(0.25f); // 设置内点比例阈值，用于判断配准的成功与否

    {
        pcl::ScopeTime t("alignment");
        align.align(*object_aligned); // 配准
    }

    pcl::visualization::PCLVisualizer vis("Alignment-刚性物体的鲁棒姿态估计");
    if(align.hasConverged()){
        printf("\n");
        Eigen::Matrix4f transformation = align.getFinalTransformation();
        pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
        pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
        pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
        pcl::console::print_info ("\n");
        pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
        pcl::console::print_info ("\n");
        pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ()); // Inliers: 1384/3432

        // show alignment

        vis.addPointCloud(scene, ColorHandlerT(scene, 0.0, 255.0, 0.0), "scene");
        vis.addPointCloud(object_aligned, ColorHandlerT(object_aligned, 0.0, 0.0, 255.0), "object_aligned");


    }
    else {
        pcl::console::print_error("Alignment failed!\n");
        return -1;
    }

    while (!vis.wasStopped()) {
        vis.spinOnce();
    }

    return 0;
}

输出：