使⽤PCL提取平⾯的⽅式本⽂主要总结了⼏种提取平⾯的⽅式，算法的原理就不细说了.
1.RANSAC
循环提取平⾯，直到⼩于20个点.
void groundRemoval(pcl::PointCloud<pcl::PointXYZ>::Ptr in,int& pl_num)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr in_totle(new pcl::PointCloud<pcl::PointXYZ>);
*in_totle = *in;
int totle_in_size = in_totle->points.size();
pl_num = 0;
std::vector<std::vector<double> > pl_nor_tot;
while (in_totle->points.size() > 20)
{
释义是什么意思pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr outgr_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr ingr_cloud(new pcl::PointCloud<pcl::PointXYZ>);
doRansacPlane(in_totle, inliers, coefficients,0.015);//plane
ingr_cloud = filterCloud(in_totle, inliers, fal);//plane points
outgr_cloud = filterCloud(in_totle, inliers, true);
*in_totle = *outgr_cloud;
if(ingr_cloud->points.size() < 200)
{
/
/cout<<"ingr_cloud size is small,continue"<<endl;
continue;
}
//attribute:normals not ud
double A = coefficients->values[0];
double B = coefficients->values[1];
double C = coefficients->values[2];
double D = coefficients->values[3];
double Aunit = A / sqrt(A*A+B*B+C*C);
double Bunit = B / sqrt(A*A+B*B+C*C);
double Cunit = C / sqrt(A*A+B*B+C*C);
std::vector<double> pl_nor{Aunit,Bunit,Cunit};
pl_nor_tot.push_back(pl_nor);
//attribute:distance
double ydistance = 0;
//double ydistance_max = 0;
double zdistance = 0;
int ingr_num = ingr_cloud->points.size();
for(int i = 0 ;i < ingr_num;i++)中老年钙片
{
if(dir_choice == 0)
{
ydistance += (ingr_cloud->points[i].x);
}
el if(dir_choice == 1)
{
ydistance += (ingr_cloud->points[i].y);
zdistance += (ingr_cloud->points[i].z);
cad卸载
}
// if((ingr_cloud->points[i].y) > ydistance_max)
// {
// ydistance_max = (ingr_cloud->points[i].y) ;
// }
}
double ydistance_ave = ydistance / double(ingr_num);
double zdistance_ave = zdistance / double(ingr_num);
cout<<"-----plane:"<<pl_num+1<<"-----"<<endl;
cout<<"distance_ave=="<<ydistance_ave<<endl;
//planeout[pl_num].distance = distance_ave;五月天壁纸
planeout[pl_num].ydistance = ydistance_ave;
planeout[pl_num].zdistance = zdistance_ave;
//attribute:rho,not ud
double rho;
//rho = computeCloudResolution (ingr_cloud);
rho = computeYDirResolution(ingr_cloud);
planeout[pl_num].rho = rho;
cout<<"rho=="<<rho<<endl;
//attribute:cloud
*planeout[pl_num].PlaneCloud = *ingr_cloud;
++pl_num;
}
}
//结构体
struct PlaneSort
{
double ydistance;
double zdistance;
double k;
double rho;
pcl::PointCloud<pcl::PointXYZ>::Ptr PlaneCloud;
};
//ransac提取平⾯龟兔赛跑英语
void doRansacPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
pcl::PointIndices::Ptr inliers,
pcl::ModelCoefficients::Ptr coefficients,
double distance_threshold)
{
int max_iterations = 500;
pcl::SACSegmentation<pcl::PointXYZ> g;
g.tOptimizeCoefficients(true);
g.tModelType(pcl::SACMODEL_PLANE);
g.tMethodType(pcl::SAC_RANSAC);
g.tDistanceThreshold(distance_threshold);
g.tMaxIterations(max_iterations);
g.tInputCloud(cloud);
<(*inliers, *coefficients);
保证书写给老婆
}
/
/根据索引提取所需点云
pcl::PointCloud<pcl::PointXYZ>::Ptr filterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointIndices::Ptr indices,
bool negative)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); pcl::ExtractIndices<pcl::PointXYZ> extract;
梦幻之旅
extract.tInputCloud(cloud);
extract.tIndices(indices);
extract.tNegative(negative);
extract.filter(*cloud_filtered);
return cloud_filtered;卡通杯子图片
}
黄⾊为提取到的平⾯，红⾊为马路⽛⼦线.
2.区域增长法
void GroundRemoval(pcl::PointCloud<pcl::PointXYZ>::Ptr in)
{
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::arch::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::arch::KdTree<pcl::PointXYZ>());
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne;
pcl::PointCloud<pcl::PointXYZ>::Ptr outgr_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PointCloud<pcl::PointXYZ>::Ptr ingr_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ne.tInputCloud (in);
ne.tSearchMethod (tree);
ne.tKSearch (15);
ne.tViewPoint(std::numeric_limits<double>::max (),std::numeric_limits<double>::max (), std::numeric_limits<double>::max ()); ne.compute (*cloud_normals);
//cout<<"cloud_normals size is "<< cloud_normals->size()<<endl;
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.tMinClusterSize (100);
reg.tMaxClusterSize (10000);
reg.tSearchMethod (tree);
reg.tNumberOfNeighbours (30); //ini =30
reg.tInputCloud (in);
//reg.tIndices (indices);
reg.tInputNormals (cloud_normals);
reg.tSmoothnessThreshold (4.0 / 180.0 * M_PI); //ini =3.
reg.tCurvatureThreshold (1.0);
std::vector <pcl::PointIndices> clusters;
/
/ cout << "Number of clusters is equal to " << clusters.size () << endl;
// cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
for(int i = 0 ;i < clusters.size();i++)
{
double distance = 0;
double distance_min = 100;
pcl::PointCloud<pcl::PointXYZ>::Ptr ingr_cloud(new pcl::PointCloud<pcl::PointXYZ>);
for(int m = 0;m < clusters[i].indices.size ();m++)
for(int m = 0;m < clusters[i].indices.size ();m++)
{
ingr_cloud->push_back(in->points[clusters[i].indices[m]]);
}
int ingr_num = ingr_cloud->points.size();
//cout<<"ingr_num=="<<ingr_num<<endl;
for(int j = 0 ;j < ingr_num;j++)
{
distance += fabs(ingr_cloud->points[j].z);//todo y
if(fabs(ingr_cloud->points[j].z) < distance_min)
{
distance_min = fabs(ingr_cloud->points[j].z);
}
}
double distance_ave = distance / double(ingr_num);
planeout[i].distance = distance_min;
*planeout[i].PlaneCloud = *ingr_cloud;
// cout<<"distance_min=="<<distance_min<<endl;
// cout<<"distance_ave=="<<distance_ave<<endl;
}
// visual
if(plane_visual == 1)
{
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = ColoredCloud (); pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
while (!viewer.wasStopped ())
{
}
}
3.Normals + RANSAC
4.DoN + RANSAC
ps：后续两种因时间问题，暂时先不整理代码上传了.
