基于激光雷达实现三边定位算法开发记录（二）——查找多个反光柱数据

开发平台为ubuntu 18.04 + ros melodic

本次实现：由于实现三边定位需要先找反光柱的圆心，目前计划先找出激光雷达获取反光柱反射激光的最强点，该点到激光雷达的距离加反光柱半径近似于圆心，误差在于激光雷达本身精度误差，可以接受。由于昨天已经找到了反光柱的最强点，但是同一时间只能找到一个，不适用与三边定位所需要的三个，因此这次需要实现找到多个反光柱目标。

（一）修改程序

首先编写查找反光柱函数findHigh代码，同样是遍历所有激光强度，当激光强度连续三次超过2000时，即标定为激光雷达。在当前雷达环境下，普通环境的激光强度为1000以下，反光柱的最大激光强度可达3300以上。

//查找反光柱
void findHigh (const sensor_msgs::LaserScan &scan) {
  int HighStop = 0;
  long sum = 0;
  int num = 0;
  for (long i = 0; i < scan.ranges.size(); i++) {
    if (scan.intensities[i] > 2000){              //强度阀值
      if((i  - sum) < 2) {
        num++;    
        std::cout<<"指数："<<sum<<"，第"<<num <<"个，强度:"<<scan.intensities[i] <<std::endl;       
      }else{
        if(num > 2 || HighStop == 0) {      //连续三个点强度超过 强度阀值 即默认为反光柱
        HighStop++;
        std::cout<<HighStop <<"号反光柱"<<std::endl;       
        }
       num = 0;
      }
       sum = i;
    } 
  }
}

修改处理激光信息函数

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

  // for (int i = 0; i < 5; i++) {
  //   //  while (ros::ok()) {
  //   auto max = findMax(scan);//查找激光数据最大值
  //   std::cout<<"激光最大数据:"<<"指数:"<<max.index<<"距离:"<<max.range<<"强度:"<<max.intensity <<"x轴:"<<max.x <<"y轴:"<<max.y <<std::endl;
  //  }
  findHigh(scan);
}

修改完毕，开始执行。

（二）运行效果

（三） 问题和下一目标

问题：该算法目前无法将多余误差点过滤，例如指数为1647的点为玻璃强反光，且强度组在上，标号在下，即1号反光柱为空，下面的强度组为2号反观柱的。

目标：优化算法，将不满连续3次的强度点过滤，并从1号反观柱开始关联数据