『点云识别』基于对应分组的三维物体识别

SHOT特征描述子

SHOT（Signature of Histograms of Orientations）是一种用于描述点云特征的算法。它基于点云的法线信息和局部区域的形态分布统计，用于表示点云中的局部形状信息。SHOT特征描述子在三维物体识别、匹配和配准等任务中广泛应用。

SHOT特征描述子的计算步骤如下：

1. 选择一个中心点，并计算该点的法线方向。

2. 在该中心点周围选择若干个邻域点（例如，使用半径搜索或Kd树搜索方法找到邻域点）。

3. 为每个邻域点计算其相对于中心点的位置、法线差异和角度差异等特征。

4. 将这些特征分成不同的直方图（histogram）进行统计，例如，位置特征分为距离直方图、法线特征分为法线角度差直方图等。

5. 将所有直方图连接起来形成一个总体的SHOT特征描述子。

SHOT特征描述子具有以下特点：

• 尺度不变性：SHOT特征描述子可以对不同尺度的点云进行计算，从而具有尺度不变性。

• 旋转不变性：由于使用了法线信息，SHOT特征可以在一定范围内具有旋转不变性。

• 区分度高：由于考虑了位置、法线和角度等多种因素，SHOT特征能够很好地区分不同的三维形状。

• 维度较高：SHOT特征的维度较高，取决于所选的特征参数和直方图的数量。

通过计算点云中的SHOT特征描述子，可以将点云数据转换为一组具有辨识度的向量表示，用于点云的匹配、识别和配准等任务。

BOP（Border-Ownership-based Perspective）特征描述子

BOP特征描述子能够有效地表示点云数据中的边缘信息，用于物体检测和匹配等任务。

BOP特征描述子的计算是基于点云中的边界点和其它邻域点之间的关系，通过分析相对于边界点的位置、法线方向和曲率等特征来推断该点是否是边界点。具体来说，BOP模块通过以下步骤来计算BOP特征描述子：

1. 预处理：对输入的点云进行预处理，例如计算法线和曲率等。

2. 边界点检测：根据一些准则或规则，对点云中的边界点进行检测和标记。

3. 边界点特征计算：对每个边界点及其邻域点，计算相对于边界点的位置、法线和曲率等特征。

4. 特征统计：将邻域点的特征进行统计，并根据特征的分布构建BOP特征描述子。

BOP特征描述子可以用来检测点云中的物体边缘、形状边界等，对物体的形状、轮廓等进行描述。它在物体识别、配准和分割等领域有广泛的应用。

PCL库的3D Hough变换

PCL（点云库）中的3D Hough变换是一种在点云数据中检测和估计3D空间中的几何形状的技术。它可以用于检测特定的3D几何形状，例如平面、圆柱体、圆锥体等。

3D Hough变换的基本思想是将点云中的每个点投影到参数空间中，形成一个参数空间的累加器。在参数空间中，每个参数对应了可能的几何形状的一组参数，例如平面的法向量和点与平面的距离。

通过对参数空间的累加器进行分析，可以找到累加器中具有显著累积值的参数组合，这些参数组合对应了在点云中存在的几何形状。

PCL中的3D Hough变换模块提供了一些类和函数来执行3D Hough变换。常用的类包括：

• Hough3DGrouping：基于3D Hough变换的点云分组类，用于检测和估计特定几何形状的参数模型（如平面、圆柱体等）。

• Hough3D：执行3D Hough变换的主要类，用于在点云中计算参数空间的累加器。

• HoughSpace3D：表示3D Hough变换的参数空间的数据结构。

• HoughCell3D：表示3D Hough变换空间中的一个单元格（bin）的数据结构。

这些类可以用于从点云数据中检测和分组特定几何形状的点，从而进行目标检测、配准、地面平面提取等任务。

使用PCL中的3D Hough变换模块时，需要设置参数空间的分辨率、累加器的阈值、搜索范围等参数，以及提供点云数据和目标对象的相关信息。具体的用法和参数设置可以参考PCL官方文档和示例代码。

pcl/recognition/cg/geometric_consistency.h

当我们说到几何一致性匹配时，我们指的是在点云数据中找到相似的形状或物体。PCL库中的geometric_consistency.h头文件包含了几个类和函数，用于实现这种匹配。

其中，GeometricConsistencyGrouping类可以用来找到点云数据中的相似形状或物体的组合。它会根据我们提供的几何一致性模型，通过计算点云之间的几何关系来确定匹配的组合。例如，如果我们想找到两个点云中匹配的平面，这个类可以帮助我们找到这些匹配的平面。

GeometricConsistencyVerification类用于验证两个点云之间的几何一致性。它可以确定点云是否是匹配的，并给出几何一致性验证是否成功的信息。

使用这些类，我们可以在点云数据中执行几何一致性匹配。这对于场景识别、目标检测和点云配准等应用非常有用。

pcl/kdtree/kdtree_flann.h

当我们处理点云数据时，有时需要在点云中找到与某个点最接近的点，或者在一个给定的范围内找到附近的点。PCL库中的 kdtree_flann.h 头文件利用了一个叫做 FLANN 的库来实现这些功能。

FLANN（Fast Library for Approximate Nearest Neighbors）是一个高效的库，用于在大型数据集中进行快速的最近邻搜索。kdtree_flann.h 头文件提供了通过 KD 树（kd-tree）进行最近邻搜索的功能。

在这个头文件中，我们主要使用 pcl::KdTreeFLANN 类来进行搜索。我们可以将点云数据加载到 KdTreeFLANN 中，然后通过调用 nearestKSearch 函数来查找最近的 K 个邻居, 或者通过调用 radiusSearch 函数来查找给定半径内的邻居。

使用这个头文件，我们可以更高效地在点云数据中进行搜索，找到最接近的点或者在一定范围内找到附近的点。

pcl::ReferenceFrame

pcl::ReferenceFrame是用来描述点云数据中物体朝向和摆放姿态的一种工具。它告诉了我们物体在空间中的位置和朝向。

我们可以将pcl::ReferenceFrame想象成一个立方体，它有一个标记的中心点和三个相互垂直的箭头，箭头的末端指向不同的方向。这里的箭头可以帮助我们理解物体的朝向。

举个例子，假设我们有一些点云数据表示一台汽车。使用pcl::ReferenceFrame，我们可以找到汽车点云数据的中心点，并计算出三个方向向量：一个指向前方，一个指向左侧，一个指向上方。这样，我们就可以知道汽车在空间中的位置和摆放方向。

通过使用pcl::ReferenceFrame类，我们可以轻松地描述和处理点云中物体的朝向和摆放姿态的信息。这对于物体识别、机器人导航和姿态估计等任务非常有用。

pcl::SHOT352

pcl::SHOT352是一个用于描述点云特征的工具，它能够帮助我们理解和比较不同点云之间的相似性。

你可以把pcl::SHOT352看作是一种计算点云形状特征的方法。它会观察点云周围的形状，并将这些形状信息编码成一个由352个数字组成的向量。这个向量描述了点云表面的特征。

通过使用pcl::SHOT352，我们可以比较两个点云之间的相似性。例如，假设我们有两个点云，一个表示苹果，另一个表示橙子。我们可以使用pcl::SHOT352来计算出它们的特征描述子，并将它们进行比较。如果两个特征描述子非常接近，那么我们可以认为这两个点云代表的是相似的物体。

pcl::SHOT352可以应用于许多任务，如点云匹配、目标识别和物体测量等。它能够帮助我们理解点云之间的相似性或者差异，从而在各种应用中发挥作用。

代码思路

读取模型点云数据、场景点云数据

去除点云中NaN点

计算模型点云、场景点云的法线向量

下采样得到关键点

为模型关键点云、场景关键点云计算SHOT特征描述子（需要用到法线）

使用KdTreeFLANN将场景关键点云，与模型关键点云进行匹配，并保存匹配结果

使用霍夫变换或者几何一致性聚类进行目标识别（这一步特别耗时，需要有GPU）

可视化 场景关键点云、模型关键点云，并连线匹配点

代码

#include <pcl/io/pcd_io.h> // 文件读写
#include <pcl/point_cloud.h> // 存储和操作点云数据
#include <pcl/correspondence.h> // 对应分组（Correspondence Grouping）相关算法和类，将点云之间的对应点分组

#include <pcl/features/normal_3d_omp.h> // 计算点云法线，利用OpenMP进行多线程加速
#include <pcl/features/shot_omp.h> // 计算SHOT（Signature of Histograms of Orientations）特征描述子，利用OpenMP进行多线程加速

#include <pcl/features/board.h> // 计算BOP（Border_Ownership-based Perspective）特征的相关算法和类，用于检测物体边缘
#include <pcl/filters/uniform_sampling.h> // 均匀采样滤波算法，用于对点云进行降采样处理

#include <pcl/recognition/cg/hough_3d.h> // 实现3D Hough变换，用于检测点云中的几何形状
#include <pcl/recognition/cg/geometric_consistency.h> // 几何一致性匹配算法，用于基于几何约束进行点云匹配

#include <pcl/visualization/pcl_visualizer.h> // 可视化点云数据
#include <pcl/kdtree/kdtree_flann.h> // 基于FLANN（Fast Library for Approximate Nearset Neighboors）的Kd数算法，用于点云数据的最近邻搜索

#include <pcl/common/transforms.h> // 点云变换操作，如平移、旋转、缩放
#include <pcl/filters/filter.h> // removeNaNFromPointCloud删除点云中的NaN点
#include <pcl/filters/voxel_grid.h>

typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptiorType;

std::string model_filename_; // 模型的文件名
std::string scence_filename_; // 场景文件名

bool show_keypoints_(true);
bool show_correspondences_(true);

bool use_cloud_resolution_(true);
bool use_hough(true);

float model_ss_(0.01f);

float scene_ss_ (0.03f);//场景采样率
float rf_rad_ (0.015f);
float descr_rad_ (0.02f);
float cg_size_ (0.01f);//聚类 霍夫空间设置每个bin的大小
float cg_thresh_ (5.0f);//聚类阈值


// 计算点云分辨率 点云 每个点距离最近点之间的距离和 的平均值
double computeCloudResolution(const pcl::PointCloud<PointType>::ConstPtr &cloud){
    double res = 0; // 用于存储最终计算得到的点云分辨率
    int n_points = 0; // 记录参与计算的点的数量
    int nres; // 最近邻点的数量
    std::vector<int> indices; // 存储最近邻点的索引
    std::vector<float> sqr_distances(2); // 存储最近邻点的距离的平方

    pcl::search::KdTree<PointType> tree; // 搜索方法kdtree
    tree.setInputCloud(cloud); // 输入点云数据作为KDTree的输入

    for (size_t i=0; i<cloud->size(); ++i){ // 遍历每一个点
        if(!pcl::isFinite((*cloud)[i])) // 剔除NAN点
            continue;
        nres = tree.nearestKSearch(i,2, indices, sqr_distances); // 查找最近的2个点，并返回结果到indices 和 sqr_distance
        if (nres == 2) // 如果找到了2个点
        {
            res += sqrt(sqr_distances[1]); // 累加第二个最近邻点的距离
            ++n_points; // 点数加一
        }
    }

    if(n_points!=0) // 如果参与计算的点的数量不为零
        res /= n_points; // 计算平均分辨率，即累积距离/点的数量

    return res; // 返回点云的分辨率
}




int main(){

    // 新建必要的指针变量
    pcl::PointCloud<PointType>::Ptr model(new pcl::PointCloud<PointType>); // 模型点云
    pcl::PointCloud<PointType>::Ptr model_keypoints(new pcl::PointCloud<PointType>); // 模型点云的关键点 点云

    pcl::PointCloud<PointType>::Ptr scene(new pcl::PointCloud<PointType>); // 场景点云
    pcl::PointCloud<PointType>::Ptr scene_kepoints(new pcl::PointCloud<PointType>); // 场景点云的关键点 点云

    pcl::PointCloud<NormalType>::Ptr model_normals(new pcl::PointCloud<NormalType>); // 模型点云的 法线向量
    pcl::PointCloud<NormalType>::Ptr scene_normals(new pcl::PointCloud<NormalType>); // 场景点云的 法线向量

    pcl::PointCloud<DescriptiorType>::Ptr model_descriptors(new pcl::PointCloud<DescriptiorType>); // 模型点云 特征点的特征描述子
    pcl::PointCloud<DescriptiorType>::Ptr scene_descriptors(new pcl::PointCloud<DescriptiorType>); // 场景点云 特征点的特征描述子

    // 加载点云数据
    std::string model_file = "/home/jason/file/pcl-learning/13recognition识别/1基于对应分组的三维物体识别/milk.pcd"; // 模型点云文件
    std::string scene_file = "/home/jason/file/pcl-learning/13recognition识别/1基于对应分组的三维物体识别/milk_cartoon_all_small_clorox.pcd"; // 场景点云文件
    if (pcl::io::loadPCDFile(model_file, *model) <0 || pcl::io::loadPCDFile(scene_file, *scene) < 0){
        PCL_ERROR("model or scene file not found");
        return -1;
    }

    // 去除NaN
    std::vector<int> indices_;
    pcl::removeNaNFromPointCloud(*model, *model, indices_);
    pcl::removeNaNFromPointCloud(*scene, *scene, indices_);

    // 使用分辨率
    float resolution = static_cast<float>(computeCloudResolution(model)); // 计算分辨率
    if (resolution != 0.0f) // 更新参数
    {
//        model_ss_ *= resolution;
//        scene_ss_ *= resolution;
//        rf_rad_ *= resolution;
//        descr_rad_ *= resolution;
//        cg_size_ *= resolution;

        model_ss_ *= 0.0114425;
        scene_ss_ *= 0.0305134;
        rf_rad_ *= 0.0152567;
        descr_rad_ *= 0.0228851;
        cg_size_ *= 0.0152567;
    }

    std::cout << "Model resolution:       " << resolution << std::endl;
    std::cout << "Model sampling size:    " << model_ss_  << std::endl;
    std::cout << "Scene sampling size:    " << scene_ss_  << std::endl;
    std::cout << "LRF support radius:     " << rf_rad_    << std::endl;
    std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
    std::cout << "Clustering bin size:    " << cg_size_   << std::endl << std::endl;


    // 计算法线向量
    pcl::NormalEstimationOMP<PointType, NormalType> norm_est; // 估计法线对象，多核; 输入点云类型为PoinType,输出法线类型为NormalType
    norm_est.setNumberOfThreads(8);
    norm_est.setKSearch(10); // 在估计每个点的法线时，使用每个点周围最近的10个点进行计算
//    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
//    norm_est.setSearchMethod(kdtree);// 多核模式不需要哦设置 搜索算法
    norm_est.setInputCloud(model); // 模型点云
    norm_est.compute(*model_normals); // 模型点云的法线向量

    norm_est.setInputCloud(scene); // 场景点云
    norm_est.compute(*scene_normals); // 场景点云的法线向量

    //下采样滤波使用均匀采样 （可以试一试体素格子i下采样）得到关键点
    pcl::VoxelGrid<PointType> grid;
    grid.setLeafSize(0.005f, 0.005f, 0.005f);
    grid.setInputCloud(model);
    grid.filter(*model_keypoints);

    grid.setInputCloud(scene);
    grid.filter(*scene_kepoints);
    std::cout << "Model total points: " << model->size() << "; Selected Keypoints: " << model_keypoints->size() << std::endl;
    std::cout << "Scene total points: " << scene->size() << "; Selected keypoints: " << scene_kepoints->size() << std::endl;



    // 为keypoints关键点计算SHOT描述子Descriptor
    pcl::SHOTEstimationOMP<PointType, NormalType, DescriptiorType> descr_est;
    descr_est.setNumberOfThreads(8);
    descr_est.setRadiusSearch(0.01f); // 设置搜索半径

    descr_est.setInputCloud(model_keypoints);  // 模型关键点云（下采样得到的）
    descr_est.setInputNormals(model_normals); // 法线
    descr_est.setSearchSurface(model); // 描述子估计对象的搜索表面
    descr_est.compute(*model_descriptors); // 模型点云描述子

    descr_est.setInputCloud(scene_kepoints);
    descr_est.setInputNormals(scene_normals);
    descr_est.setSearchSurface(scene);
    descr_est.compute(*scene_descriptors); // 场景点云描述子



    // 使用KdTreeFLANN，将模型点云的描述子与场景点云的描述子进行匹配，并将匹配的对应关系存储在model_scene_corrs中
    // 匹配的准则是找到场景描述子的最近邻，并判断其平方距离小于0.25
    pcl::CorrespondencesPtr model_scene_corrs(new pcl::Correspondences()); // 用于保存模型于场景的对应关系

    pcl::KdTreeFLANN<DescriptiorType> match_search; // KdTreeFLANN对象，输入类型为DescriptiorType
    match_search.setInputCloud(model_descriptors); // 设置KdTreeFLANN对象的输入为model_descriptors

    for (size_t i=0; i<scene_descriptors->size(); ++i){
        std::vector<int> neigh_indices(1); // 用于保存最近邻的索引
        std::vector<float> neigh_sqr_dists(1); // 用于保存最近邻的平方距离
        if (!std::isfinite(scene_descriptors->at(i).descriptor[0])) // 检查描述子的第一个值是否为有限数
            continue;
        int found_neighs = match_search.nearestKSearch(scene_descriptors->at(i), // 使用math_searcch，去模型点云查找，离场景描述子i,最近的一个邻居
                                                       1, neigh_indices, neigh_sqr_dists);  // 将索引、平方距离保存在neigh_indices、neigh_sqr_dists

        if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25){ // 用场景描述子匹配到的目标描述子，二者之间的平方距离小于0.25
            pcl::Correspondence corr(neigh_indices[0],  // KdTreeFLANN输入描述子的索引
                    static_cast<int>(i), // 场景描述子的索引
                    neigh_sqr_dists[0]);  // 平方距离
            model_scene_corrs->push_back(corr); // 将corr添加到model_scene_corrs中，表示模型与场景之间的对应关系
        }

    }
    std::cout << "Correspondences found: " << model_scene_corrs->size() << std::endl; // 打印目前所找到的模型与场景之间的对应关系数量

    // 执行聚类
    // 这个特别耗时
    // 也可以用霍夫变换
    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> rototranslations; // 用于存储旋转-平移矩阵

    std::vector<pcl::Correspondences> clustered_corrs; // 用于存储聚类后的对应关系

    pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer; // 几何一致性聚类对象
    gc_clusterer.setGCSize(cg_size_); // 设置几何一致性聚类尺寸
    gc_clusterer.setGCThreshold(cg_thresh_); // 设置几何一致性聚类的阈值

    gc_clusterer.setInputCloud(model_keypoints);  // 设置模型关键点云
    gc_clusterer.setSceneCloud(scene_kepoints); // 设置场景关键点云
    gc_clusterer.setModelSceneCorrespondences(model_scene_corrs); // 设置模型于场景之间的对应关系

    gc_clusterer.cluster(clustered_corrs); // 执行几何一致性聚类，将结果存储在clustered_corrs中
    gc_clusterer.recognize(rototranslations, clustered_corrs); // 对聚类结果进行识别，将旋转-平移矩阵存在在rototranslations中

    // 输出识别结果
    std::cout << "Model instance found: " << rototranslations.size() << std::endl;
    for (size_t i=0; i<rototranslations.size(); ++i){
        std::cout << "\n      Instance " << std::endl;
        std::cout << "           Correspondces belonging to this instance: " << clustered_corrs[i].size() << std::endl;

        // 打印 相对于输入模型的旋转矩阵与平移矩阵  rotation matrix and translation vector
        // [R t]
        Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);//旋转矩阵
        Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);//平移向量

        printf ("\n");
        printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
        printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
        printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
        printf ("\n");
        printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
    }


    // 可视化
    pcl::visualization::PCLVisualizer viewer("Correspondence Grouping");
    viewer.addPointCloud(scene, "scene_cloud");

    pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr off_scen_model_keypoints(new pcl::PointCloud<PointType>);

    // 可视化平移后的模型点云
    pcl::transformPointCloud(*model, *off_scene_model, Eigen::Vector3f(-1,0,0), Eigen::Quaternionf(1, 0, 0, 0));
    pcl::transformPointCloud(*model_keypoints, *off_scen_model_keypoints, Eigen::Vector3f(-1, 0, 0), Eigen::Quaternionf(1, 0, 0, 0));
    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler(off_scene_model, 255, 255, 128);
    viewer.addPointCloud(off_scene_model, off_scene_model_color_handler, "off_scene_model");

    // 可视化 场景关键点和模型关键点
    pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler(scene_kepoints, 0, 0, 255);
    viewer.addPointCloud(off_scen_model_keypoints, scene_keypoints_color_handler, "scene_kypoints");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");

    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler(off_scen_model_keypoints, 0, 0, 255);
    viewer.addPointCloud(off_scen_model_keypoints, off_scene_model_keypoints_color_handler);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_kepoints");

    for (size_t i=0; i<rototranslations.size(); ++i){ // 对于模型在场景中匹配的点云
        pcl::PointCloud<PointType>::Ptr rotated_model(new pcl::PointCloud<PointType>); // 按匹配变换矩阵 模型
        pcl::transformPointCloud(*model, *rotated_model, rototranslations[i]);
        std::stringstream ss_cloud;
        ss_cloud << "instance" << i;

        pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler(rotated_model, 255, 0, 0);
        viewer.addPointCloud(rotated_model, rotated_model_color_handler, ss_cloud.str());

        // 显示匹配点连线
        for (size_t j = 0; j < clustered_corrs[i].size(); ++j)
        {
            std::stringstream ss_line; // 匹配点连线字符串
            ss_line << "correspondence_line" << i << "_"<<j;
            PointType& model_point = off_scen_model_keypoints->at(clustered_corrs[i][j].index_query);
            PointType& scene_point = scene_kepoints->at(clustered_corrs[i][j].index_match);

            viewer.addLine<PointType, PointType>(model_point, scene_point, 0, 2255, 0, ss_line.str());
        }
    }

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

应该出现的效果：

参考：

PCL-基于对应分组的三维物体识别_pcl::shotcolorestimation_程序猿小泽的博客-CSDN博客