slam 与定位中的 地面检测

首先需要认识一下什么是地面,一般来说的地面就是可供行驶的平面, 那么一个假设是,地面是法线与激光雷达接近平行的平面(±30度都可接受), 墙面的法线与激光是垂直的,还有一些坡度很大的斜坡,这些都不认为是地面.
地面检测与分割的作用:

地面检测算法汇总

1 基于RANSAC的方法 推荐
RANSAC 激光雷达地面检测
2 基于角度的方法 推荐
基于角度方法的地面检测
来源 : Effificient Online Segmentation for Sparse 3D Laser Scans
3 Ray Ground Filter 不推荐
4 基于平面拟合 和RANSAC类似

常见开源SLAM算法的地面检测

HDL

hdl_graph_slam使用的是一种基于RANSAC的地面检测方法, 主要流程是:

1 高度滤波 , 地面假设只出现在雷达下方, 所以把雷达上方的点滤除.

2 法向量滤波 , 由于默认为雷达与地面接近垂直(可以优化) 求出雷达局部坐标系中点云的法向量, 将法向量与Z轴偏差过大的去掉, 这样就将墙面以及其他物体的点剔除了.

3 RANSAC 拟合平面 , 获取地面在激光雷达坐标系下的方程, 并提取内点就可获得地面点.

hdl_graph_slam主要是利用地面信息提供约束,

通过实时检测当前地面，并求解该地面在激光雷达坐标系中的方程表示，从而和设定的全局一致地面在当前位姿状态R，T下的方程构成残差，优化位姿使得全局地面在该位姿下的表示与检测到的地面尽可能一致。下面重点学习如何通过激光雷达检测地面方程。
先分析一下节点的订阅发布关系。
订阅：/filtered_points 即数据处理后的激光点云。
发布：/floor_detection/floor_coeffs ：地面参数话题 ， 另外还有地面点云的话题。

经过数据处理节点后每一帧点云会进入该nodelet的cloud_callback()中进行处理获取地面信息。
在回调函数中每帧点云通过boost::optionalEigen::Vector4f floor = detect(cloud)函数进行地面检测。
然后将检测到的地面参数进行发布floor_pub.publish(coeffs)。
下面来看一下地面检测的代码。

detect()

首先我们要理解代码中检测的地面是什么，首先代码中有一个前提假设，就是车体的Z轴与地面接近垂直，通过这个假设我们可以首先将那些点云法向量与雷达正上方夹角过大的点剔除地面的候选中。这个假设仅仅适用与轮式机器人这种姿态与地面平行的移动平台, 如果运行时雷达倾斜较大,则通过法向量无法判断是否为地面。
代码如下，地面检测主要分一下几个步骤：
1、首先是假设我们的激光雷达相对于车体有倾斜(这里只考虑前后的倾斜)，那么根据这个已知的倾斜角将点云转换到车体坐标系下。
2、对点云进行裁剪plane_clip()，只保留在某一高度范围内的点云。
3、执行法线滤波 ，滤除那些法线方向与车体正上方偏差较大的点云。
4、如果进行了倾斜校正，则恢复为原始的点云，因为，要求解的是地面在激光雷达坐标系中的参数表示，而不是车体坐标。。
5、执行RANSAC来拟合平面参数，并获取内点。
6、检查求出来的地面法向量是否与车体Z轴接近垂直，如果接近则认为该地面有效，因为对于车来说，车体总是于地面接近平行的。

boost::optional<Eigen::Vector4f> detect(const pcl::PointCloud<PointT>::Ptr& cloud) const {
     
// compensate the tilt rotation  对激光雷达的倾斜进行补偿   
Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
// 绕y轴旋转 tilt_deg   tilt_deg默认为0    
tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix();

// filtering before RANSAC (height and normal filtering)
pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);    
pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);                         // 倾斜补偿
// 获取 [sensor_height - height_clip_range,sensor_height + height_clip_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);
// 法线滤波   
if(use_normal_filtering) {
     
  filtered = normal_filtering(filtered);                                          // 返回地面点云部分
}
// 将点云恢复     因为要求取地面相对与雷达的方程      
pcl::transformPointCloud(*filtered, *filtered, static_cast<Eigen::Matrix4f>(tilt_matrix.inverse()));

if(floor_filtered_pub.getNumSubscribers()) {
     
  filtered->header = cloud->header;
  floor_filtered_pub.publish(filtered);   // 这里还不是真正的地面
}

// 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 拟合平面    
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;         // 提取index
  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);
}

点云裁剪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);                       // 点云裁剪类对象    裁剪平面方程plane
 pcl::PointIndices::Ptr indices(new pcl::PointIndices);            // 裁剪后索引
 // 将平面一下的部分放入索引indices中
 clipper.clipPointCloud3D(*src_cloud, indices->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);                                   // false :提取索引   true: 索引之外
 extract.filter(*dst_cloud);                                      // 提取之后的点云

 return dst_cloud;
}

其中有两个主要的类，pcl::PlaneClipper3D和pcl::ExtractIndices。
pcl::PlaneClipper3D用于对点云进行裁剪，clipPointCloud3D()函数将提取指定平面一下的部分的索引。
参数如下
pcl::ExtractIndices根据索引pcl::PointIndices::Ptr对点云进行提取，setNegative()设置提取的对象，参数为false时提取索引的点云，为true时提取非索引部分。

pcl::PointIndices在PCL中的定义如下，它通过std::vector indices来存储索引。

struct PointIndices
  {
     
    PointIndices ()
    {
     }

    ::pcl::PCLHeader header;

    std::vector<int> indices;

    public:
      using Ptr = boost::shared_ptr< ::pcl::PointIndices>;
      using ConstPtr = boost::shared_ptr<const ::pcl::PointIndices>;
  }; // struct PointIndices

法向量滤波

首先注意输入的激光点云是以激光雷达为原点的，这段代码中，是将点云中法向量与Z轴即激光雷达正上方的夹角小于normal_filter_thresh的点放入filtered中，作为地面的候选点，也就是那些法向量接近雷达方向的点作为地面候选点，显然，这个简单的策略只在雷达的运动与地面接近垂直的前提假设下有效。

  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>);     // kd树
    ne.setSearchMethod(tree);                 // 设置搜索方法为kd树   通过这种方法查找临近点    
    // 创建一个pcl::Normal即法相量点云  
    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());
    // 遍历输入点云的所有点   将法线偏离垂直20度以上的点去除    
    for (int i = 0; i < cloud->size(); i++) {
     
      // 第i个点的法相量与Z轴法相量点积   即  cos
      float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ());
      // normal_filter_thresh为20度   误差最大20度
      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;
  }

效果实测还是很好的,每帧处理的时间基本在10ms以内.
下图是地面滤除效果

legoloam