PCL入门tutorial

本文是上述书籍的简要笔记。

1 IO

• read and arite PCD file

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd.pcd", *cloud)
pcl::io::savePCDFileASCII("test_pcd.pcd",*cloud);
// 还有另外两种方法
pcl::io::savePCDFileBinary()
pcl::io::savePCDFileBinaryCompressed()

• 拼接

// 1 只拼接points xyz+xyz = xyz
cloud_c = cloud_a;
cloud_c += cloud_b;

// 2 拼接字段 xyz + normal = xyz_normal
// xyz+rgb = xyzrgb
pcl::concatenateFields(cloud_a, n_cloud_b, p_n_cloud_c);

• 格式转换

.las
.ply
.pcd

• rviz中点云的自定义显示

// 参看第3章/9/2文件夹下

2 kd tree和octo tree

基于FLANN进行快速最近邻检索。
在匹配，描述子计算，邻域特征提取中是非常基础的操作。

2.1 kdtree search

// set input cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
// set search point
pcl::PointXYZ searchPoint;
searchPoint.x = 1;
searchPoint.y = 2;
searchPoint.z = 3;

// init result vector
int K = 10;
std::vector<int> pointIdxNKNSearch(K);//idx
std::vector<float> pointNKNSquaredDistance(K);//dis

// 1 最近邻的指定数量查找
// 返回值是实际搜索到的neighbor数量，查找的neighbor索引和距离在两个vector中
int number_neighbors_found = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)

// 2 指定半径内所搜索查找
int radiusSearch (const PointT &p_q, double radius, 
                std::vector<int> &k_indices,
                std::vector<float> &k_sqr_distances, unsigned int max_nn = 0);

2.2 octotree search

• neighbors within voxel search （体素内最近邻搜索）
• k nearest neighbor search （K近邻搜索）
• neighbor within radius search（半径内近邻搜索）

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();

pcl::PointXYZ searchPoint;
// 1 体素内最近邻搜索
std::vector<int>pointIdxVec;
int num = octree.voxelSearch (searchPoint, pointIdxVec);
// 2 K近邻搜索
int K =10;
std::vector<int>pointIdxNKNSearch;
std::vector<float>pointNKNSquaredDistance;
int num = octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
// 3 半径内近邻搜索
std::vector<int>pointIdxRadiusSearch;
std::vector<float>pointRadiusSquaredDistance;
float radius = 0.5;
int num = octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

2.3 点云压缩 octree_pointcloud_compression

2.4 其他八叉树应用

• pcl::octree::OctreePointCloudSearch
  搜索
• pcl::octree::OctreePointCloudPointVector=OctotreePointCloud
  保存叶节点的点索引列
• pcl::octree::OctreePointCloudSinglePoint
  仅保存叶节点单个点索引
• pcl::octree::OctreePointCloudOccupancy
  不存储叶节点任何信息，用于空间填充情况检查
• pcl::octree::OctreePointCloudDensity
  存储叶节点体素中点数目，进行空间点密集程度查询
• pcl::octree::OctreePointCloudVoxelCentroid
  保存占据的体素中心点坐标
• pcl::octree::OctreePointCloudChangeDetector
  用于无序点云的空间变化检测，可能在尺寸，分辨率，密度，点顺序等方面有所差异
  这个只能检测cloudB相对cloudA增加的点，不能获取减少的点

pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (newpcl::PointCloud<pcl::PointXYZ> );
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (newpcl::PointCloud<pcl::PointXYZ> );
octree.setInputCloud (cloudA);
octree.addPointsFromInputCloud ();// 从输入点云构建八叉树
octree.switchBuffers (); // 交换缓存，但是cloudA的八叉树仍然在内存中
octree.setInputCloud (cloudB);
octree.addPointsFromInputCloud ();
std::vector<int>newPointIdxVector;
octree.getPointIndicesFromNewVoxels (newPointIdxVector);//获取B相对A增加的点的索引

3 点云的可视化

• 比如填充intensity，在rviz中可视化，表示区别
• 填充rgb字段，能够也能在rviz中可视化？
• 可视化点云法线
• 可视化画出某一形状
• 自定义交互，click
• 直方图绘制PCLPlotter
• 使用Qt进行更好的交互

4 点云滤波

问题：

1 点运输局密度不规则需要平滑
2 因为遮挡问题造成outliers，需要去除
3 数据量大，需要downsample
4 noise数据去除

方法：

1 按具体规则限制过滤去除点
2 通过filter，修改点的部分属性
3 对数据进行downsample

4.1 直通滤波器PassThrough

总之就是根据某一字段的限制条件，筛选出符合这些条件的点indices

  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud);
  
  // 可以使用pcl::getFieldsList()获取字段属性
  pass.setFilterFieldName ("z");//设置滤波的字段，这个字段可以是xyzrgb,intensity，或者自定义的属性。
  pass.setFilterLimits (0.0, 1.0);
  //pass.setFilterLimitsNegative (true); // 设置保留范围内，还是保留范围外的点
  pass.filter (*cloud_filtered);

4.2 VoxelGrid下采样

使用voxel中所有点的重心而不是中心，来逼近原有点云，减少点的数量，同时保持形状，在提高配准、曲面重建、形状识别的算法速度中非常实用。

pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);//体素大小为0.01m的立方体
sor.filter (*cloud_filtered);

4.3 statistical outlier removal

对每个点的邻域进行一个统计分析。
在输入数据中对点到其K个临近点的距离分布计算，得到计算点到K个临近点的平均距离。
假设所有的点得到结果为高斯分布，形状由均值和标准差决定，平均距离在标准范围之外的点为outliers。

  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50); //进行统计时考虑的查询点临近点树
  sor.setStddevMulThresh (1.0);//是否为outliers阈值
  //   sor.setNegative (true);
  sor.filter (*cloud_filtered);

4.4 使用参数化模型投影点云

将点云投影到某一个参数化的模型上面

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
// 参数模型
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;
// 投影设置
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);//模型设置
proj.setInputCloud (cloud);
proj.setModelCoefficients (coefficients);//参数模型设置
proj.filter (*cloud_projected);

4.5 从点云中提取一个子集

// 只保留距离激光雷达0.5-30m内的点
// 根据一定条件获取inliers的indices
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
for (size_t i = 0; i < filtered_scan_ptr->size(); i++) {
    pcl::PointXYZ pt(filtered_scan_ptr->points[i].x, filtered_scan_ptr->points[i].y, filtered_scan_ptr->points[i].z);
    double distance = sqrt(pt.x*pt.x + pt.y*pt.y);
    if (distance < 30 && distance > 0.5) {
        inliers->indices.push_back(i);
    }
}

// 提取出指定indices的inliners点
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(filtered_scan_ptr);
extract.setIndices(inliers);
extract.setNegative(false); //设置为false，提取指定indice处的点
extract.filter(*filtered_scan_ptr);

4.6 使用ConditionalRemoval和RadiusOutlierRemoval移除outliers

• RadiusOutlierRemoval
  指定每个点范围内周围至少有足够多的紧邻点

pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(0.8);//0.8m范围内
outrem.setMinNeighborsInRadius (2);//至少有2个points，否则视为outliers
outrem.filter (*cloud_filtered);

• ConditionalRemoval
  保留满足一定条件的点云

// 创建滤波器的限定条件
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZ> ());
// >0.0
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));
// <0.8
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));

// 根据限定条件得到符合条件的点云
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (cloud);
condrem.setKeepOrganized(true);//设置保持点云的结构
condrem.filter (*cloud_filtered);

4.7 CropHull任意多边形内部点云提取

// 创建凸包点云
pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
boundingbox_ptr->push_back(pcl::PointXYZ(0.1, 0.1, 0));
boundingbox_ptr->push_back(pcl::PointXYZ(0.1, -0.1,0 ));
boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, 0.1,0 ));
boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, -0.1,0 ));
boundingbox_ptr->push_back(pcl::PointXYZ(0.15, 0.1,0 ));
//创建凸包对象
pcl::ConvexHull<pcl::PointXYZ> hull;
hull.setInputCloud(boundingbox_ptr);//设置点云
hull.setDimension(2);//设置维度
std::vector<pcl::Vertices> polygons;//顶点点云
pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);//形状点云
hull.reconstruct(*surface_hull, polygons);//得到顶点和形状点云

pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
pcl::CropHull<pcl::PointXYZ> bb_filter;
bb_filter.setDim(2);/
bb_filter.setInputCloud(cloud);
bb_filter.setHullIndices(polygons);//设置顶点
bb_filter.setHullCloud(surface_hull);//设置形状
bb_filter.filter(*objects);

5 点云转深度图

pcl::RangeImage rangeImage;
rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);

6 key points提取

• Normal Aligned Radial Feature(NARF)
• Harris
  2D Harris检测利用图像梯度，3D利用点云表面法向量信息
• SIFT

7 采样一致性算法

应用：

• 主要是对点云进行分割，根据设定的几何模型，估计随影的几何模型参数，在一定允许误差范围内分割出模型上的点云
• 点云配准的outliers剔除

7.1 RANSANC随机采样一致性算法

• 算法流程
  从样本中随机抽选一个样本子集，使用最小方差估计算法对子集计算模型参数，然后计算所有样本与该模型的偏差。然后使用一个预先设定好的阈值与偏差比较，偏差小于阈值的时候，样本点为inliers，否则为outliers。记录下当前的inliers数目和估计的模型参数。每一次重复，都记录下当前最佳的参数，也就是inliers最多的那个。重复N次，得到最佳模型，对应这估计的模型。
  每次迭代结束，都会根据期望的误差率，best_inliners、样本综述、当前迭代次数，计算一个评判因子，决定是否结束迭代。

• 算法缺点

  • 需要事先设定阈值
  • 迭代次数无法预知，运行期才能确定

7.2 LMedS 最小终止方差估计算法

• 算法流程
  从样本中随机抽选一个样本子集，使用最小方差估计算法对子集计算模型参数，然后计算所有样本与该模型的偏差。但是与RANSAC不同的是，LMedS记录的是所有样本中，**偏差值居中的那个样本的偏差，以及本次计算得到的模型参数。**由于这个变化，LMedS不需要预先设定阈值来区分inliers和outliers。
  重复前面N次，从N个Med偏差中挑选出最小的一个，其对应的模型参数就是最终的估计值。
  其中迭代次数N由样本子集中样本个数、期望的模型误差、事先估计样本中outliers的比例决定的。
• 对比
  LMedS理论上消除了RANSAC的两个缺点。但是当outliers超过50%的时候，算法也会失效。

7.3 PCL中的sample_consensus模块支持的几何模型

• line
• circle2D
• cylinder圆柱体
• cone圆锥
• torus圆环面
• parallel_line有条件限制的直线模型，与给定直线平行
• perpendicular_plane有条件限制的平面模型，与给定轴线垂直
• parallel_plane 有条件限制的平面模型，与给定轴线平行
• normal_parallel_plane 有条件限制的平面模型，法线约束下，三维平面模型必须与非定轴线平行

8 点云特征描述与提取

8.1 normal estimate

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

ne.setInputCloud (cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);//设置搜索方法
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);//设置搜索半径
ne.compute (*cloud_normals);//计算法线

求解法线问题就是一个最小二乘法平面拟合问题：

• 得到该点附近的紧邻点
• 求出这些点的协方差矩阵
• 求出特征值最小的对应的那个特征向量就是法线向量，但是方向具有二义性

关于近邻k的设置问题，设置太小和太大都不合适。

8.2

待更新。。。