基于激光雷达实现三边定位算法开发记录（四）——确定多根反光柱位置

22年暑假实习过程中第一个开发任务，基于激光雷达实现三边定位开发功能

 

开发平台为ubuntu 18.04 + ros melodic

日期：2022.7.21

本次实现：由于实现三边定位需要先找反光柱的圆心，目前计划先找出激光雷达获取反光柱反射激光的最强点，该点到激光雷达的距离加反光柱半径近似于圆心，误差在于激光雷达本身精度误差，可以接受。本次将优化查找高强点函数，并将高强点组传入查找最大值函数组内最强点，将最强点距离加上反光柱半径再进行运算，解算出反光柱圆心坐标，并在日viz中可视化。

（一）修改代码

查找高强度点组代码，首先遍历激光点数据，将其中超过强度阀值的点放入容器中作为大组，再根据指数突变标志分为各个小组，每一个小组为一个反光柱，并作处理。

//查找高强度点组作为反光柱
void findHigh (const sensor_msgs::LaserScan &scan) {
  std::vector< Point > points;
  std::vector< Point > points2;

  Point high{};
  Point stopMax{};
  Point end{2000,0,0,0,0};

  int HighStop = 1;

  for (long i = 0; i < scan.ranges.size(); i++) {        //获取高强度点
    if (scan.intensities[i] > 1500){                       //强度阀值
        high.index = i;
        high.range = scan.ranges[i] + RADIUS;
        high.intensity = scan.intensities[i];
        high.x = high.range * cos(scan.angle_min + scan.angle_increment * high.index);
        high.y = high.range * sin(scan.angle_min + scan.angle_increment * high.index);
      points.push_back(high);
    }
  }
  if(points.size() >2){
      long sum = points[0].index - 1;
      points.push_back(end);                            //结束标志位
  for (long j = 0; j< points.size(); j++) {          //分离各反光柱数据
    if((points[j].index  - sum)== 1) {
        points2.push_back(points[j]);
        std::cout<<HighStop <<"号反光柱"<<std::endl;    
        std::cout<<"指数：" <<points[j].index  <<"，第"<<points2.size()<<"个，强度:"<<points[j].intensity<<std::endl;       
     }else{
        if(points2.size() > 2 ) {                                //至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
        HighStop++;  
        std::cout<<"数量：" <<points2.size()<<std::endl;       
        stopMax = findMax(points2);                  //单独处理各个反光柱数据
        std::cout<<"激光最大数据:"<<"指数:"<<stopMax.index<<"距离:"<<stopMax.range<<"强度:"<<stopMax.intensity <<"x轴:"<<stopMax.x <<"y轴:"<<stopMax.y <<std::endl;
        set_marker_fixed_property(stopMax.x,stopMax.y,HighStop);
        }
       points2.clear();
      }
       sum =points[j].index;
    } 
  }else{
    std::cout<<"无法找到有效数据" <<std::endl;     
  }
       points.clear();
}

修改查找最大值函数，将小组信息传入，返回小组内最大值的点。

//查找最大值
Point findMax(std::vector< Point >  &points ) {
  Point max{};
  for (long i = 0; i < points.size(); i++) {
    double intensity = points[i].intensity;
    if (intensity > max.intensity) {
      max.index = points[i].index;
      max.range = points[i].range;
      max.intensity = points[i].intensity;
      max.x = points[i].x;
      max.y = points[i].y;
    }
  }
  return max;
}

修改设置反光柱标记函数，将标注id改为传入。

//设置反光柱标记并发布
void set_marker_fixed_property(float mx,float my,int n){
 ......
  marker_.id = n;
 ......
};

修改处理激光信息函数。

//处理激光信息
void scan_CB(sensor_msgs::LaserScan scan) {
  std::vector< Outline > outlines;
  std::vector< Point > points;

  findHigh(scan);
}

（二）运行结果

 （三）问题和下一目标

问题：反光柱距离不能太远，不然特征点不够

目标：三边定位算法编写