点云PCL库的学习笔记

基本类型：

PointCloud<>，是一个模板类，模板所包含的数据内容有以下几个

• width(int)：指定点云数据集的宽度
  • 对于无组织格式的数据集，width代表了所有点的总数
  • 对于有组织格式的数据集，width代表了一行中的总点数
• height(int)：制定点云数据集的高度
  • 对于无组织格式的数据集，值为1
  • 对于有组织格式的数据集，表示总行数
• points(std::vector<PointT>)：包含所有PointT类型的点的数据列表

模板中经常使用的数据类型一般有以下几种：

• PointXYZ - float x, y, z
• PointXYZI - float x, y, z, intensity
• PointXYZRGB - float x, y, z, rgb
• PointXYZRGBA - float x, y, z, uint32_t rgba
• Normal - float normal[3], curvature 法线方向，对应的曲率的测量值
• PointNormal - float x, y, z, normal[3], curvature 采样点，法线和曲率
• Histogram - float histogram[N] 用于存储一般用途的n维直方图

上述的数据类型都是用来初始化points中的的内容。

点云pcd文件查看的常用的指令：

pcl_viewer工具

• 基本使用

  进入：pcl_viewer xxxxx.pcd

  帮助：在界面中输入h，可以在控制台看到帮助信息

  退出：界面中输入q

  放大缩小：鼠标滚轮 或 Alt + [+/-]

  平移：Shift+鼠标拖拽

  旋转：Ctrl+鼠标拖拽

• 其他技巧

  修改点颜色：数字1,2,3,4,5....9，重复按1可切换不同颜色方便观察

  放大缩小点：放大Ctrl+Shift+加号，缩小 Ctrl+减号

  保存截图：j

  显示颜色尺寸：u

  显示比例尺寸：g

  在控制列出所有几何和颜色信息：l

• 鼠标选点打印坐标

  选点模式进入：pcl_viewer -use_point_picking bunny.pcd

  选择指定点：shift+鼠标左键

点云常用的数据结构：k-d tree

初始化

//初始化kd树的实现类
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree
//<>中的数据类型需要和输入的点云数据的数据类型格式一致
//设置搜索空间
kdtree.setInputCloud(cloud);

kd树的使用：两种方法

方法一：设置邻近点数量进行搜索

int K = 10;
//设置用来存放返回结果的容器
    // pointIdxNKNSearch        保存搜索到的临近点的索引
    // pointNKNSquaredDistance  保存对应临近点的距离的平方
vector<int> pointIdxNKNSearch(K);
vector<float> pointNKNSquaredDistance(K);

//开始搜索使用函数
kdtree.nearestKSearch(serach_point,K,pointIdxNKNSearch,pointNKNSquaredDistance)
//如果找到了的话，那么上面的函数的返回值就大于0
//然后只需要将前面定义的容器内的数据提取出来即可
for (int i = 0; i < pointIdxNKNSearch.size(); i++)
        {
            std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x
                      << " " << cloud->points[pointIdxNKNSearch[i]].y
                      << " " << cloud->points[pointIdxNKNSearch[i]].z
                      << " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl;
        }

方法二：指定半径进行搜索

    // 方式二：通过指定半径搜索
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    // 创建一个随机[0,256)的半径值
    float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

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


    if (kdtree.radiusSearch(serach_point, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
            std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x
                      << " " << cloud->points[pointIdxRadiusSearch[i]].y
                      << " " << cloud->points[pointIdxRadiusSearch[i]].z
                      << " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }

Livox_LOam中就是利用kd树，取当前帧中的对应类型的特征点，从地图中已经匹配好的场景中的特征点进行搜索对应的5个离当前帧中的点最近的点，并且这五个点需要近似在一条直线上

判断在一条直线的是因为，求残差的公式，本质也是求点到直线的距离，因此需要确保五个点组成的线是一条直线

PCL体素滤波（降采样）使用

体素滤波可以在保持点云形状完整的基础上降低点云的密度，减少点云的数据量

体素滤波的原理就是通过设定好的box的规格，将整个点云集合分成若干个box，然后box中选择具有代表性的一点，重点或者中点来表示着整个box 的其他点，以此来减少点云的复杂程度

//创建体素滤波器
pcl::VoxelGrid<pcl::PointXYZ> filter_vec;
//<>中的数据类型需要和点云的数据类型一致
float leafsize = %.4f;//创建cube的大小为4cm
//定义输出的点云变量，需要和输入的变量类型一致
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
//开始滤波
filter_vec.setInputCloud(cloudin);
filter_vec.setLeafSize(leftSize,leftSize,leftSize);//正方体
filter_vec.filter(*cloud_filter);

//输出后的信息保存在了cloud_filter中
cloud_filter->height * cloud_filter->width//可以计算出来点云中点的数量
//也可以通过
cloud_filter->points.size()

快速降采样

• 对一个点云进行降采样：

  输入input.pcd，输出output.pdf

pcl_voxel_grid input.pcd output.pcd -leaf 0.03,0.03,0.03

PCL自带的随机一致性算法接口进行点云的分割

算法会查找在一定与值范围内的内点，其他大于阈值范围的会被当作离群点，然后将内点保存在容器inliers当中，inliers当中包含的是所有内点在原始点云中的索引

最简单的是RANSAC

//创建模型系数对象，用来保存算法所古迹出来的俄模型的方程的系数  ax+by+ca+d=0形式 
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
//创建存储内点的点索引集合对象 inliers
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
//创建估计器，类型需要和点云的输入类型一致
pcl::SACSegmentation<pcl::PointXYZ> seg;

创建好相对应的容器之后，就要进行配置

// 可选配置：是否优化模型系数

seg.setOptimizeCoefficients(true);

// 必选配置：设置分割的模型类型、分割算法、距离阈值、输入点云

seg.setModelType(pcl::SACMODEL_PLANE);//平面点

seg.setMethodType(pcl::SAC_RANSAC);//选择算法

seg.setDistanceThreshold(0.01);//设定离群点阈值

seg.setInputCloud(cloud);//设置输入点云

// 执行分割操作，并存储分割结果保存到点集合 inliers 及存储平面模型系数 coefficients

seg.segment(*inliers, *coefficients);//

得到点的索引合集之后，可以采用ExtractIndices容器将其的索引进行提取

使用方法如下：

//用来将分割后的点进行提取，点提取类型，跟<>数据类型有关
pcl::ExtractIndices<PointType>::Ptr  extract(new pcl::ExtractIndices<PointType>);
//设置输入的点云
extract->setInputCloud(cloudin);
//设置前一步提取得到的索引列表
extract->setIndices(indicies);
//提取范围内的点，选择false的时候就是提取索引内的点，true的话就是选择索引外的点
extract->setNegative(false);
//提取点并保存到cloudout当中
extract->filter(*cloudout);

pcl点云点的平面法向量的计算

点的法向量需要用到以下的数据类型

pcl::Normal

上述表示的就是点云的发现爱唔靓的数据类型，其包含的主要内容有：

     normal_x = p.normal_x; //x方向的法向量的值
     normal_y = p.normal_y; //y方向的法向量的值
     normal_z = p.normal_z;//z方向的法向量的值
     data_n[3] = 0.0f;
     curvature = p.curvature;//法向量的俄曲率

想要求取某一点的法向量的估值，就需要创建以下的估计法向量的容器：

pcl::NormalEstimation<PointType,pcl::Normal> ne;

同时还需要创建一个用来储存法向量的数据集合

pcl::PointCloud<pcl::Normal>::Ptr cloud_normal(new pcl::PointCloud<pcl::Normal>);

另外根据官方的文档（估计一个点云的表面法线）中可以得得知，求解方向量的时候需要用到对当前点p的附近的一定数量（k）的邻近点进行检索，然后对当前的点p所处的平面是用最小二乘法来进行法向量的求解，因此这个过程就需要用到kdtree来辅助检索

这一步中的kd树跟之前的不同：

//法向量估计中的kdtree
pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);

//普通的kdtree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

然后就可以进行配置：

ne.setInputCloud(cloudin);

ne.setSearchMethod(tree);

ne.setKSearch(30);

//cloud_normal中包含了每个点法线的xyz信息

ne.compute(*cloud_normal);

最终输出的结果会储存在cloud_normal当中

需要注意的是，cloud_normal当中可能会存在nan的情况，也就是找不到法向的情况，因此如果需要直接使用法向量中的值的话需要进行去除nan

cloud_normal中对应的输入点云中每一个点的法向的值，对于地面法向量会有一个特点就是z方向的值会比较大，因此可以根据这个特点来进行一次筛选，这样筛选过后，后面再进行分割的时候，效果就会好一些，可以提前过滤一些点

可以用一下的函数

    //地面法线的z值都比较高，因此可以利用这一点将较高的z值的索引保存起来，然后在这做鞋垫当中再进行滤波
    for (size_t i = 0; i < cloud_normal->size(); i++)
    {
        const auto &pt = cloud_normal->points[i];
        const double norm_z = pt.normal_z;
        if(isnan(pt.normal_x) || isnan(pt.normal_x) || isnan(pt.normal_x))
        {
            //不执行剩下的语句，直接开始下一次的循环
            continue;
        }
        if(abs(norm_z) > 0.3)
        {
            //将z值比较大的点云点的索引全部装入索引的容器当中，用于后续的点云的过滤
            indices_temp->indices.push_back(i);
        }
    }

通过以上的代码就可以将法向量集合中满足z方向值的条件的索引提取出来并放置在容器中，后续就可以用这个进行分割

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.01);
    seg.setInputCloud(cloudin);
    seg.setIndices(indices_temp);
    seg.segment(*Indices_plane,*Coefficients_plane);

然后再配合前面所说的更改true或者false的情况来进行分割后别的物体点云的提取

对于圆柱体形状的对象的分割

需要创建一个特殊的分割器

pcl::SACSegmentationFromNormals<PointType,pcl::Normal> seg_cylinder;

上述分割器属于pcl::SACSegmentation的一个子类，它可以用法向量来进行辅助的圆柱体分割，因为对于圆柱体来说法向量的方向跟地面方向不同，因此可以利用这点来进行分割

分割需要进行的步骤

要先通过前面的全部的法向的数据得到除开平面之外的法向量数据，用到的就是通过前面第一次分割得到的平面点的索引集合Indices_plane，然后使用以下的代码

    extract_cylinder.setNegative(true);
    extract_cylinder.setInputCloud(cloud_normal);
    extract_cylinder.setIndices(Indices_plane);
    extract_cylinder.filter(*cloud_normal2);

得到了除开平面之外的数据之后，就可以开始进行配置

    // 设置圆柱体分割对象参数
    seg_cylinder.setOptimizeCoefficients(true);
    seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER);   // 设置分割模型为圆柱体
    seg_cylinder.setMethodType(pcl::SAC_RANSAC);         // 设置采用RANSAC算法进行参数估计
    seg_cylinder.setNormalDistanceWeight(0.1);           // 设置表面法线权重系数
    seg_cylinder.setMaxIterations(10000);                // 设置最大迭代次数10000
    seg_cylinder.setDistanceThreshold(0.05);             // 设置内点到模型的最大距离 0.05m
    seg_cylinder.setRadiusLimits(0, 0.1);                // 设置圆柱体的半径范围0 -> 0.1m
    seg_cylinder.setInputCloud(cloud_noneground);
    seg_cylinder.setInputNormals(cloud_normal2);

    seg.segment(*Indices_cylinder,*Coefficients_cylinder);

这样便提取出来了。

关于pcl点云的分割

需要灵活的运用分割器和取点器的特性，对于不同的情况下的true或者false的取值，就可以得到不同的点云数据

extract->setNegative(false);
extract->setNegative(true);