2. 程式人生 > 其它 >基於線特徵的點雲分割實現

基於線特徵的點雲分割實現

阿新 • • 發佈：2020-12-22

技術標籤：工具人

基於線特徵的點雲分割實現

野外環境下地形較為複雜，大體可粗略分為地平面、坡面和障礙物。對於鐳射雷達的原始點雲，通過分割演算法將地面、坡面、障礙物有效分離，對接下來的感知、規劃與決策過程非常重要。考慮一種簡單易於實現的傳統思路來分割點雲：基於線特徵的點雲分割技術，其包含橫向與縱向的判斷，可實現三種地形的有效分割。

實驗環境

速騰聚創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&&sectors_[i].sec_lines_[j].flag==3)//未判斷且無上鄰線段
			{
				/*
					//待新增
				*/
				sectors_[i].sec_lines_[j].flag=2;				
				continue;
			}
			//如果neborID!=0說明該線段有上鄰線段 可進行縱向判斷 若該線段還未進行判斷
			if(neborID!=0&&sectors_[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&&sectors_[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&&sectors_[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;
}

實驗結果

地面
地面

相關推薦

基於線特徵的點雲分割實現

技術標籤：工具人 基於線特徵的點雲分割實現 野外環境下地形較為複雜，大體可粗略分為地平面、坡面和障礙物。對於鐳射雷達的原始點雲，通過分割演算法將地面、坡面、障礙物有效分離，對接下來的感知、規劃與決策

python實點雲分割k-means(sklearn)詳解

本文例項為大家分享了Python實點雲分割k-means（sklearn），供大家參考，具體內容如下

Python實現點雲投影到平面顯示

值得學習的地方： 1.選擇合法索引的方式 2.陣列轉影象顯示 import numpy as np from PIL import Image

opencv2基於SURF特徵提取實現兩張影象拼接融合

本文例項為大家分享了opencv2實現兩張影象拼接融合的具體程式碼，供大家參考，具體內容如下

論文題目：基於K-means聚類的三維點雲分類

論文題目：基於K-means聚類的三維點雲分類論文主要思想： 先對原始點雲進行預處理保留關鍵點，密集去冗餘、稀疏進行三角形插值。再通過K-means聚類操作獲取區域性特徵，之後並行同過pointnet進行特徵提取。

基於點雲描述子的立體視覺里程計快速魯棒的位置識別方法

點雲PCL免費知識星球，點雲論文速讀。 文章：A Fast and Robust Place Recognition Approach for Stereo Visual Odometry Using LiDAR Descriptors

基於微信小程式雲開發實現的電商專案，可以自行定製開發

前言 很簡單的一個小程式，小程式中所有程式碼幾乎是我一個個敲出來的，所有邏輯也是自己構思梳理的，因此很多實現方式並不是很好，冗餘程式碼很多，程式碼質量堪憂，但我也在學習中，隨著技術提升，會不斷

基於體素化方法的點雲降取樣

技術標籤：點雲資料處理c++ 前兩天做了一個點雲降取樣的專案，用pcl自帶的降取樣方法出來的結果不是很理想，於是就自己寫了一個。為了使程式碼執行效率高點就採用了基於點雲索引的方式。 本文使用的方法為：首先

基於PCL- 利用kdtree計算點雲點距均值

技術標籤：三維點雲學習過程c++ 基於PCL- 利用kdtree計算點雲點距均值 //功能：利用kdtree計算點雲點距均值

基於百度和echars介面,實現多點連線

引言 今天用百度地圖和echarts,實現多點連線。 展示 前端html <!DOCTYPE html> <html class=\"x-admin-sm\">

【細胞分割】基於matlab GUI闕值+邊緣+形態學+種子點影象分割【含Matlab原始碼 615期】

一、簡介 基於matlab GUI闕值、邊緣、形態學、種子點影象分割 二、原始碼 function varargout = bishe2(varargin)

6：C++搭配PCL點雲配準之FPFH特徵

程式碼： 1 #pragma warning(disable:4996) 2 #include <pcl/registration/ia_ransac.h>//取樣一致性

5：C++搭配PCL點雲配準之PFH特徵

程式碼： 1 #pragma warning(disable:4996) 2 #include <pcl/registration/ia_ransac.h>//取樣一致性

基於華為雲TICS實現聯合風控模型訓練

背景 在銀行傳統的信用評估決策機制中，最常用的幾個特徵維度無非是個人資產、收入、信貸歷史、抵押擔保等。這些維度雖然能夠反映借款人的還款能力，但是過於簡單的規則往往也會拒絕掉很多潛在的優質客戶。並且稽核過

三維點雲處理系列---- 二叉樹實現K-NN Radius-NN Search

二叉樹的搜尋方法 一種是遞迴，一種是迴圈判斷，本質區別並不大 #遞迴搜尋 def search_recursively(root,key):#1NN 搜尋 ，遞迴法

基於Moya、RxSwift和ObjectMapper實現REST API請求

在Android開發中有非常強大的 Retrofit 請求，結合RxJava可以非常方便實現 RESTful API 網路請求。在 iOS開發中也有非常強大的網路請求庫 Moya ，Moya是一個基於 Alamofire 開發的,輕量級的Swift網路層。Moya的可擴充

如何基於RocketMQ的事務訊息特性實現分散式系統的最終一致性？

前言 在這篇文章中我們將介紹RocketMQ的事務訊息相關的內容，並通過一些實踐和大家一起來探索下事務訊息如何解決分散式系統中的分散式事務問題。

分散式事務一：基於資料庫原生分散式事務方案實現

宣告： 本篇主要對所用到的技術做了歸納總結，對原始碼講解較少，如果有基礎的朋友可以直接下載原始碼結合時序圖更能容易理解；基礎比較弱的朋友建議先看看資料自看原始碼這樣更容易理解。這裡的部分資料來源於網路

基於swoole與php協程實現非同步非阻塞IO

本文協程排程的實現參考了有讚的zanphp實現：zanphp.io/ 自己寫了一個簡單的swoole+php協程的框架：github.com/neuSnail/Pc…

Django搭建個人部落格：基於 LocalStorage 的點贊功能

假設你的部落格已經順利部署到了線上。你寫了很多好文章，和粉絲們互動並感受成就感。