PCL学习笔记（二）：PCL官方教程学习

本节学习PCL的官方文档教程，记录学习过程

---

PCD文件制作

在gazebo中建立仿真环境，通过Realsense获取点云，保存为PCD文件，一个是走廊path_corner.pcd，一个是几何物体objects.pcd

---

Features

表面法线提取

参考官方文档：Estimating Surface Normals in a PointCloud

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

// 计算表面法线
pcl::PointCloud<pcl::Normal>::Ptr estimate_surface_normals( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    // Create the normal estimation class, and pass the input dataset to it
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud (cloud);
    // Create an empty kdtree representation, and pass it to the normal estimation object.
    // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    ne.setSearchMethod (tree);
    // Output datasets
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    // Use all neighbors in a sphere of radius 3cm
    ne.setRadiusSearch (0.03);
    // Compute the features
    ne.compute (*cloud_normals);
    
    return cloud_normals;
}
 
int main(int argc, char** argv) {
    // 读取pcd点云文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/objects.pcd", *cloud) == -1) {
        PCL_ERROR("Couldn't read file objects.pcd\n");
        return(-1);
    }
    // 计算法线
     pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    normals = estimate_surface_normals(cloud);
    std::cout<<"normals points size:"<<normals->points.size()<<std::endl;
    return 0;
}

其他参考博客：
pcl::Normal的定义以及cout
调用pcl计算法向量，并将法向量可视化
PCL：从法线计算到曲率计算并可视化

---

Keypoints

提取NARF关键点

参考代码：PCL系列——从深度图像(RangeImage)中提取NARF关键点
参考博客：
PCL关键点检测–NARF关键点
点云库PCL学习——NARF关键点

NARF（Normal Aligned Radial Feature）关键点是为了从深度图像中识别物体而提出的，关键点探测的重要一步是减少特征提取时的搜索空间，把重点放在重要的结构上。
对NARF关键点提取过程有以下要求：提取的过程必须将边缘以及物体表面变化信息考虑在内；关键点的位置必须稳定，可以被重复探测，即使换了不同视角；关键点所在的位置必须具有稳定的支持区域，可以计算描述子并进行唯一的法向量估计。
该方法提取步骤如下：
1、遍历每个深度图像点，通过寻找在邻近区域有深度突变的位置进行边缘检测。
2、遍历每个深度图像点，根据邻近区域的表面变化决定一种测度表面变化的系数，以及变化的主方向。
3、根据第二步找到的主方向计算兴趣值，表征该方向与其它方向的不同，以及该处表面的变化情况，即该点有多稳定
4、对兴趣值进行平滑滤波
5、进行无最大值压缩找到最终的关键点，即为NARF关键点

关键代码

  //从点云数据，创建深度图像
  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage); //创建RangeImage对象（指针）
  pcl::RangeImage& range_image = *range_image_ptr;  //引用
  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size); //从点云创建深度图像
  range_image.integrateFarRanges (far_ranges); //整合远距离点云
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();
    
  //提取NARF关键点
  pcl::RangeImageBorderExtractor range_image_border_extractor; //创建深度图像的边界提取器，用于提取NARF关键点
  pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor); //创建NARF对象
  narf_keypoint_detector.setRangeImage (&range_image);
  narf_keypoint_detector.getParameters ().support_size = support_size;
  //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
  //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;

运行结果如下，提取了10个特征点

---

KdTree

A k-d tree, or k-dimensional tree, is a data structure used in computer science for organizing some number of points in a space with k dimensions. It is a binary search tree with other constraints imposed on it. K-d trees are very useful for range and nearest neighbor searches.

KdTree常用于进行范围搜索或K近邻搜索的计算
参考代码：How to use a KdTree to search

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <vector>
#include <iostream>
 
int main(int argc, char** argv) {
    // 读取pcd点云文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/objects.pcd", *cloud) == -1) {
        PCL_ERROR("Couldn't read file objects.pcd\n");
        return(-1);
    }

    // 构造kd tree
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud (cloud);
    
    // 选取搜索点
    pcl::PointXYZ searchPoint;
    searchPoint.x = 0.1;
    searchPoint.y = 0.1;
    searchPoint.z = 0.1;

    // K nearest neighbor search
    int K = 10;
    std::vector<int> pointIdxKNNSearch(K);
    std::vector<float> pointKNNSquaredDistance(K);

    std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y  << " " << searchPoint.z<< ") with K=" << K << std::endl;

    if ( kdtree.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0 )
    {
    for (std::size_t i = 0; i < pointIdxKNNSearch.size (); ++i)
        std::cout << "    "  <<   (*cloud)[ pointIdxKNNSearch[i] ].x 
                << " " << (*cloud)[ pointIdxKNNSearch[i] ].y 
                << " " << (*cloud)[ pointIdxKNNSearch[i] ].z 
                << " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl;
    }

    // Neighbors within radius search
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;
    float radius = 1;

    std::cout << "Neighbors within radius search at (" << searchPoint.x  << " " << searchPoint.y  << " " << searchPoint.z<< ") with radius=" << radius << std::endl;

    if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
    {
    for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
        std::cout << "    "  <<   (*cloud)[ pointIdxRadiusSearch[i] ].x 
                << " " << (*cloud)[ pointIdxRadiusSearch[i] ].y 
                << " " << (*cloud)[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }
   
    return 0;
}

---

Range Image

How to create a range image from a point cloud

参考代码：How to create a range image from a point cloud

根据输入的PCD文件生成Range Image，并进行可视化，结果如下图所示

How to extract borders from range images

参考代码:
How to extract borders from range images
【点云处理技术之PCL】range image——提取深度图像的边界并可视化
从深度图像中提取三种边界：
object borders：目标最外层的边界
veil points：障碍物边界和阴影边界之间的插值点
shadow border：与遮挡相邻的背景点

关键代码

  pcl::RangeImageBorderExtractor border_extractor(&range_image);
  pcl::PointCloud<pcl::BorderDescription> border_descriptions;
  border_extractor.compute(border_descriptions);

运行结果

---

Segmentation

Plane model segmentation

参考代码：Plane model segmentation
参考博客：PCL系列——平面模型分割

完成平面模型的分割

Cylinder model segmentation

参考代码：Cylinder model segmentation
参考博客：
PCL 平面、圆柱 模型分割
利用 PCL 分割平面上的物体

算法流程：

1. 直通滤波在Z轴上滤除桌后的点云，得到滤波后的点云cloud_filtered
2. 法线估计，输入为cloud_filtered，得到法线估计结果cloud_normals
3. 平面分割，使用RANSAC算法进行平面分割，输入为cloud_filtered和cloud_normals，需要设置最大迭代次数、距离阈值等参数，得到平面分割的４个系数coefficients_plane，然后通过extract对输入点云cloud_filtered进行提取
4. 反向提取剩余的点云，得到cloud_filtered2，获取法线cloud_normals2
5. 圆柱分割，同样使用RANSAC算法，输入为cloud_filtered2和cloud_normals2，设置相应的参数，得到分割系数coefficients_cylinder，通过extract提取圆柱点云

原PCD文件

分割结果