PCL教程-点云分割之平面模型分割
原文链接：Plane model segmentation — Point Cloud Library 0.0 documentation
基于RANSAC的基本检测算法虽然具有较高的鲁棒性和效率，但是目前仅针对平面，球，圆柱体，圆锥和圆环物种基本的基元。
在本次教程中，我们将学习对一组点云做简单的平面分割，也就是在点云中找到组成平面模型的所有点。
目录
程序代码
实验结果
程序分析
步骤1：创建在同一个平面上的点云（z=1）：
步骤2：设置几个平面外的点（z != 1)
步骤3：平面分割
步骤4：分割结果-系数因子
步骤5：分割结果-模型内点的下标
步骤6：打印结果并显示
CmakeLists.txt
程序代码
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
int
main(int argc, char** argv)
{
//原始点云
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
//平面上的点云
pcl::PointCloud::Ptr cloud_inner(new pcl::PointCloud);
//平面外的点云
pcl::PointCloud::Ptr cloud_outer(new pcl::PointCloud);
//填充点云数据
(*cloud).width = 15;
(*cloud).height = 1;
(*cloud).points.resize((*cloud).width * (*cloud).height);
//生成数据
for (size_t i = 0; i < (*cloud).points.size(); ++i)
{
(*cloud).points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
(*cloud).points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
//z 坐标始终为1，说明这些点位于同一个平面
(*cloud).points[i].z = 1.0;
}
//设置几个局外点，三个平面外的点
(*cloud).points[0].z = 200.0;
(*cloud).points[3].z = -200.0;
(*cloud).points[6].z = 400.0;
std::cerr << "Point cloud data: " << (*cloud).points.size() << " points" << std::endl;
for (size_t i = 0; i < (*cloud).points.size(); ++i)
std::cerr <<"index:\t"<< i<<"\t" << (*cloud).points[i].x << "\t"
<< (*cloud).points[i].y << "\t"
<< (*cloud).points[i].z << std::endl;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
//创建分割对象
pcl::SACSegmentation seg;
//可选设置
seg.setOptimizeCoefficients(true);
//必须设置
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
//判断是否分割成功
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << std::endl << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl << std::endl;
//根据分割结果填充平面内和平面外点云
cloud_inner->width = inliers->indices.size();
cloud_inner->height = 1;
cloud_inner->points.resize(cloud_inner->width * cloud_inner->height);
cloud_outer->width = cloud->points.size() - inliers->indices.size();
cloud_outer->height = 1;
cloud_outer->points.resize(cloud_outer->width * cloud_outer->height);
//创建一个数组，大小为点云总数,初始化为0
std::vector p_flag(cloud->points.size());
//将平面内的点标记
for (size_t i = 0; i < inliers->indices.size(); ++i)
p_flag[inliers->indices[i]] = 1;
for (size_t i = 0,j=0 ; i < (*cloud).points.size(); ++i)
{
//遍历，找出平面外的点
if (p_flag[i] == 0)
{
cloud_outer->points[j].x = (*cloud).points[i].x;
cloud_outer->points[j].y = (*cloud).points[i].y;
cloud_outer->points[j].z = (*cloud).points[i].z;
++j;
std::cerr << "outer points index:\t" << i << "\t" << (*cloud).points[i].x << "\t"
<< (*cloud).points[i].y << "\t"
<< (*cloud).points[i].z << std::endl;
}
}
//打印出平面外的点
std::cerr << std::endl << "Outer points: " << cloud_outer->points.size() << std::endl;
for (size_t i = 0; i < cloud_outer->points.size(); ++i)
{
std::cerr << "\t" << (*cloud_outer).points[i].x << "\t"
<< (*cloud_outer).points[i].y << "\t"
<< (*cloud_outer).points[i].z << std::endl;
}
//平面内的点
std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
for (size_t i = 0; i < inliers->indices.size(); ++i)
{
cloud_inner->points[i].x = (*cloud).points[inliers->indices[i]].x;
cloud_inner->points[i].y = (*cloud).points[inliers->indices[i]].y;
cloud_inner->points[i].z = (*cloud).points[inliers->indices[i]].z;
std::cerr << "index:\t" << inliers->indices[i] << "\t" << (*cloud).points[inliers->indices[i]].x << "\t"
<< (*cloud).points[inliers->indices[i]].y << "\t"
<< (*cloud).points[inliers->indices[i]].z << std::endl;
}
//图形化显示
//创建PCLVisualzer对象
pcl::visualization::PCLVisualizer viewer("Plane Model Segmentation");
int v1(1);
int v2(2);
//创建视角v1，v2
viewer.createViewPort(0.0, 0.0, 0.5, 1.0,v1);
viewer.createViewPort(0.5, 0.0, 1.0, 1.0,v2);
//设置背景颜色为白色
viewer.setBackgroundColor(255, 255, 255, v1);
viewer.setBackgroundColor(255, 255, 255, v2);
//添加直角坐标，放大1000倍
viewer.addCoordinateSystem(1000,v1);
viewer.addCoordinateSystem(1000,v2);
//设置点云颜色
pcl::visualization::PointCloudColorHandlerCustom cloud_origin(cloud, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom cloud_in(cloud_inner, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom cloud_out(cloud_outer, 0, 0, 255);
viewer.addPointCloud(cloud, cloud_origin, "v1", v1);
viewer.addPointCloud(cloud_outer, cloud_out, "v2", v2);
viewer.addPointCloud(cloud_inner, cloud_in, "v3", v2);
//设置点云的大小，point_size默认为1，这里设置为1000，突出显示
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1000,"v1",v1);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1000,"v2",v2);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1000,"v3",v2);
viewer.spin();
return (0);
}
实验结果
打印输出：
Point cloud data: 15 points
index: 0 1.28125 577.094 2
index: 1 197.938 828.125 1
index: 2 599.031 491.375 1
index: 3 358.688 917.438 -2
index: 4 842.562 764.5 1
index: 5 178.281 879.531 1
index: 6 727.531 525.844 4
index: 7 311.281 15.3438 1
index: 8 93.5938 373.188 1
index: 9 150.844 169.875 1
index: 10 1012.22 456.375 1
index: 11 121.938 4.78125 1
index: 12 9.125 386.938 1
index: 13 544.406 584.875 1
index: 14 616.188 621.719 1Model coefficients: 0 0 1 -1
outer points index: 0 1.28125 577.094 2
outer points index: 3 358.688 917.438 -2
outer points index: 6 727.531 525.844 4Outer points: 3
1.28125 577.094 2
358.688 917.438 -2
727.531 525.844 4
Model inliers: 12
index: 1 197.938 828.125 1
index: 2 599.031 491.375 1
index: 4 842.562 764.5 1
index: 5 178.281 879.531 1
index: 7 311.281 15.3438 1
index: 8 93.5938 373.188 1
index: 9 150.844 169.875 1
index: 10 1012.22 456.375 1
index: 11 121.938 4.78125 1
index: 12 9.125 386.938 1
index: 13 544.406 584.875 1
index: 14 616.188 621.719 1
结果图：
左图为原始点云，右图为处理结果：红色为同一平面上的点，蓝色为平面外的点。
程序分析
步骤1：创建在同一个平面上的点云（z=1）：
//填充点云数据
(*cloud).width = 15;
(*cloud).height = 1;
(*cloud).points.resize((*cloud).width * (*cloud).height);
//生成数据
for (size_t i = 0; i < (*cloud).points.size(); ++i)
{
(*cloud).points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
(*cloud).points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
//z 坐标始终为1，说明这些点位于同一个平面
(*cloud).points[i].z = 1.0;
}步骤2：设置几个平面外的点（z != 1)
//设置几个局外点，三个平面外的点
(*cloud).points[0].z = 200.0;
(*cloud).points[3].z = -200.0;
(*cloud).points[6].z = 400.0;步骤3：平面分割
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
//创建分割对象
pcl::SACSegmentation seg;
//可选设置
seg.setOptimizeCoefficients(true);
//必须设置
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
//判断是否分割成功
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
} 创建:pcl:`SACSegmentation
在篇教程中，我们使用了RANSAC方法(pcl::SAC_RANSAC) ，因为Ransac的简单性的动机（其他强大的估算器用作基础并添加额外的更复杂的概念）。
步骤4：分割结果-系数因子
分割结果包括：模型内的点的下标以及该模型的系数因子。
比如一个平面的方程式为：aX + bY + cZ + d = 0
在此次实验中得出的系数因子为：0 0 1 -1
即 a=0 , b=0 , c=1, d=-1
将平面上的点代入该方程即可验证结果为正确。
步骤5：分割结果-模型内点的下标
分割结果中包括在平面模型内的点的下标（在原始点云中），通过这个下标，就可以将平面内和平面外的点云分割开，单独显示。
Model inliers: 12
index: 1 197.938 828.125 1
index: 2 599.031 491.375 1
index: 4 842.562 764.5 1
index: 5 178.281 879.531 1
index: 7 311.281 15.3438 1
index: 8 93.5938 373.188 1
index: 9 150.844 169.875 1
index: 10 1012.22 456.375 1
index: 11 121.938 4.78125 1
index: 12 9.125 386.938 1
index: 13 544.406 584.875 1
index: 14 616.188 621.719 1
//根据分割结果填充平面内和平面外点云
cloud_inner->width = inliers->indices.size();
cloud_inner->height = 1;
cloud_inner->points.resize(cloud_inner->width * cloud_inner->height);
cloud_outer->width = cloud->points.size() - inliers->indices.size();
cloud_outer->height = 1;
cloud_outer->points.resize(cloud_outer->width * cloud_outer->height);
//创建一个数组，大小为点云总数,初始化为0
std::vector p_flag(cloud->points.size());
//将平面内的点标记
for (size_t i = 0; i < inliers->indices.size(); ++i)
p_flag[inliers->indices[i]] = 1;
for (size_t i = 0,j=0 ; i < (*cloud).points.size(); ++i)
{
//遍历，找出平面外的点
if (p_flag[i] == 0)
{
cloud_outer->points[j].x = (*cloud).points[i].x;
cloud_outer->points[j].y = (*cloud).points[i].y;
cloud_outer->points[j].z = (*cloud).points[i].z;
++j;
std::cerr << "outer points index:\t" << i << "\t" << (*cloud).points[i].x << "\t"
<< (*cloud).points[i].y << "\t"
<< (*cloud).points[i].z << std::endl;
}
}
//打印出平面外的点
std::cerr << std::endl << "Outer points: " << cloud_outer->points.size() << std::endl;
for (size_t i = 0; i < cloud_outer->points.size(); ++i)
{
std::cerr << "\t" << (*cloud_outer).points[i].x << "\t"
<< (*cloud_outer).points[i].y << "\t"
<< (*cloud_outer).points[i].z << std::endl;
}
//平面内的点
std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
for (size_t i = 0; i < inliers->indices.size(); ++i)
{
cloud_inner->points[i].x = (*cloud).points[inliers->indices[i]].x;
cloud_inner->points[i].y = (*cloud).points[inliers->indices[i]].y;
cloud_inner->points[i].z = (*cloud).points[inliers->indices[i]].z;
std::cerr << "index:\t" << inliers->indices[i] << "\t" << (*cloud).points[inliers->indices[i]].x << "\t"
<< (*cloud).points[inliers->indices[i]].y << "\t"
<< (*cloud).points[inliers->indices[i]].z << std::endl;
} 步骤6：打印结果并显示
- 使用PCLVisualizer创建视图对象
- 使用pcl::visualization::PointCloudColorHandlerCustom设置点云颜色
- 使用setPointCloudRenderingProperties()设置点的大小（突出显示）
//图形化显示
//创建PCLVisualzer对象
pcl::visualization::PCLVisualizer viewer("Plane Model Segmentation");
int v1(1);
int v2(2);
//创建视角v1，v2
viewer.createViewPort(0.0, 0.0, 0.5, 1.0,v1);
viewer.createViewPort(0.5, 0.0, 1.0, 1.0,v2);
//设置背景颜色为白色
viewer.setBackgroundColor(255, 255, 255, v1);
viewer.setBackgroundColor(255, 255, 255, v2);
//添加直角坐标，放大1000倍
viewer.addCoordinateSystem(1000,v1);
viewer.addCoordinateSystem(1000,v2);
//设置点云颜色
pcl::visualization::PointCloudColorHandlerCustom cloud_origin(cloud, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom cloud_in(cloud_inner, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom cloud_out(cloud_outer, 0, 0, 255);
viewer.addPointCloud(cloud, cloud_origin, "v1", v1);
viewer.addPointCloud(cloud_outer, cloud_out, "v2", v2);
viewer.addPointCloud(cloud_inner, cloud_in, "v3", v2);
//设置点云的大小，point_size默认为1，这里设置为1000，突出显示
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1000,"v1",v1);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1000,"v2",v2);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1000,"v3",v2);
viewer.spin(); CmakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(planar_segmentation)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (planar_segmentation planar_segmentation.cpp)
target_link_libraries (planar_segmentation ${PCL_LIBRARIES})