基于线特征的点云分割实现
野外环境下地形较为复杂，大体可粗略分为地平面、坡面和障碍物。对于激光雷达的原始点云，通过分割算法将地面、坡面、障碍物有效分离，对接下来的感知、规划与决策过程非常重要。考虑一种简单易于实现的传统思路来分割点云：基于线特征的点云分割技术，其包含横向与纵向的判断，可实现三种地形的有效分割。
实验环境
速腾聚创16线激光雷达（rslidar-16）ubuntu16.04 ros-kinetic C++
点云预处理
rslidar-16的驱动包发出的topic是rslidar_points，不包含线束信息ring，因此，在预处理环节需要给每一个points赋予相应的线束信息。16线的rslidar的16跟线的俯仰向角度依次为-15，-13，…，-1，1，…，13，15，依次对应1-8，16-9条线，但为了方便，我们按照1-16的ring id来赋予-15~15角度的16根线。计算ring id则是根据point.x point.y和point.z计算夹角。
void slopeSegmentation::selectThread(const PointCloud& cloud,const size_t start_index,const size_t end_index){
const double angle1 = params_.theta1*M_PI/180;
const double angle2 = params_.theta2*M_PI/180;
const double r_min = sqrt(params_.r_min_square);//最小的半径
const double r_max = sqrt(params_.r_max_square);//最大的半径
const int line_index=0;
//std::vector <int> line;
//line_.index.clear();
//std::cout << "cloudsize" << cloud.size() << std::endl;
//line.ringID
//这块计算ring得大改
//修改
int rowID;
float pitch;
for(unsigned int i=start_index;i<end_index;++i)
{
pcl::PointXYZ point(cloud[i]);
partCloud.push_back(point);
pitch = std::atan2(point.z,sqrt(point.x*point.x+point.y*point.y))*180/M_PI;
//对于rs_16 划分16条点云
rowID=round((pitch+ang_bottom)/ang_res_y);
if(rowID>=0&&rowID<N_scn)
{
lines_[rowID].index.push_back(i);
//test
/*if(rowID==7)
{
int size=lines_[rowID].index.size();
std::cout << "pitch:" << pitch << " index in cloud:" << lines_[rowID].index[size-1] << std::endl;
}*/
}
if(rowID<0||rowID>=N_scn)
continue;
}
//
/*
for(unsigned int i=start_index;i<end_index;++i)
{
pcl::PointXYZ point(cloud[i]);
const double range_square = point.x * point.x + point.y * point.y;
//const double range = sqrt(range_square);//每个点云距中心点的距离
if (range_square < params_.r_max_square && range_square > params_.r_min_square) {//若该点云在有效范围内
const double angle = std::atan2(point.y, point.x);//偏移角度
//if(angle>angle1&&angle<angle2)
//{
partCloud.push_back(point);
pc_index_.push_back(i);//在原始点云中的索引
line_.index.push_back(i);
if(pc_index_.size()>1)
{
int a = pc_index_.size();
//判断两个点俯仰角是否相同 若不同 则不是同一线束
//if(pc_index_[a-1]-pc_index_[a-2]>200)
//std::cout << fabs(std::atan2(cloud[pc_index_[a-1]].z,cloud[pc_index_[a-1]].x)-std::atan2(cloud[pc_index_[a-2]].z,cloud[pc_index_[a-2]].x)) << std::endl;
if(fabs(angle)<0.6&&std::atan2(cloud[pc_index_[a-1]].z,sqrt(cloud[pc_index_[a-1]].x*cloud[pc_index_[a-1]].x+cloud[pc_index_[a-1]].y*cloud[pc_index_[a-1]].y))-std::atan2(cloud[pc_index_[a-2]].z,sqrt(cloud[pc_index_[a-2]].x*cloud[pc_index_[a-2]].x+cloud[pc_index_[a-2]].y*cloud[pc_index_[a-2]].y))>0.02)
{
lines_.push_back(line_);
std::cout << "line_.index.size:" << line_.index.size() << std::endl;
//std::cout << "pc_index_.size:" << pc_index_.size() << std::endl;
line_.index.clear();
}
}
//}
}
else;//若不在有效范围 则该点无效
}
std::cout << "lines_.size():" << lines_.size() << endl;
*/
}
相同ring分段以及横向判断
1.将具有相同ring的点云作为一类，划分不同的线段，划分的依据为相邻的point是否在欧氏空间距离大于阈值。
2.横向判断：得到每一个线段的笛卡尔坐标系下坐标均值，根据其z方向均值以及线段内点的数目来判断地面与非地面。
void slopeSegmentation::getLineSeg(const PointCloud& cloud){
const int linesNum = N_scn;
std::pair<int,int> start_end;
segment seg_;
pcl::PointXYZ point_;
double x_square,y_square,z_square;
int segLength;
for(int i=0;i<linesNum;i++){
start_end.first=lines_[i].index[0];
const int lineSize = lines_[i].index.size();//每一条线束lines[i].line_segs.push_back()
/*int num1=0;
int num2=0;
int num3=0;
int num4=0;*/
point_.x=0.0;
point_.y=0.0;
point_.z=0.0;
for(int j=1;j<lineSize;j++)
{
point_.x+=cloud[lines_[i].index[j-1]].x;
point_.y+=cloud[lines_[i].index[j-1]].y;
point_.z+=cloud[lines_[i].index[j-1]].z;
if(fabs(cloud[lines_[i].index[j]].z-cloud[lines_[i].index[j-1]].z)<params_.Th)//绝对高度差小于Th
{
x_square=pow(fabs(cloud[lines_[i].index[j]].x-cloud[lines_[i].index[j-1]].x),2);
y_square=pow(fabs(cloud[lines_[i].index[j]].y-cloud[lines_[i].index[j-1]].y),2);
//z_square=pow(fabs(cloud[lines_[i].index[j]].z-cloud[lines_[i].index[j-1]].z),2);
//if(x_square+y_square+z_square>params_.Tr_square)//欧式距离大于Tr 认为是线段的分割点
if(sqrt(x_square+y_square)>params_.Tr)//欧式距离大于Tr 认为是线段的分割点
{
start_end.second=lines_[i].index[j-1];
seg_.startEndIndex=start_end;
segLength=start_end.second-start_end.first;
if(segLength!=0)
{
point_.x=(point_.x+cloud[lines_[i].index[j]].x)/(segLength+1);
point_.y=(point_.y+cloud[lines_[i].index[j]].y)/(segLength+1);
point_.z=(point_.z+cloud[lines_[i].index[j]].z)/(segLength+1);
}
else;
seg_.point=point_;
lines_[i].line_segs_.push_back(seg_);
//if(start_end.first==start_end.second)
//num3++;
start_end.first=lines_[i].index[j];
//num1++;
point_.x=0.0;
point_.y=0.0;
point_.z=0.0;
}
else;
}
else//绝对高度差大于Th
{
start_end.second=lines_[i].index[j-1];
seg_.startEndIndex=start_end;
segLength=start_end.second-start_end.first;
if(segLength!=0)
{
point_.x=point_.x/(segLength+1);
point_.y=point_.y/(segLength+1);
point_.z=point_.z/(segLength+1);
}
else;
seg_.point=point_;
lines_[i].line_segs_.push_back(seg_);
//if(start_end.first==start_end.second)
//num4++;
start_end.first=lines_[i].index[j];
//num2++;
point_.x=0.0;
point_.y=0.0;
point_.z=0.0;
}
}
//last lineseg
start_end.second=lines_[i].index[lineSize-1];
seg_.startEndIndex=start_end;
segLength=start_end.second-start_end.first;
point_.x=0.0;
point_.y=0.0;
point_.z=0.0;
for(unsigned int a=start_end.first;a<start_end.second;a++)
{
point_.x+=cloud[a].x;
point_.y+=cloud[a].y;
point_.z+=cloud[a].z;
}
if(segLength>1)
{
point_.x=point_.x/(segLength-1);
point_.y=point_.y/(segLength-1);
point_.z=point_.z/(segLength-1);
}
seg_.point=point_;
lines_[i].line_segs_.push_back(seg_);
//std::cout << "num1:" << num1 << " num2:" << num2 << " num3:" << num3 << " num4:" << num4 << std::endl;
//std::cout << "lines_[" << i << "].line_segs_.size: " << lines_[i].line_segs_.size() << std::endl;
}
}
//得到部分坡面和全部地面 该函数处理后 障碍中包含部分坡面 需进一步处理
void slopeSegmentation::rlDecision(const PointCloud& cloud){
//double min_ground=params_.sensor_height*(-1)-0.1;
double max_ground=params_.sensor_height*(-1)+0.1;
double max_slope=params_.sensor_height*(-1)+1;
//寻找最低地平面
double min_ground=lines_[0].line_segs_[0].point.z;
if(lines_[0].line_segs_.size()>1){
for(unsigned int i=1;i<lines_[0].line_segs_.size();i++)
{
if(min_ground>lines_[0].line_segs_[i].point.z)
min_ground=lines_[0].line_segs_[i].point.z;
}
}
//每一条line左右方向判断
float xoyLength,xoyLength1;
float zHeight;
float zError;
for(unsigned int i=0;i<N_scn;i++){
int num3=0;
if(lines_[i].line_segs_.size()==1)//如果一个ring里只有一个段，那么超过一定高度，则认为是障碍物或坡面，极少见
{
if(lines_[i].line_segs_[0].point.z>-0.5&&lines_[i].line_segs_[0].point.z<-0.4)
lines_[i].line_segs_[0].flag=1;
else if(lines_[i].line_segs_[0].point.z>=-0.7&&lines_[i].line_segs_[0].point.z<=-0.5)
lines_[i].line_segs_[0].flag=0;
else;
}
else if(lines_[i].line_segs_.size()>=2){
int size_segs=lines_[i].line_segs_.size();
for(unsigned int j=0;j<size_segs-1;j++)//对ring里面相邻两个seg判断：1）高度差 2）与雷达中心点xoy投影距离
{
num3++;
//xoyLength1=sqrt(lines_[i].line_segs_[j+1].point.x*lines_[i].line_segs_[j+1].point.x+lines_[i].line_segs_[j+1].point.y*lines_[i].line_segs_[j+1].point.y);
xoyLength=sqrt(lines_[i].line_segs_[j].point.x*lines_[i].line_segs_[j].point.x+lines_[i].line_segs_[j].point.y*lines_[i].line_segs_[j].point.y);
zHeight=lines_[i].line_segs_[j].point.z;
//test
/*
if(i==0||i==1||i==2||i==3||i==4||i==5)
{
std::cout << "i:" << i << " point.x:" << lines_[i].line_segs_[j].point.x << " point.y:" << lines_[i].line_segs_[j].point.y << " point.z:" << lines_[i].line_segs_[j].point.z << std::endl;
std::cout << "xoyLength:" << xoyLength << " min_ground:" << min_ground << " zHeight:" << zHeight << std::endl;
}
*/
//计算zError 即每个线段的点在z方向与平均值的偏差绝对值之和
zError=0.0;
for(unsigned k=lines_[i].line_segs_[j].startEndIndex.first;k<lines_[i].line_segs_[j].startEndIndex.second;k++)
{
zError+=fabs(cloud[k].z-lines_[i].line_segs_[j].point.z);
}
//if(fabs(zHeight-min_ground)<params_.Theight||zHeight<=(min_ground-params_.Theight))//地面
if(fabs(zHeight-min_ground)<params_.Theight)//得到全部地面
{
lines_[i].line_segs_[j].flag=0;
}
//坡面或障碍
if(fabs(zHeight-min_ground)<params_.Theight*3&&fabs(zHeight-min_ground)>=params_.Theight)
{
if(lines_[i].line_segs_[j].startEndIndex.second-lines_[i].line_segs_[j].startEndIndex.first>40)//得到部分坡面
lines_[i].line_segs_[j].flag=1;
//else if(lines_[i].line_segs_[j].startEndIndex.second-lines_[i].line_segs_[j].startEndIndex.first>20&&zError>)
}
//zError=zError/(lines_[i].line_segs_[j].startEndIndex.second-lines_[i].line_segs_[j].startEndIndex.first+1);
//std::cout << "zError:" << zError << std::endl;
}
zHeight=lines_[i].line_segs_[size_segs-1].point.z;
//std::cout << "here point.z:" << zHeight << std::endl;
//得到全部地面
if(fabs(zHeight-min_ground)<params_.Theight)
{
//std::cout << "here" << std::endl;
lines_[i].line_segs_[size_segs-1].flag=0;
continue;
}
zError=0.0;
for(unsigned int k=lines_[i].line_segs_[size_segs-1].startEndIndex.first;k<lines_[i].line_segs_[size_segs-1].startEndIndex.second;k++)
{
zError+=fabs(cloud[k].z-lines_[i].line_segs_[size_segs-1].point.z);
}
if(fabs(zHeight-min_ground)<params_.Theight*3&&fabs(zHeight-min_ground)>=params_.Theight)
{
if(lines_[i].line_segs_[size_segs-1].startEndIndex.second-lines_[i].line_segs_[size_segs-1].startEndIndex.first>40)//得到部分坡面
lines_[i].line_segs_[size_segs-1].flag=1;
}
}
//else
//std::cout << "lines_[i].line_segs_.size() " << lines_[i].line_segs_.size() << std::endl;
//std::cout << "num3:" << num3 << std::endl;
}
}
纵向判断
1.划分扇形区域，将扫描空间划分为12个扇形区域，每个区域30°，计算每个扇形内有多少线段。
2.对同一扇形内不同ring的线段进行纵向比较，计算斜度，若斜度大于阈值，则认为是障碍，否则是坡面
3.多条件约束，判断是坡面还是障碍还根据线段内point的数目、与激光雷达的距离等。
void slopeSegmentation::fbDecision(const PointCloud& cloud){
double disError;
double pointZ;
/*
for(int i=0;i<N_scn/2;i++)//只判断水平线以下1-8根线
{
for(int j=0;j<lines_[i].line_segs_.size();j++)
{
if(lines_[i].line_segs_[j].flag==2)//疑似障碍 包含部分坡面
{
pointZ=lines_[i].line_segs_[j].point.z;
disError=(params_.sensor_height-pointZ)/tan(fabs(laser_angle[i]));
std::cout << "disError:" << disError << std::endl;
if(disError<0.5)//距离误差的阈值
{
lines_[i].line_segs_[j].flag=3;
continue;
}
}
}
}
*/
//划分扇形 对每个扇形先比较同一个ring的线段 是slope则置1 再比较flag为2且不同ring的线段
float pointAng,pointAngTemp;
int Index,IndexTemp;
int nnn=0;
for(int i=0;i<N_scn;i++)
{
for(int j=0;j<lines_[i].line_segs_.size();j++)
{
if(lines_[i].line_segs_[j].flag==2)//如果是疑似扇形 Index算的不对
{
nnn++;
int line_size=lines_[i].line_segs_[j].startEndIndex.second-lines_[i].line_segs_[j].startEndIndex.first;
for(int k=lines_[i].line_segs_[j].startEndIndex.first;k<lines_[i].line_segs_[j].startEndIndex.second;k++)
{
if(cloud[k].x!=cloud[k].x||cloud[k].y!=cloud[k].y||cloud[k].z!=cloud[k].z)
continue;
//std::cout << "11111" << std::endl;
pointAng = atan2(cloud[k].y,cloud[k].x)*180/M_PI;
Index = pointAng>=0?floor(pointAng/30):floor((pointAng+360)/30);//向下取整
secLine_.line_index.push_back(k);
//std::cout << "Index" << Index << std::endl;
if(line_size==1){
//std::cout << "22222" << std::endl;
secLine_.ring_id=i;
sectors_[Index].sec_lines_.push_back(secLine_);
secLine_.line_index.clear();
continue;
}
if(k==lines_[i].line_segs_[j].startEndIndex.second-1)
{
//std::cout << "33333" << std::endl;
secLine_.ring_id=i;
sectors_[Index].sec_lines_.push_back(secLine_);
secLine_.line_index.clear();
continue;
}
pointAngTemp = atan2(cloud[k+1].y,cloud[k+1].x)*180/M_PI;
IndexTemp = pointAngTemp>=0?floor(pointAngTemp/30):floor((pointAngTemp+360)/30);//向下取整
//std::cout << "IndexTemp" << IndexTemp << std::endl;
//std::cout << pointAng/30 << std::endl;
//std::cout << pointAngTemp/30 << std::endl;
if(abs(Index-IndexTemp)!=0)
{
//std::cout << "44444" << std::endl;
secLine_.ring_id=i;
sectors_[Index].sec_lines_.push_back(secLine_);
secLine_.line_index.clear();
continue;
}
}
}
}
}
/*for(int i=0;i<12;i++)
{
for(int j=0;j<sectors_[i].sec_lines_.size();j++)
std::cout << "i:" << i << " ring:" << sectors_[i].sec_lines_[j].ring_id << " index.size:" << sectors_[i].sec_lines_[j].line_index.size() << std::endl;
}*/
}
//针对每个扇形进行纵向判断
void slopeSegmentation::ptDecision(const PointCloud& cloud)
{
//计算扇形内每条线段均值
pcl::PointXYZ point_temp;
int pIndex;
for(unsigned int i=0;i<12;i++)
{
for(unsigned int j=0;j<sectors_[i].sec_lines_.size();j++)
{
point_temp.x=0.0;
point_temp.y=0.0;
point_temp.z=0.0;
for(int k=0;k<sectors_[i].sec_lines_[j].line_index.size();k++)
{
pIndex=sectors_[i].sec_lines_[j].line_index[k];
point_temp.x+=cloud[pIndex].x;
point_temp.y+=cloud[pIndex].y;
point_temp.z+=cloud[pIndex].z;
}
point_temp.x=point_temp.x/sectors_[i].sec_lines_[j].line_index.size();
point_temp.y=point_temp.y/sectors_[i].sec_lines_[j].line_index.size();
point_temp.z=point_temp.z/sectors_[i].sec_lines_[j].line_index.size();
sectors_[i].sec_lines_[j].point=point_temp;
}
}
//纵向比较每个line
float z_error;
float xy_error,xy_dist1,xy_dist2;
for(unsigned int i=0;i<12;i++)
{
for(unsigned int j=0;j<sectors_[i].sec_lines_.size();j++)
{
std::cout << "ring:" << sectors_[i].sec_lines_[j].ring_id << " point.z:" << sectors_[i].sec_lines_[j].point.z << " size:" << sectors_[i].sec_lines_[j].line_index.size() << " point.x:" << sectors_[i].sec_lines_[j].point.x << " point.y:" << sectors_[i].sec_lines_[j].point.y << std::endl;
if(sectors_[i].sec_lines_[j].line_index.size()<5)//认为是离散的障碍点
{
sectors_[i].sec_lines_[j].flag=2; //障碍
continue;
}
//对于line_index.size()>4的line 寻找相邻的ring 进行纵向判断 每一个都应该和上面一个ring比较
int neborID=NeborLine(i,j,sectors_[i].sec_lines_[j].ring_id);
std::cout << "neborID:" << neborID << std::endl;
//如果neborID=0说明没有邻近的line 则针对该line单独判断
if(neborID==0&§ors_[i].sec_lines_[j].flag==3)//未判断且无上邻线段
{
/*
//待添加
*/
sectors_[i].sec_lines_[j].flag=2;
continue;
}
//如果neborID!=0说明该线段有上邻线段 可进行纵向判断 若该线段还未进行判断
if(neborID!=0&§ors_[i].sec_lines_[j].flag==3)
{
z_error=fabs(sectors_[i].sec_lines_[j].point.z-sectors_[i].sec_lines_[neborID].point.z);
xy_dist1=sqrt(sectors_[i].sec_lines_[j].point.x*sectors_[i].sec_lines_[j].point.x+sectors_[i].sec_lines_[j].point.y*sectors_[i].sec_lines_[j].point.y);
xy_dist2=sqrt(sectors_[i].sec_lines_[neborID].point.x*sectors_[i].sec_lines_[neborID].point.x+sectors_[i].sec_lines_[neborID].point.y*sectors_[i].sec_lines_[neborID].point.y);
xy_error=xy_dist2-xy_dist1;
/*
std::cout << "ring1:" << sectors_[i].sec_lines_[j].ring_id << " ring2:" << sectors_[i].sec_lines_[neborID].ring_id << std::endl;
std::cout << "pointz:" << sectors_[i].sec_lines_[j].point.z << " " << sectors_[i].sec_lines_[neborID].point.z << std::endl;
std::cout << "xy_dist:" << xy_dist1 << " " << xy_dist2 << std::endl;
std::cout << "zerror:" << z_error << " xy_error" << xy_error << " theta:" << atan2(z_error,xy_error)*180/M_PI << std::endl;
*/
if(z_error<0.02)
{
sectors_[i].sec_lines_[j].flag=1;
sectors_[i].sec_lines_[neborID].flag=1;
continue;
}
if(atan2(z_error,fabs(xy_error))*180/M_PI>40||xy_error<-0.05)
//if(atan2(z_error,xy_error)*180/M_PI>40||(params_.sensor_height/tan(laser_angle[sectors_[i].sec_lines_[j].ring_id])-xy_dist1>1.0))//角度
{
std::cout << "angle:" << atan2(z_error,xy_error)*180/M_PI << std::endl;
sectors_[i].sec_lines_[j].flag=2;
sectors_[i].sec_lines_[neborID].flag=2;
continue;
}
if(sectors_[i].sec_lines_[j].ring_id>=8&§ors_[i].sec_lines_[j].line_index.size()<20&&xy_dist1<3.0)
{
sectors_[i].sec_lines_[j].flag=2;
continue;
}
sectors_[i].sec_lines_[j].flag=1;
sectors_[i].sec_lines_[neborID].flag=1;
}
}
}
}
int slopeSegmentation::NeborLine(int i,int j,int ringid)
{
int minIndex;
std::vector<int> candidate;
for(int k=0;k<sectors_[i].sec_lines_.size();k++)
{
//std::cout << sectors_[i].sec_lines_[k].ring_id << " ";
if(sectors_[i].sec_lines_[k].ring_id-ringid==1&§ors_[i].sec_lines_[k].line_index.size()>4)//上一个ring候选
candidate.push_back(k);
}
//std::cout << std::endl;
std::cout << "candidate.size()" << candidate.size() << std::endl;
if(candidate.size()==0)
return 0;
if(candidate.size()==1)
return candidate[0];
float xoyDist = sqrt((sectors_[i].sec_lines_[j].point.x-sectors_[i].sec_lines_[candidate[0]].point.x)*(sectors_[i].sec_lines_[j].point.x-sectors_[i].sec_lines_[candidate[0]].point.x)+(sectors_[i].sec_lines_[j].point.y-sectors_[i].sec_lines_[candidate[0]].point.y)*(sectors_[i].sec_lines_[j].point.y-sectors_[i].sec_lines_[candidate[0]].point.y));
minIndex=candidate[0];
float xoyDistTemp;
for(int a=1;a<candidate.size();a++)
{
xoyDistTemp = sqrt((sectors_[i].sec_lines_[j].point.x-sectors_[i].sec_lines_[candidate[a]].point.x)*(sectors_[i].sec_lines_[j].point.x-sectors_[i].sec_lines_[candidate[a]].point.x)+(sectors_[i].sec_lines_[j].point.y-sectors_[i].sec_lines_[candidate[a]].point.y)*(sectors_[i].sec_lines_[j].point.y-sectors_[i].sec_lines_[candidate[a]].point.y));
if(xoyDistTemp<xoyDist)
{
xoyDist = xoyDistTemp;
minIndex = candidate[a];
}
std::cout << ringid << " " << sectors_[i].sec_lines_[candidate[a]].ring_id << " " << candidate[a] << " " << sectors_[i].sec_lines_[candidate[a]].point.z << std::endl;
}
std::cout << ringid << " " << sectors_[i].sec_lines_[candidate[0]].ring_id << " " << candidate[0] << " " << sectors_[i].sec_lines_[candidate[0]].point.z << std::endl;
std::cout << sectors_[i].sec_lines_[j].point.z << std::endl;
return minIndex;
}
实验结果
地面
2698
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
