一、简介
根据不同尺度下法向量特征的差异性,利用类pcl::DifferenceOfNormalEstimation实现点云分割,该方法基于尺度的分割方法,可以根据给定尺度分割出对应尺度下的点云。基于Dof的多尺度分割方法计算效率高,可处理规模庞大的3D点云。该算法在概念上虽然非常简单,但在处理有较大尺度变化的场景点云分割时却异常有效。对输入点云P的每一个点p,利用不同的半径r1 > rs去估算同一点的两个单位法向量。利用输入点云的单位法向量的差定义DoN特征。通常DoN定义如下:
式中,rs,rl属于R,rs < rl,n(p, r)是在支撑半径为r的条件下,点p的表面法向量估计值。注意,该特征的输出值是单位法向量。因此,输出值是有方向的,该输出值的方向是关键特征,然而,该算子的范数提供了一个更易操作的量化方式,并且范围在0和1之间。
DoN特征远于观察到基于所给半径估计的表面法向量反映了曲面的内在几何特征。尽管,有许多方法可以去估计曲面法向量,但是常用的方法主要有两种,第一种是根据支撑半径,另一种是根据固定的邻近点的数量。综合来看,支撑半径决定了法向量表示的曲面结构的尺度。
利用DoN算子进行分割的步骤:
(1)对于输入点云数据的每一点,利用较大的支撑半径rl计算法向量。
(2)对于输入点云数据的每一点,利用较小的支撑半径rs计算法向量。
(3)对于输入点云数据的每一点,单位话每一点的DoN特征。
(4)过滤所得的向量域(DoN特征向量),分割出目标尺度对应的点云。
二、代码分析
1)估计法向量严重影响了算法的整体计算效率,所以采用NormalEstimationOMP类,借助多处理器技术,该类充分利用OpenMP去使用多线程去计算法向量。也可以仅仅使用单线程的pcl::NormalEstimation类或者GPU加速的类pcl::gpu::NormalEstimation。无论使用哪个类,设定一个任意但固定的视点非常重要,在法向量估计过程中,视点用于判断法线的最终方向,唯一的视点保证了在不同尺度下估计的法向量都有一致的方向:
// Compute normals using both small and large scales at each point
pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;//对输入点集的每一个点利用大和小两种尺度计算法向量
ne.setInputCloud (cloud);
ne.setSearchMethod (tree);
/**
* NOTE: setting viewpoint is very important, so that we can ensure
* normals are all pointed in the same direction!
*/
ne.setViewPoint (std::numeric_limits<float>::max (), std::numeric_limits<float>::max (), std::numeric_limits<float>::max ());//随机设定视点的坐标
2)接下来我们分别设置大尺度和小尺度两种支撑半径,利用法向量估计类计算输入点云的法线。这里,请使用NormalEstimation.setRadiusSearch()方法而不是NormalEstimation.set-MaximumNeighbours方法进行法向量估计。因为,法向量估计如果仅依赖于紧邻点,而不是依赖支撑半径的完整曲面,是不是和DoN特征向量估计的。对于密度较大的输入点云,如果支撑半径设置的较大,计算法向量是计算量很大的任务。当进行大尺度的支撑半径搜索时,一个简单的提升计算速度的方法是对输入点云进行均匀下采样处理:
// calculate normals with the small scale
//利用较小尺度辅助半径计算输入点集的法向量
cout << "Calculating normals for scale..." << scale1 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_small_scale (new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch (scale1);
ne.compute (*normals_small_scale);
// calculate normals with the large scale
//利用较大尺度辅助半径计算输入点集的法向量
cout << "Calculating normals for scale..." << scale2 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch (scale2);
ne.compute (*normals_large_scale);
3)这里利用pcl::DifferenceOfNormalsEstimation类计算DoN向量域,pcl::DifferenceOfNormals-Estimation类有三个参数。第一个参数是输入点云的类型,在本例中是pcl::PointXYZRGB。第二个参数是输入点云所计算出的法向量类型,在本例中是pcl::PointNormal。第三个参数是输出的向量域类型,在本例中,也是pcl::PointNormal类型。接下来设置输入点云,并计算输入点云的两个法向量,并利用pcl::DifferenceOfNormalsEstimation::initCompute()方法来核对计算特征向量的要求是否满足。最后利用pcl::DifferenceOfNormalsEstimation::computeFeature()方法计算DoN特征向量:
// Create output cloud for DoN results
PointCloud<PointNormal>::Ptr doncloud (new pcl::PointCloud<PointNormal>);
copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);
cout << "Calculating DoN... " << endl;
// Create DoN operator
pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
don.setInputCloud (cloud);
don.setNormalScaleLarge (normals_large_scale);
don.setNormalScaleSmall (normals_small_scale);
if (!don.initCompute ())
{
std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
exit (EXIT_FAILURE);
}
// Compute DoN
don.computeFeature (*doncloud);
4)当计算出每一个的DoN向量域,仍然保留完整的输入点云。为了开始分割,必须利用DoN特征向量的计算结果区分点云。常用的量化标准如下表所示:
点的向量域 | 描述 | 使用场景 |
float normal[3] | DoN向量 | 通过DoN相对角度分类 |
float curvature | DoN的二范数 | 通过尺度效应进行滤波,较大的模对输入尺度参数有较大效应 |
float normal[0] | DoN向量的x部分 | 利用方向尺度进行滤波,例如建筑外墙可能有较大的DoN向量的x部分、y部分或者z部分 |
float normal[1] | DoN向量的y部分 | |
float normal[2] | DoN向量的z部分 |
接下来,利用条件输入函数设定上述的量化标准,对输入点云进行滤波,滤波后的点云与输入尺度参数有极大的关联性:
// Build the condition for filtering
pcl::ConditionOr<PointNormal>::Ptr range_cond (
new pcl::ConditionOr<PointNormal> ()
);
range_cond->addComparison (pcl::FieldComparison<PointNormal>::ConstPtr (
new pcl::FieldComparison<PointNormal> ("curvature", pcl::ComparisonOps::GT, threshold))
);
// Build the filter
pcl::ConditionalRemoval<PointNormal> condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud (doncloud);
pcl::PointCloud<PointNormal>::Ptr doncloud_filtered (new pcl::PointCloud<PointNormal>);
// Apply filter
condrem.filter (*doncloud_filtered);
doncloud = doncloud_filtered;
// Save filtered output
std::cout << "Filtered Pointcloud: " << doncloud->points.size () << " data points." << std::endl;
if(SAVE==1)writer.write<pcl::PointNormal> ("don_filtered.pcd", *doncloud, false);
5)整体代码如下:
#include <string>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/impl/extract_clusters.hpp>
#include <pcl/features/don.h>
#include <pcl/visualization/pcl_visualizer.h>
using namespace pcl;
using namespace std;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr getColoredCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_, std::vector <pcl::PointIndices> clusters_, float r, float g, float b)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud;
if (!clusters_.empty())
{
colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared();
srand(static_cast<unsigned int>(time(0)));
std::vector<unsigned char> colors;
for (size_t i_segment = 0;i_segment < clusters_.size();i_segment++)
{
colors.push_back(static_cast<unsigned char>(rand() % 256));
colors.push_back(static_cast<unsigned char>(rand() % 256));
colors.push_back(static_cast<unsigned char>(rand() % 256));
}
colored_cloud->width = input_->width;
colored_cloud->height = input_->height;
colored_cloud->is_dense = input_->is_dense;
for (size_t i_point = 0;i_point < input_->points.size();i_point++)
{
pcl::PointXYZRGB point;
point.x = *(input_->points[i_point].data);
point.y = *(input_->points[i_point].data + 1);
point.z = *(input_->points[i_point].data + 2);
point.r = r;
point.g = g;
point.b = b;
colored_cloud->points.push_back(point);
}
std::vector<pcl::PointIndices>::iterator i_segment;
int next_color = 0;
for (i_segment = clusters_.begin();i_segment != clusters_.end();i_segment++)
{
std::vector<int>::iterator i_point;
for (i_point = i_segment->indices.begin();i_point != i_segment->indices.end();i_point++)
{
int index;
index = *i_point;
colored_cloud->points[index].r = colors[3 * next_color];
colored_cloud->points[index].r = colors[3 * next_color + 1];
colored_cloud->points[index].r = colors[3 * next_color + 2];
}
next_color++;
}
}
return(colored_cloud);
}
int main(int argc, char* argv[])
{
int VISUAL = 1, SAVE = 0;
double segRadius, mean_radius, threshold, scale1, scale2;
if (argc < 6)
{
cerr << "usage: " << argv[0] << " inputfile smallscale(5) largescale(10) threshold(0.1) segradius(1.5) VISUAL(1) SAVE(0)" << std::endl;
cerr << "usage: " << "smallscale largescale segradius :multiple with mean radius of point cloud" << std::endl;
exit(EXIT_FAILURE);
}
string infile = argv[1];
istringstream(argv[2]) >> scale1; //利用较小尺度辅助计算点集的法向量
istringstream(argv[3]) >> scale2; //利用较大尺度辅助计算点集的法向量
istringstream(argv[4]) >> threshold;
istringstream(argv[5]) >> segRadius; //设置聚类半径
istringstream(argv[6]) >> VISUAL; //设置可视化标志位
istringstream(argv[7]) >> SAVE; //设置存储文件标志位
pcl::PointCloud<PointXYZRGB>::Ptr cloud(new pcl::PointCloud<PointXYZRGB>); //加载点云
pcl::io::loadPCDFile(infile.c_str(), *cloud);
pcl::search::Search<PointXYZRGB>::Ptr tree;
if (cloud->isOrganized())
{
tree.reset(new pcl::search::OrganizedNeighbor<PointXYZRGB>());
}
else
{
tree.reset(new pcl::search::KdTree<PointXYZRGB>(false));
}
tree->setInputCloud(cloud); //初始化kdtree
{
int size_cloud = cloud->size(); //设置点云的大小
int step = size_cloud / 10; //设置kdtree的搜索步长
double total_distance = 0;
int i, j = 1;
for (i = 0;i < size_cloud;i += step, j++)
{
std::vector<int> pointIdxNKNSearch(2); //设置k近邻搜索索引的动态数组
std::vector<float> pointNKNSquaredDistance(2); //设置k近邻搜索的距离的动态数组
tree->nearestKSearch(cloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance); //利用kdtree进行k近邻搜索
total_distance += pointNKNSquaredDistance[1] + pointNKNSquaredDistance[0]; //计算k近邻搜索的距离之和
}
mean_radius = sqrt((total_distance / j)); //计算中值半径
cout << "mean radius of cloud is: " << mean_radius << endl;
scale1 *= mean_radius; //更新较小尺度
scale2 *= mean_radius; //更新较大尺度
segRadius *= mean_radius; //更新聚类半径
}
if (scale1 >= scale2)
{
cerr << "Error: Large scale must be > small scale" << endl;
exit(EXIT_FAILURE);
}
pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne; //使用NormalEstimationOMP类,利用多线程进行计算
ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
ne.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max());
cout << "Caculating normals for scale..." << scale1 << endl; //利用较小尺度辅助半径计算输入点集的法向量
pcl::PointCloud<PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch(scale1);
ne.compute(*normals_small_scale);
cout << "Caculating normals for scale..." << scale2 << endl; //利用较大尺度辅助半径计算输入点集的法向量
pcl::PointCloud<PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch(scale2);
ne.compute(*normals_large_scale);
if (VISUAL = 1)
{
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing normals with different scale"));
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> green(cloud, 0, 255, 0);
int v1(0), v2(0);
MView->createViewPort(0.0, 0.0, 0.5, 1.0, v1); //初始化不同的视口v1与v2
MView->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(cloud, green, "small_scale", v1);
MView->addPointCloud(cloud, green, "large_scale", v2);
MView->addPointCloudNormals<pcl::PointXYZRGB, pcl::PointNormal>(cloud, normals_small_scale, 100, mean_radius * 10, "small_scale_normal");
MView->addPointCloudNormals<pcl::PointXYZRGB, pcl::PointNormal>(cloud, normals_large_scale, 100, mean_radius * 10, "large_scale_normal");
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "small_scale", v1);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "small_scale", v1);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "large_scale", v1);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "large_scale", v1);
MView->spin();
}
PointCloud<PointNormal>::Ptr doncloud(new pcl::PointCloud<PointNormal>);
copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud); //计算DoN向量域
cout << "Caculating DoN..." << endl;
pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal>don; //计算DoN向量特征
don.setInputCloud(cloud);
don.setNormalScaleLarge(normals_large_scale);
don.setNormalScaleSmall(normals_small_scale);
if (!don.initCompute())
{
std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
exit(EXIT_FAILURE);
}
don.computeFeature(*doncloud);
{
cout << "You may have some sense about the input threshold(curvature) next time for your data" << endl;
int size_cloud = doncloud->size();
int step = size_cloud / 10;
for (int i = 0;i < size_cloud;i += step)
cout << " " << doncloud->points[i].curvature << " " << endl;
}
if (VISUAL = 1) //判断是否进行可视化
{
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("showing the difference of curvature of two scale"));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(doncloud, handler_k);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);
MView->spin();
}
pcl::PCDWriter writer;
if (SAVE = 1) //判断是否保存don文件
writer.write<pcl::PointNormal>("don.pcd", *doncloud, false);
cout << "Filtering out DoN mag <= " << threshold << "..." << endl;
pcl::ConditionOr<PointNormal>::Ptr range_cond(new pcl::ConditionOr<PointNormal>());
range_cond->addComparison(pcl::FieldComparison<PointNormal>::ConstPtr(new pcl::FieldComparison<PointNormal>("curvature", pcl::ComparisonOps::GT, threshold)));
pcl::ConditionalRemoval<PointNormal> condrem; //初始化一个条件滤波器
condrem.setCondition(range_cond); //设置滤波条件
condrem.setInputCloud(doncloud); //将DoN后的点云作为输入
pcl::PointCloud<PointNormal>::Ptr doncloud_filtered(new pcl::PointCloud<PointNormal>);
condrem.filter(*doncloud_filtered); //执行滤波
doncloud = doncloud_filtered;
std::cout << "Filted PointCloud: " << doncloud->points.size() << " data points." << std::endl;
if (SAVE == 1)
writer.write<pcl::PointNormal>("don_filtered.pcd", *doncloud, false); //保存滤波后的文件
if (VISUAL = 1) · //对滤波后的文件进行可视化
{
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("showing the results of keeping relative small curvature points"));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(doncloud, handler_k);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);
MView->spin();
}
cout << "Clustering using EuclideanClusterExtration with tolerance <= " << segRadius << "..." << endl;
pcl::search::KdTree<PointNormal>::Ptr segtree(new pcl::search::KdTree<PointNormal>);
segtree->setInputCloud(doncloud);
std::vector<pcl::PointIndices> cluster_indices; //动态数组保存聚类索引
pcl::EuclideanClusterExtraction<PointNormal> ec;
ec.setClusterTolerance(segRadius); //设置聚类半径
ec.setMinClusterSize(50);
ec.setMaxClusterSize(100000);
ec.setSearchMethod(segtree);
ec.setInputCloud(doncloud);
ec.extract(cluster_indices);
if (VISUAL = 1) //欧式聚类以后进行可视化
{
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_xyz(new pcl::PointCloud<pcl::PointXYZ>);
copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*doncloud, *tmp_xyz);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud = getColoredCloud(tmp_xyz, cluster_indices, 0, 255, 0);
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("visualize the clustering results"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgbps(colored_cloud);
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(colored_cloud, rgbps);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);
MView->spin();
}
if (SAVE == 1) //保存欧式聚类的结果
{
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();it != cluster_indices.end(); ++it, j++)
{
pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don(new pcl::PointCloud<PointNormal>);
for (std::vector<int>::const_iterator pit = it->indices.begin();pit != it->indices.end();++pit)
{
cloud_cluster_don->points.push_back(doncloud->points[*pit]);
}
cloud_cluster_don->width = int(cloud_cluster_don->points.size());
cloud_cluster_don->height = 1;
cloud_cluster_don->is_dense = true;
cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size() << " data points." << std::endl;
stringstream ss;
ss << "don_cluster_" << j << ".pcd";
writer.write<pcl::PointNormal>(ss.str(), *cloud_cluster_don, false);
}
}
}
三、编译结果:
1)曲率信息可视化结果:
2)利用曲率分类结果可视化
3)进一步欧式聚类分割结果