DON点云滤波
DoN算法应该算是一种比较先进的点云滤波算法。分割本质上还是由欧式分割算法完成的。算法的目的是在去除点云低频滤波,低频信息(例如建筑物墙面,地面)往往会对分割产生干扰,高频信息(例如建筑物窗框,路面障碍锥)往往尺度上很小,直接采用 基于临近信息 的滤波器会将此类信息合并至墙面或路面中。所以DoN算法利用了多尺度空间的思想,算法如下:
(1) 在小尺度上计算点云法线1
(2)在大尺度上计算点云法线2
(3)法线1-法线2
(4) 滤去3中值较小的点
(5) 欧式分割
显然,在小尺度上是可以对高频信息进行检测的,此算法可以很好的小尺度高频信息。其在大规模点云(如LiDar点云)中优势尤其明显。
实现算法:
// Create a search tree, use KDTreee for non-organized data.
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));
}
// Set the input pointcloud for the search tree
tree->setInputCloud (cloud);
//生成法线估计器(OMP是并行计算,忽略)
pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;
ne.setInputCloud (cloud);
ne.setSearchMethod (tree);
//设定法线方向(要做差,同向很重要)
ne.setViewPoint (std::numeric_limits<float>::max (), std::numeric_limits<float>::max (), std::numeric_limits<float>::max ());
//计算小尺度法线
pcl::PointCloud<PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch (scale2);
ne.compute (*normals_large_scale);
//计算大尺度法线
pcl::PointCloud<PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch (scale2);
ne.compute (*normals_large_scale);
//生成DoN分割器
pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
don.setInputCloud (cloud);
don.setNormalScaleLarge (normals_large_scale);
don.setNormalScaleSmall (normals_small_scale);
//计算法线差
PointCloud<PointNormal>::Ptr doncloud (new pcl::PointCloud<PointNormal>);
copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);
don.computeFeature (*doncloud);
//生成滤波条件:把法线差和阈值比
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 (range_cond);
condrem.setInputCloud (doncloud);
//导出滤波结果
pcl::PointCloud<PointNormal>::Ptr doncloud_filtered (new pcl::PointCloud<PointNormal>);
condrem.filter (*doncloud_filtered);
//欧式聚类~~~(略)
点云分割——DON
根据不同尺度下法向量特征的差异性,利用pcl::DifferenceOfNormalsEstimation实现点云分割,在处理有较大尺度变化的场景点云分割效果较好,利用不同支撑半径去估算同一点的两个单位法向量,单位法向量的差定义DoN特征。
DoN算法:
DoN特征源于观察到基于所给半径估计的表面法向量可以反映曲面的内在几何特征,因此这种分割算法是基于法线估计的,需要计算点云中某一点的法线估计。而通常在计算法线估计的时候都会用到邻域信息,很明显邻域大小的选取会影响法线估计的结果。而在DoN算法中,邻域选择的大小就被称为support radius(支持半径)。对点云中某一点选取不同的支持半径,即可以得到不同的法线估计,而法线之间的差异,就是是所说的法线差异。
在这里插入图片描述
代码例程:
(1)对于输入点云数据中的每一点,利用较大的支撑半径rl计算法向量;
(2)对于输入点云数据中的每一点,利用较大的支撑半径rs计算法向量;
(3)对于输入点云数据中的每一点,单位化每一点的法向量差异
(4)过滤所得的向量域(DoN特征向量),分割出目标尺寸对应的点云;
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
// for visualization
#include
#include
VTK_MODULE_INIT(vtkRenderingOpenGL);
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].g = colors[3 * next_color + 1];
colored_cloud->points[index].b = colors[3 * next_color + 2];
}
next_color++;
}
}
return (colored_cloud);
}
int
main(int argc, char *argv[])
{
int VISUAL = 1, SAVE = 0;//0 indicate shows nothing, 1 indicate shows very step output 2 only shows the final results
///The smallest scale to use in the DoN filter.
double scale1 = 5, mean_radius;
///The largest scale to use in the DoN filter.
double scale2 = 10;
///The minimum DoN magnitude to threshold by
double threshold = 0.3;
///segment scene into clusters with given distance tolerance using euclidean clustering
double segradius = 10;
pcl::PointCloud<PointXYZRGB>::Ptr cloud(new pcl::PointCloud<PointXYZRGB>);
pcl::io::loadPCDFile("region_growing_tutorial_.pcd", *cloud);
// Create a search tree, use KDTreee for non-organized data.
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));
}
// Set the input pointcloud for the search tree
tree->setInputCloud(cloud);
//caculate the mean radius of cloud and mutilply with corresponding input
{
int size_cloud = cloud->size();
int step = size_cloud / 10;
double total_distance = 0;
int i, j = 1;
for (i = 0; i < size_cloud; i += step, j++)
{
std::vector<int> pointIdxNKNSearch(2);
std::vector<float> pointNKNSquaredDistance(2);
tree->nearestKSearch(cloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance);
total_distance += pointNKNSquaredDistance[1] + pointNKNSquaredDistance[0];
}
mean_radius = sqrt((total_distance / j));
cout << "mean radius of cloud is: " << mean_radius << endl;
scale1 *= mean_radius;//5*mean_radius
scale2 *= mean_radius;//10*mean_radius
segradius *= mean_radius;
}
if (scale1 >= scale2)
{
cerr << "Error: Large scale must be > small scale!" << endl;
exit(EXIT_FAILURE);
}
// 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());
// calculate normals with the small scale
cout << "Calculating normals for scale1..." << scale1 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<PointNormal>);
ne.setNumberOfThreads(4);
ne.setRadiusSearch(scale1);
ne.compute(*normals_small_scale);
// calculate normals with the large scale
cout << "Calculating normals for scale2..." << scale2 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<PointNormal>);
ne.setNumberOfThreads(4);
ne.setRadiusSearch(scale2);
ne.compute(*normals_large_scale);
//visualize the normals
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);
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();
}
// 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);//对输入点集,计算每一个点的DON特征向量,并输出
//输出一些不同的曲率
{
cout << "You may have some sense about the input threshold(curvature) next time for your data" << endl;
int size_cloud = doncloud->size();
cout << size_cloud << endl;
int step = size_cloud / 10;
for (int i = 0; i < size_cloud; i += step)cout << " " << doncloud->points[i].curvature << " " << endl;
}
//show the differences of curvature with both large and small 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 the difference of curvature of two scale"));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");//PointCloudColorHandlerGenericField方法是将点云按深度值(“x”、“y”、"z"均可)的差异着以不同的颜色。
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();
}
// Save DoN features
pcl::PCDWriter writer;
if (SAVE == 1) 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))
);//添加比较条件
// 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);
//show the results of keeping relative small curvature points
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();
}
// Filter by magnitude
cout << "Clustering using EuclideanClusterExtraction 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)
{//visualize the clustering results
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;
//Save cluster
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);
}
}
}