# hdl_graph_slam （3）地面分割过滤 代码分析

hdl_graph_slam （3）地面分割过滤 代码分析

在点云预处理后，与点云配准同时进行的时输入点云的地面分割，分割采用基本的RANSAC算法（随机抽样一致），本篇文章分析一下代码的结构及关键部分。

• 根据传感器位置和设定的高度阈值进行对应高度平面的滤波 plane_clip，仅保留sensor_height±range中间值的点云

  filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
      filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);
  

  下面为plane_clip函数

  pcl::PointCloud<PointT>::Ptr plane_clip(const pcl::PointCloud<PointT>::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative) const {
      pcl::PlaneClipper3D<PointT> clipper(plane);
      pcl::PointIndices::Ptr indices(new pcl::PointIndices);
  
      clipper.clipPointCloud3D(*src_cloud, indices->indices);
  
      pcl::PointCloud<PointT>::Ptr dst_cloud(new pcl::PointCloud<PointT>);
  
      pcl::ExtractIndices<PointT> extract;
      extract.setInputCloud(src_cloud);
      extract.setIndices(indices);
      extract.setNegative(negative);
      extract.filter(*dst_cloud);
  
      return dst_cloud;
    }
  
  
  

• 若使用法线滤波（适用于地面较为平整的情况）则使用表面法线估计过滤掉点云中非垂直法线的点（仅保留法线是垂直的点）

  if(use_normal_filtering) {
        filtered = normal_filtering(filtered);
      }
  

  法线滤波函数，创建法线估计对象，使用kdtree搜索并计算近邻点法线，滤除掉法线不垂直的点

  pcl::PointCloud<PointT>::Ptr normal_filtering(const pcl::PointCloud<PointT>::Ptr& cloud) const {
      pcl::NormalEstimation<PointT, pcl::Normal> ne;
      ne.setInputCloud(cloud);
  
      pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
      ne.setSearchMethod(tree);
  
      pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
      ne.setKSearch(10);
      ne.setViewPoint(0.0f, 0.0f, sensor_height);
      ne.compute(*normals);
  
      pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
      filtered->reserve(cloud->size());
  
      for(int i = 0; i < cloud->size(); i++) {
        float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ());
        if(std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) {
          filtered->push_back(cloud->at(i));
        }
      }
  
      filtered->width = filtered->size();
      filtered->height = 1;
      filtered->is_dense = false;
  
      return filtered;
    }
  

• RANSAC平面参数拟合 直接调用pcl库中的ransacmodelplane 模型进行参数赋值计算即可

  // too few points for RANSAC 
      if(filtered->size() < floor_pts_thresh) {
        return boost::none;
      }
  
      // RANSAC
      pcl::SampleConsensusModelPlane<PointT>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointT>(filtered));
      pcl::RandomSampleConsensus<PointT> ransac(model_p);
      ransac.setDistanceThreshold(0.1);
      ransac.computeModel();
  
      pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
      ransac.getInliers(inliers->indices);
  
      // too few inliers
      if(inliers->indices.size() < floor_pts_thresh) {
        return boost::none;
      }
  
      // verticality check of the detected floor's normal
      Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();
  
      Eigen::VectorXf coeffs;
      ransac.getModelCoefficients(coeffs);
  
      double dot = coeffs.head<3>().dot(reference.head<3>());
      if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) {
        // the normal is not vertical
        return boost::none;
      }
  
      // make the normal upward
      if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) {
        coeffs *= -1.0f;
      }
  
      if(floor_points_pub.getNumSubscribers()) {
        pcl::PointCloud<PointT>::Ptr inlier_cloud(new pcl::PointCloud<PointT>);
        pcl::ExtractIndices<PointT> extract;
        extract.setInputCloud(filtered);
        extract.setIndices(inliers);
        extract.filter(*inlier_cloud);
        inlier_cloud->header = cloud->header;
  
        floor_points_pub.publish(inlier_cloud);
      }
  
      return Eigen::Vector4f(coeffs);
    }
  

• 小结：地面检测使用RANSAC，pcl自带，平面模型检测添加初步滤波，缩小样本的数量，加速滤波过程。但在滤波过程中只对z轴进行 高度滤波。若地面在某处产生小幅度阶跃，会将不同高度地面拟合为同一平面。可使用分段地面检测进行模型拟合。

• 参考内容：
  [1] ransac地面分割
  [2] pcl法线估计