hdl_graph_slam源码解读(六)：地面检测

1. 概述

地面检测的作用我们在第一节就讲过了，它为建图提供一个约束，可以提高建图精度。

地面检测的代码全都在apps/floor_detection_nodelet.cpp文件中，这是一个节点文件，里面关于订阅和发布数据的内容我们不讲了，只介绍关于检测的核心算法。

地面检测共分为三个步骤：

1. 根据高度参数把地面分割出来，由函数plane_clip完成
2. 把地面点云中，把法向量和地面实际法向量差别较大的点去掉，这一步由函数normal_filtering完成
3. 对剩下的点进行平面参数拟合，这部分代码在函数detect中（前两部对应的函数也是在detect中被调用）

2. 分割地面

思路比较简单，由于pcl::PlaneClipper3D可以把指定点云区域一侧的点全部去掉，所以执行两次滤波就可以了，两次分别把地面高度上面和下面的点全部去掉，就只剩下地面点云了。

先看plane_clip函数，也就是去掉一侧点云的函数，没太多可讲的，pcl函数调用的标准流程，给它参数，然后执行就可以了。

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;
  }

然后这个函数在detect函数中被调用了两次：

pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
    pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
    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);

这两次就把地面上下两个方向的其他点云都去掉了

3. 去掉法向量异常的点

地面点云每个点所处位置对应的法向量都应该是向上的，即使有些差异，也不应该过大，否则就不是地面点，按照这个思路，我们又可以去掉一部分点云。这里法向量计算是用的pcl库中的函数pcl::NormalEstimation：

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;
  }

4. 拟合平面参数

现在我们有了一个平面对应的点云，需要对他进行拟合。这部分代码来自detect函数，拟合所用的算法也是使用pcl库：

    //点太少，则直接退出
    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;
    }

参考文献

[1] hdl_graph_slam源码解读(六)：地面检测