PCL入门tutorial
文章目录
- 1 IO
- 2 kd tree和octo tree
-
- 2.1 kdtree search
- 2.2 octotree search
- 2.3 点云压缩 octree_pointcloud_compression
- 2.4 其他八叉树应用
- 3 点云的可视化
- 4 点云滤波
-
- 4.1 直通滤波器PassThrough
- 4.2 VoxelGrid下采样
- 4.3 statistical outlier removal
- 4.4 使用参数化模型投影点云
- 4.5 从点云中提取一个子集
- 4.6 使用ConditionalRemoval和RadiusOutlierRemoval移除outliers
- 4.7 CropHull任意多边形内部点云提取
- 5 点云转深度图
- 6 key points提取
- 7 采样一致性算法
-
- 7.1 RANSANC随机采样一致性算法
- 7.2 LMedS 最小终止方差估计算法
- 7.3 PCL中的sample_consensus模块支持的几何模型
- 8 点云特征描述与提取
-
- 8.1 normal estimate
- 8.2
本文是上述书籍的简要笔记。
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
待更新。。。