livox_mapping 特征提取代码解析

代码来自于 livox_mapping，先简单说结论：

异常点提取：与左右点的距离大于深度值的十分之一

平面点提取： 主要通过左边或者右边四个点的曲率小于 0.001 计算得到；

角点提取：主要有几个来源

平面角点，左右都是平面且平面夹角 60-120之间; （可以理解为方形柱子的边缘特征点）

断点角点：（可以理解为物体的边缘到远处的扫描点 ）根据当前点与左右附近点的距离判断，断点阈值0.1； 并且有一边的点是平面； 点与点之间没有太远； 最远点距离不超过深度值的二十分之一； 当前点和最远点合成的平面 和当前点向量的夹角，夹角 37度到 143度；

其他的livox 项目特征提取也是大同小异的，看懂了这个基本其他的也都类似；

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
  pcl::PointCloud<PointType> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

  int cloudSize = laserCloudIn.points.size();

  std::cout<<"DEBUG first cloudSize "<<cloudSize<<std::endl; 

  if(cloudSize > 32000) cloudSize = 32000;
  
  int count = cloudSize;

  PointType point;
  std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].x;
    point.y = laserCloudIn.points[i].y;
    point.z = laserCloudIn.points[i].z;
    point.intensity = laserCloudIn.points[i].intensity;
    point.curvature = laserCloudIn.points[i].curvature;
    int scanID = 0;
    if (N_SCANS == 6) {
      scanID = (int)point.intensity;
    }
    laserCloudScans[scanID].push_back(point);
  }  //todo  这里先把他分成了6份，又在下面把点云合成了一份 意思是先把点云按照每段分开，然后再合成；

  pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());

  for (int i = 0; i < N_SCANS; i++) {
    *laserCloud += laserCloudScans[i];
  }

  cloudSize = laserCloud->size();

  for (int i = 0; i < cloudSize; i++) {
    CloudFeatureFlag[i] = 0;
  }
    
  pcl::PointCloud<PointType> cornerPointsSharp;

  pcl::PointCloud<PointType> surfPointsFlat;

  pcl::PointCloud<PointType> laserCloudFull;

  int debugnum1 = 0;
  int debugnum2 = 0;
  int debugnum3 = 0;
  int debugnum4 = 0;
  int debugnum5 = 0;

  int count_num = 1;
  bool left_surf_flag = false;
  bool right_surf_flag = false;
  Eigen::Vector3d surf_vector_current(0,0,0);
  Eigen::Vector3d surf_vector_last(0,0,0);
  int last_surf_position = 0;
  double depth_threshold = 0.1;

//  todo 第一次遍历 找特征点
  //********************************************************************************************************************************************
  for (int i = 5; i < cloudSize - 5; i += count_num ) {
    float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
                       laserCloud->points[i].y * laserCloud->points[i].y +
                       laserCloud->points[i].z * laserCloud->points[i].z);

    // if(depth < 2) depth_threshold = 0.05;
    // if(depth > 30) depth_threshold = 0.1;
    //left curvature//  todo 以 i-2 为中心的曲率计算
    float ldiffX = 
                laserCloud->points[i - 4].x + laserCloud->points[i - 3].x
                - 4 * laserCloud->points[i - 2].x
                + laserCloud->points[i - 1].x + laserCloud->points[i].x;

    float ldiffY = 
                laserCloud->points[i - 4].y + laserCloud->points[i - 3].y
                - 4 * laserCloud->points[i - 2].y
                + laserCloud->points[i - 1].y + laserCloud->points[i].y;

    float ldiffZ = 
                laserCloud->points[i - 4].z + laserCloud->points[i - 3].z
                - 4 * laserCloud->points[i - 2].z
                + laserCloud->points[i - 1].z + laserCloud->points[i].z;
//  todo 左边曲率
    float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ;

    if(left_curvature < 0.01){  //  todo 平面点

      std::vector<PointType> left_list;  //  todo 这个list没卵用啊。。。。

      for(int j = -4; j < 0; j++){
        left_list.push_back(laserCloud->points[i+j]);
      }
//  todo 为啥还弄了2个平面点的标准   这个更严格  但是它是把 i-2 设置为平面点了， 不是i   平面点主要在这里找到的 还是靠曲率 不过标准好高啊
      if( left_curvature < 0.001) CloudFeatureFlag[i-2] = 1; //surf point flag  && plane_judge(left_list,1000) 
      //  todo 平面点判断在这里

      left_surf_flag = true;  //  todo 起作用的是它
    }
    else{
      left_surf_flag = false;
    }

    //right curvature
    float rdiffX = 
                laserCloud->points[i + 4].x + laserCloud->points[i + 3].x
                - 4 * laserCloud->points[i + 2].x
                + laserCloud->points[i + 1].x + laserCloud->points[i].x;

    float rdiffY = 
                laserCloud->points[i + 4].y + laserCloud->points[i + 3].y
                - 4 * laserCloud->points[i + 2].y
                + laserCloud->points[i + 1].y + laserCloud->points[i].y;

    float rdiffZ = 
                laserCloud->points[i + 4].z + laserCloud->points[i + 3].z
                - 4 * laserCloud->points[i + 2].z
                + laserCloud->points[i + 1].z + laserCloud->points[i].z;

    float right_curvature = rdiffX * rdiffX + rdiffY * rdiffY + rdiffZ * rdiffZ;

    if(right_curvature < 0.01){
      std::vector<PointType> right_list;

      for(int j = 1; j < 5; j++){
        right_list.push_back(laserCloud->points[i+j]);
      }
        if(right_curvature < 0.001 ) CloudFeatureFlag[i+2] = 1; //surf point flag  && plane_judge(right_list,1000)
//  todo 平面点判断在这里

      count_num = 4;
      right_surf_flag = true;
    }
    else{
      count_num = 1;
      right_surf_flag = false;
    }

    //surf-surf corner feature
    //  todo 如果左右都是平面，那么就继续处理； 这里找到的是左右都是平面的角点
    if(left_surf_flag && right_surf_flag){
     debugnum4 ++;

     Eigen::Vector3d norm_left(0,0,0);
     Eigen::Vector3d norm_right(0,0,0);
     for(int k = 1;k<5;k++){
         Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x,
                            laserCloud->points[i-k].y-laserCloud->points[i].y,
                            laserCloud->points[i-k].z-laserCloud->points[i].z);
        tmp.normalize();
        norm_left += (k/10.0)* tmp;  //  todo 左右点的向量相加，越远的权重越大
     }
     for(int k = 1;k<5;k++){
         Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x,
                            laserCloud->points[i+k].y-laserCloud->points[i].y,
                            laserCloud->points[i+k].z-laserCloud->points[i].z);
        tmp.normalize();
        norm_right += (k/10.0)* tmp;
     }

// todo 左右点群的向量夹角
      //calculate the angle between this group and the previous group
      double cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) );
      //calculate the maximum distance, the distance cannot be too small
      Eigen::Vector3d last_tmp = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x,
                                                 laserCloud->points[i-4].y-laserCloud->points[i].y,
                                                 laserCloud->points[i-4].z-laserCloud->points[i].z);
      Eigen::Vector3d current_tmp = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x,
                                                    laserCloud->points[i+4].y-laserCloud->points[i].y,
                                                    laserCloud->points[i+4].z-laserCloud->points[i].z);
      double last_dis = last_tmp.norm();
      double current_dis = current_tmp.norm();
//  todo   与左，与右最远点的距离大于阈值  并且 左右点群的向量夹角 小于 0.5;  0.5 是60度，0.95是18度； 0.8是37度； 这里就是 60-120度
  todo   为什么 小于 60度的不算角点呢？？？ 可能因为这里还只是初步计算了平面类型的角点，不是所有类型的
      if(cc < 0.5 && last_dis > 0.05 && current_dis > 0.05 ){ //
        debugnum5 ++;
        CloudFeatureFlag[i] = 150;  //  todo 角点
      }
    }
  }


  //  todo   第二次遍历计算特征   断点角点
  for(int i = 5; i < cloudSize - 5; i ++){
    float diff_left[2];
    float diff_right[2];
    float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
                        laserCloud->points[i].y * laserCloud->points[i].y +
                        laserCloud->points[i].z * laserCloud->points[i].z);

    for(int count = 1; count < 3; count++ ){
      float diffX1 = laserCloud->points[i + count].x - laserCloud->points[i].x;
      float diffY1 = laserCloud->points[i + count].y - laserCloud->points[i].y;
      float diffZ1 = laserCloud->points[i + count].z - laserCloud->points[i].z;
      diff_right[count - 1] = sqrt(diffX1 * diffX1 + diffY1 * diffY1 + diffZ1 * diffZ1);
//  todo  就是与左右点的距离计算 得到了一个列表 这里是计算了左右各三个点的距离
      float diffX2 = laserCloud->points[i - count].x - laserCloud->points[i].x;
      float diffY2 = laserCloud->points[i - count].y - laserCloud->points[i].y;
      float diffZ2 = laserCloud->points[i - count].z - laserCloud->points[i].z;
      diff_left[count - 1] = sqrt(diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2);
    }

//  todo  这里是左右单个点的深度值
    float depth_right = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
                    laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
                    laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
    float depth_left = sqrt(laserCloud->points[i - 1].x * laserCloud->points[i - 1].x +
                    laserCloud->points[i - 1].y * laserCloud->points[i - 1].y +
                    laserCloud->points[i - 1].z * laserCloud->points[i - 1].z);

     // todo outliers   与左右点的距离大于深度值的十分之一，就是异常点，不要他
    if( (diff_right[0] > 0.1*depth && diff_left[0] > 0.1*depth) ){
      debugnum1 ++;  
      CloudFeatureFlag[i] = 250;
      continue;
    }

    //break points  //  todo  与左边点的距离差 - 与右边点的距离差 大于 0.1 ，就是断点
    if(fabs(diff_right[0] - diff_left[0])>0.1){
      if(diff_right[0] > diff_left[0]){  //  todo  当与右边点距离更大的时候，使用左边点
//  todo  最远点合成的向量
        Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x,
                                                  laserCloud->points[i-4].y-laserCloud->points[i].y,
                                                  laserCloud->points[i-4].z-laserCloud->points[i].z);
        Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x,
                                                      laserCloud->points[i].y,
                                                      laserCloud->points[i].z);
        double left_surf_dis = surf_vector.norm();
        //  todo 最远点平面 和当前点向量的夹角
        //calculate the angle between the laser direction and the surface
        double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) );

        std::vector<PointType> left_list;
        double min_dis = 10000;
        double max_dis = 0;
        for(int j = 0; j < 4; j++){   //TODO: change the plane window size and add thin rod support
          left_list.push_back(laserCloud->points[i-j]);
          Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i-j].x-laserCloud->points[i-j-1].x,
                                                  laserCloud->points[i-j].y-laserCloud->points[i-j-1].y,
                                                  laserCloud->points[i-j].z-laserCloud->points[i-j-1].z);

          if(j == 3) break;
          double temp_dis = temp_vector.norm();
          if(temp_dis < min_dis) min_dis = temp_dis;
          if(temp_dis > max_dis) max_dis = temp_dis;
        }
        //  todo   第2个特征根大于100倍的最小特征根，就是平面
        bool left_is_plane = plane_judge(left_list,100);
//  todo  左边点是平面；  点与点之间没有太远； 左边最远点距离不超过深度值的二十分之一； 夹角 37度到 143度；
        if(left_is_plane && (max_dis < 2*min_dis) && left_surf_dis < 0.05 * depth  && cc < 0.8){//
          if(depth_right > depth_left){
            CloudFeatureFlag[i] = 100;  //  todo  角点
          }
          else{
            if(depth_right == 0) CloudFeatureFlag[i] = 100;  //  todo   只在右边点深度值为0的时候才是角点
          }
        }
      }
      else{

        Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x,
                                                      laserCloud->points[i+4].y-laserCloud->points[i].y,
                                                      laserCloud->points[i+4].z-laserCloud->points[i].z);
        Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x,
                                                      laserCloud->points[i].y,
                                                      laserCloud->points[i].z);
        double right_surf_dis = surf_vector.norm();
        //calculate the angle between the laser direction and the surface
        double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) );

        std::vector<PointType> right_list;
        double min_dis = 10000;
        double max_dis = 0;
        for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support
          right_list.push_back(laserCloud->points[i-j]);
          Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i+j].x-laserCloud->points[i+j+1].x,
                                                  laserCloud->points[i+j].y-laserCloud->points[i+j+1].y,
                                                  laserCloud->points[i+j].z-laserCloud->points[i+j+1].z);

          if(j == 3) break;
          double temp_dis = temp_vector.norm();
          if(temp_dis < min_dis) min_dis = temp_dis;
          if(temp_dis > max_dis) max_dis = temp_dis;
        }
        bool right_is_plane = plane_judge(right_list,100);

        if(right_is_plane && (max_dis < 2*min_dis) && right_surf_dis < 0.05 * depth && cc < 0.8){ // 

          if(depth_right < depth_left){
            CloudFeatureFlag[i] = 100;
          }
          else{
            if(depth_left == 0) CloudFeatureFlag[i] = 100;       
          }
        }
      }
    }

    // break point select  //  todo 如果当前点是断点 继续操作
    if(CloudFeatureFlag[i] == 100){
      debugnum2++;
      std::vector<Eigen::Vector3d> front_norms;
      Eigen::Vector3d norm_front(0,0,0);
      Eigen::Vector3d norm_back(0,0,0);
      for(int k = 1;k<4;k++){
          Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x,
                            laserCloud->points[i-k].y-laserCloud->points[i].y,
                            laserCloud->points[i-k].z-laserCloud->points[i].z);
        tmp.normalize();
        front_norms.push_back(tmp);
        norm_front += (k/6.0)* tmp;
      }
      std::vector<Eigen::Vector3d> back_norms;
      for(int k = 1;k<4;k++){
          Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x,
                            laserCloud->points[i+k].y-laserCloud->points[i].y,
                            laserCloud->points[i+k].z-laserCloud->points[i].z);
        tmp.normalize();
        back_norms.push_back(tmp);
        norm_back += (k/6.0)* tmp;
      }
      //  todo 左边三个点群的加权向量 和右边三个点群的加强向量的夹角  37~143之间 ； 超过了就把它标签设置为0， 对之前判断的100标签的再次判断
      double cc = fabs( norm_front.dot(norm_back) / (norm_front.norm()*norm_back.norm()) );
        if(cc < 0.8){
        debugnum3++;
      }else{
        CloudFeatureFlag[i] = 0;
      }

      continue;

    }
  }


//  todo 第三次遍历 放入特征点

  //push_back feature
  for(int i = 0; i < cloudSize; i++){
    //laserCloud->points[i].intensity = double(CloudFeatureFlag[i]) / 10000;
    float dis = laserCloud->points[i].x * laserCloud->points[i].x
                + laserCloud->points[i].y * laserCloud->points[i].y
                + laserCloud->points[i].z * laserCloud->points[i].z;
    float dis2 = laserCloud->points[i].y * laserCloud->points[i].y + laserCloud->points[i].z * laserCloud->points[i].z;
    float theta2 = std::asin(sqrt(dis2/dis)) / M_PI * 180;
    //std::cout<<"DEBUG theta "<<theta2<<std::endl;
    // if(theta2 > 34.2 || theta2 < 1){
    //    continue;
    // }
    //if(dis > 30*30) continue;

    if(CloudFeatureFlag[i] == 1){
      surfPointsFlat.push_back(laserCloud->points[i]);
      continue;
    }

    if(CloudFeatureFlag[i] == 100 || CloudFeatureFlag[i] == 150){
        cornerPointsSharp.push_back(laserCloud->points[i]);
    } 
  }


  std::cout<<"ALL point: "<<cloudSize<<" outliers: "<< debugnum1 << std::endl
            <<" break points: "<< debugnum2<<" break feature: "<< debugnum3 << std::endl
            <<" normal points: "<< debugnum4<<" surf-surf feature: " << debugnum5 << std::endl;

  sensor_msgs::PointCloud2 laserCloudOutMsg;
  pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
  laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
  laserCloudOutMsg.header.frame_id = "/livox";
  pubLaserCloud.publish(laserCloudOutMsg);

  sensor_msgs::PointCloud2 cornerPointsSharpMsg;
  pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
  cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
  cornerPointsSharpMsg.header.frame_id = "/livox";
  pubCornerPointsSharp.publish(cornerPointsSharpMsg);

  sensor_msgs::PointCloud2 surfPointsFlat2;
  pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
  surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
  surfPointsFlat2.header.frame_id = "/livox";
  pubSurfPointsFlat.publish(surfPointsFlat2);

}