#include <pcl/io/pcd_io.h>
#include <pcl/search/kdtree.h>
#include <vector>
#include <pcl/features/normal_3d.h>
#include <Eigen/dense>
#include <cmath>
#include <ctime>
#include<pcl/visualization/pcl_visualizer.h>
using namespace pcl;
using namespace Eigen;
using namespace std;
int main(){
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
io::loadPCDFile("C:\\Users\\admin\\Desktop\\test.pcd", *cloud);
search::KdTree<PointXYZ>::Ptr tree(new search::KdTree<PointXYZ>);
PointCloud<Normal>::Ptr normal_(new PointCloud<Normal>);
NormalEstimation<PointXYZ, Normal>ne;
tree->setInputCloud(cloud);
ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
ne.setKSearch(10);
ne.compute(*normal_);
//邻域************************************************************************************
int point_num = cloud->points.size();
vector<int>nebor_index;
vector<float>nebor_dis;
int nebor_size(10);
vector<vector<int>>point_nebor;
point_nebor.resize(point_num, nebor_index);
for (int i_point = 0; i_point < point_num;i_point++)
{
tree->nearestKSearch(cloud->points[i_point], nebor_size, nebor_index, nebor_dis);
point_nebor[i_point].swap(nebor_index);
}
//曲率*************************************************************************************
vector<pair<float, int>>point_curvature_index;
for (int i_point = 0; i_point < point_num;i_point++)
{
point_curvature_index.push_back(make_pair(normal_->points[i_point].curvature, i_point));
}
sort(point_curvature_index.begin(), point_curvature_index.end());
//区域生长*******************************************************************************
float normal_threshold = cosf(10.0 / 180.0 * M_PI);
float curvature_threshold = 0.1;//法向量阈值,曲率阈值
int counter_(0),segment_label(0);
int seed_index_orginal = point_curvature_index[0].second;//初始种子点的索引
vector<int>point_label;
vector<int>seg_ave_num;
point_label.resize(point_num, -1);
while (counter_<point_num)
{
queue<int>seed;
seed.push(seed_index_orginal);
int counter_1(1);
point_label[seed_index_orginal] = segment_label;
while (!seed.empty())
{
int curr_seed = seed.front();
seed.pop();
int K_nebor(0);
while (K_nebor<nebor_size)
{
int index_nebor = point_nebor[curr_seed][K_nebor];
if (point_label[index_nebor]!=-1)
{
K_nebor++;
continue;
}
bool is_a_seed = false;
Map<Vector3f>curr_seed(static_cast<float*>(normal_->points[curr_seed].normal));
Map<Vector3f>seed_nebor(static_cast<float*>(normal_->points[index_nebor].normal));
float dot_normal = fabsf(curr_seed.dot(seed_nebor));
if (dot_normal<normal_threshold)
{
is_a_seed = false;
}
else if (normal_->points[index_nebor].curvature>curvature_threshold)
{
is_a_seed = false;
}
else
{
is_a_seed = true;
}
if (!is_a_seed)
{
K_nebor++;
continue;
}
point_label[index_nebor] = segment_label;
counter_1++;
if (is_a_seed)
{
seed.push(index_nebor);
}
K_nebor++;
}
}
segment_label++;
counter_ += counter_1;
seg_ave_num.push_back(counter_1);
for (int i_seg = 0; i_seg < point_num;i_seg++)
{
int index_ = point_curvature_index[i_seg].second;
if (point_label[index_]==-1)
{
seed_index_orginal = index_;
break;
}
}
}
//分割结果汇总***************************************************
vector<PointIndices>cluster_;
PointIndices segments;
int seg_num = seg_ave_num.size();
cluster_.resize(seg_num, segments);
for (int i_seg = 0; i_seg < seg_num;i_seg++){
cluster_[i_seg].indices.resize(seg_ave_num[i_seg], 0);
}
vector<int> counter;
counter.resize(seg_num, 0);
for (int i_point = 0; i_point < point_num; i_point++)
{
int segment_index = point_label[i_point];
int nebor_idx = counter[segment_index];
cluster_[segment_index].indices[nebor_idx] = i_point;
counter[segment_index] += 1;
}
//剔除一定数量的分割单元*****************************************
vector<PointIndices>clusters;
int min_number = 100, max_number = 1000000;
for (int i_seg = 0; i_seg < seg_num;i_seg++)
{
if (cluster_[i_seg].indices.size()>min_number&&cluster_[i_seg].indices.size() < max_number)
{
clusters.push_back(cluster_[i_seg]);
}
}
//可视化*********************************************************
visualization::PCLVisualizer::Ptr viewer(new visualization::PCLVisualizer("view"));
PointCloud<PointXYZRGB>::Ptr color_point(new PointCloud<PointXYZRGB>);
srand(time(nullptr));
vector<unsigned char>color;
for (int i_segment = 0; i_segment < clusters.size(); i_segment++)
{
color.push_back(static_cast<unsigned char>(rand() % 256));
color.push_back(static_cast<unsigned char>(rand() % 256));
color.push_back(static_cast<unsigned char>(rand() % 256));
}
int color_index = 0;
for (int i_seg = 0; i_seg < clusters.size(); i_seg++)
{
int clusters_size = clusters[i_seg].indices.size();
for (int i_idx = 0; i_idx < clusters_size;i_idx++)
{
PointXYZRGB point;
point.x = cloud->points[clusters[i_seg].indices[i_idx]].x;
point.y = cloud->points[clusters[i_seg].indices[i_idx]].y;
point.z = cloud->points[clusters[i_seg].indices[i_idx]].z;
point.r = color[3 * color_index];
point.g = color[3 * color_index+1];
point.b = color[3 * color_index+2];
color_point->push_back(point);
}
color_index++;
}
viewer->addPointCloud(color_point);
viewer->spin();
return 0;
}
PCL基于法向量和曲率的区域生长算法(源码):
最新推荐文章于 2024-05-24 21:10:29 发布