autoware.ai感知随笔--地面滤波

autwoware.ai中点云预处理–points_preprocessor
points_preprocessor
cloud_transformer: 点云坐标转换,将输入的点云转化为velodyne坐标系下的点云。
compare_map_filter: 对比激光雷达点云和点云地图，然后提取（或去除）一致的点。

|`input_point_topic`|*String*|PointCloud source topic. Default `/points_raw`.|
|`input_map_topic`|*String*|PointCloud Map topic. Default `/points_map`.|
|`output_match_topic`|*String*|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which matched. Default `/points_ground`.|
|`output_unmatch_topic`|*String*|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which unmatched. Default `/points_no_ground`.|
|`distance_threshold`|*Double*|Threshold for comparing LiDAR PointCloud and PointCloud Map. Euclidean distance (mether).  Default `0.3`.|
|`min_clipping_height`|*Double*|Remove the points where the height is lower than the threshold. (Based on sensor coordinates). Default `-2.0`.|
|`max_clipping_height`|*Double*|Remove the points where the height is higher than the threshold. (Based on sensor coordinates). Default `0.5`.|

configcallback:主要是为 distance_threshold、min_clipping_height、max_clipping_height三个参数赋值。

 pcl::KdTreeFLANN<pcl::PointXYZI> tree_; // k-d树进行快速最近邻搜索。

pointsMapCallback：为map_frame_进行赋值，tree_.setInputCloud(map_cloud_ptr);
sensorPointsCallback：首先根据min_clipping_height和max_clipping_height进行一个高度的提取。

 tf::TransformListener* tf_listener_;
  tf_listener_->waitForTransform(map_frame_, sensor_frame, sensor_time, ros::Duration(3.0));
    pcl_ros::transformPointCloud(map_frame_, sensor_time, *sensorTF_clipping_height_cloud_ptr, sensor_frame,
                                 *mapTF_cloud_ptr, *tf_listener_);

利用以上代码将原始点云转化为地图坐标系下的点云。

searchMatchingCloud(mapTF_cloud_ptr, mapTF_match_cloud_ptr, mapTF_unmatch_cloud_ptr);

通过searchMatchingCloud函数得到匹配的点云和未匹配得到的点云。

void CompareMapFilter::searchMatchingCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
                                           pcl::PointCloud<pcl::PointXYZI>::Ptr match_cloud_ptr,
                                           pcl::PointCloud<pcl::PointXYZI>::Ptr unmatch_cloud_ptr)
{
  match_cloud_ptr->points.clear();
  unmatch_cloud_ptr->points.clear();

  match_cloud_ptr->points.reserve(in_cloud_ptr->points.size());
  unmatch_cloud_ptr->points.reserve(in_cloud_ptr->points.size());

  std::vector<int> nn_indices(1);
  std::vector<float> nn_dists(1);
  const double squared_distance_threshold = distance_threshold_ * distance_threshold_;

  for (size_t i = 0; i < in_cloud_ptr->points.size(); ++i)
  {
    tree_.nearestKSearch(in_cloud_ptr->points[i], 1, nn_indices, nn_dists);
    if (nn_dists[0] <= squared_distance_threshold)
    {
      match_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
    }
    else
    {
      unmatch_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
    }
  }
}

主要是根据k-d树的快速最近邻搜索算法得到匹配点云和未匹配点云。

points_concat_filter: 合并点云，同步接收多个点云信息。

cloud_synchronizer_ = new message_filters::Synchronizer<SyncPolicyT>(
      SyncPolicyT(10), *cloud_subscribers_[0], *cloud_subscribers_[1], *cloud_subscribers_[2], *cloud_subscribers_[3],
      *cloud_subscribers_[4], *cloud_subscribers_[5], *cloud_subscribers_[6], *cloud_subscribers_[7]);
  cloud_synchronizer_->registerCallback(
      boost::bind(&PointsConcatFilter::pointcloud_callback, this, _1, _2, _3, _4, _5, _6, _7, _8));

ray_ground_filter:地面滤波
射线法去除地面，该算法适用于机械式旋转雷达
// Model | Horizontal | Vertical | FOV(Vertical) degrees / rads
// ----------------------------------------------------------
// HDL-64 |0.08-0.35(0.32) | 0.4 | -24.9 <=x<=2.0 (26.9 / 0.47)
// HDL-32 | 0.1-0.4 | 1.33 | -30.67<=x<=10.67 (41.33 / 0.72)
// VLP-16 | 0.1-0.4 | 2.0 | -15.0<=x<=15.0 (30 / 0.52)
// VLP-16HD| 0.1-0.4 | 1.33 | -10.0<=x<=10.0 (20 / 0.35)

|`input_point_topic`|*String*|PointCloud source topic. Default `/points_raw`.|
|`ground_topic`|*String*|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which matched. Default `/points_ground`.|
|`no_ground_topic`|*String*|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which unmatched. Default `/points_no_ground`.|
|`general_max_slope`|*Double*|Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees).|
|`local_max_slope`|*Double*|Max Slope of the ground between Points (default 8 degrees).|
|`min_height_threshold`|*Double*|Minimum height threshold between points (default 0.05 meters).|
|`clipping_height`|*Double*|Remove Points above this height value (default 2.0 meters).|
|`min_point_distance`|*Double*|Removes Points closer than this distance from the sensor origin (default 1.85 meters).|
|`radial_divider_angle`|*Double*|Angle of each Radial division on the XY Plane (default 0.08 degrees).|
|`reclass_distance_threshold`|*Double*|Distance between points at which re classification will occur (default 0.2 meters).|

首先经过TransformPointCloud函数将点云坐标系转化为车身坐标系下的坐标。

bool RayGroundFilter::TransformPointCloud(const std::string& in_target_frame,
                                          const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
                                          const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr)
{
  if (in_target_frame == in_cloud_ptr->header.frame_id)
  {
    *out_cloud_ptr = *in_cloud_ptr;
    return true;
  }

  geometry_msgs::TransformStamped transform_stamped;
  try
  {
    transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
                                                   in_cloud_ptr->header.stamp, ros::Duration(1.0));
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return false;
  }
  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
  out_cloud_ptr->header.frame_id = in_target_frame;
  return true;
}

在进行地面滤除之前裁减过高的点

void RayGroundFilter::ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
                                pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr)
{
  pcl::ExtractIndices<pcl::PointXYZI> extractor;
  extractor.setInputCloud(in_cloud_ptr);
  pcl::PointIndices indices;
// #pragma omp for语法OpenMP的并行化语法，即希望通过OpenMP并行化执行这条语句后的for循环，从而起到加速的效果。
#pragma omp for
  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    // 添加判断,去除空点
    if (in_cloud_ptr->points[i].z > in_clip_height || isnan(in_cloud_ptr->points[i].x) || isnan(in_cloud_ptr->points[i].y) || isnan(in_cloud_ptr->points[i].z))
    {
      indices.indices.push_back(i);
    }
  }
  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  extractor.setNegative(true); // true removes the indices, false leaves only the indices
  extractor.filter(*out_clipped_cloud_ptr);
}

通过RemovePointsUpTo函数避免车身点云的影响

/*!
 * Removes points up to a certain distance in the XY Plane
 * @param in_cloud_ptr Input PointCloud
 * @param in_min_distance Minimum valid distance, points closer than this will be removed.
 * @param out_filtered_cloud_ptr Resulting PointCloud with the invalid points removed.
 */
void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
                                       pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr)
{
  pcl::ExtractIndices<pcl::PointXYZI> extractor;
  extractor.setInputCloud(in_cloud_ptr);
  pcl::PointIndices indices;

#pragma omp for
  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    if (sqrt(in_cloud_ptr->points[i].x * in_cloud_ptr->points[i].x +
             in_cloud_ptr->points[i].y * in_cloud_ptr->points[i].y) < in_min_distance)
    {
      indices.indices.push_back(i);
    }
  }
  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  extractor.setNegative(true);  // true removes the indices, false leaves only the indices
  extractor.filter(*out_filtered_cloud_ptr);
}

Ray_groud_filter算法主要是以射线的方式（RAY）来组织点云，现在将点云(x,y,z)三维空间降到(x,y)平面去看，计算每一个点到车辆x正方向的平面夹角θ，我们对360度进行微分，分成若干等份，每一份的角度为0.18，这个微分可以看作一条射线，每条射线上的点又根据水平距离(点到lidar的水平距离，半径)0.001再进行微分。

为了方便对点进行半径和夹角的表示，我们使用如下数据结构代替 PointXYZIRTColor

  struct PointXYZIRTColor
  {
    pcl::PointXYZI point;

    float radius;  // cylindric coords on XY Plane
    float theta;   // angle deg on XY plane

    size_t radial_div;      // index of the radial divsion to which this point belongs to
    size_t concentric_div;  // index of the concentric division to which this points belongs to

    size_t red;    // Red component  [0-255]
    size_t green;  // Green Component[0-255]
    size_t blue;   // Blue component [0-255]

    size_t original_index;  // index of this point in the source pointcloud
  };

其中radius表示点到lidar的水平距离(半径)，即：sqrt(pow(x,2) + pow(y,2))
theta是点相对于车头正方向(即x正方向)的夹角，计算公式为 theta = arctan(y/x) * 180/pi
用radial_div 和 concentric_div 分别描述角度微分和距离微分。对点云进行水平角度微分之后，可得到: 360/0.18=2000条射线，将这些射线中的点按照距离的远近进行排序，如下所示：

/*!
 *
 * @param[in] in_cloud Input Point Cloud to be organized in radial segments
 * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
 * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
 * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
 */
void RayGroundFilter::ConvertXYZIToRTZColor(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
    const std::shared_ptr<PointCloudXYZIRTColor>& out_organized_points,
    const std::shared_ptr<std::vector<pcl::PointIndices> >& out_radial_divided_indices,
    const std::shared_ptr<std::vector<PointCloudXYZIRTColor> >& out_radial_ordered_clouds)
{
  out_organized_points->resize(in_cloud->points.size());
  out_radial_divided_indices->clear();
  out_radial_divided_indices->resize(radial_dividers_num_);
  out_radial_ordered_clouds->resize(radial_dividers_num_);

  for (size_t i = 0; i < in_cloud->points.size(); i++)
  {
    PointXYZIRTColor new_point;
    auto radius = static_cast<float>(
        sqrt(in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y));
    auto theta = static_cast<float>(atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI);
    if (theta < 0)
    {
      theta += 360;
    }
    if (theta >= 360)
    {
      theta -= 360;
    }

    auto radial_div = (size_t)floor(theta / radial_divider_angle_);

    auto concentric_div = (size_t)floor(fabs(radius / concentric_divider_distance_));

    new_point.point = in_cloud->points[i];
    new_point.radius = radius;
    new_point.theta = theta;
    new_point.radial_div = radial_div;
    new_point.concentric_div = concentric_div;
    new_point.red = (size_t)colors_[new_point.radial_div % color_num_].val[0];
    new_point.green = (size_t)colors_[new_point.radial_div % color_num_].val[1];
    new_point.blue = (size_t)colors_[new_point.radial_div % color_num_].val[2];
    new_point.original_index = i;

    out_organized_points->at(i) = new_point;

    // radial divisions
    out_radial_divided_indices->at(radial_div).indices.push_back(i);

    out_radial_ordered_clouds->at(radial_div).push_back(new_point);
  }  // end for

// order radial points on each division
#pragma omp for
  for (size_t i = 0; i < radial_dividers_num_; i++)
  {
    std::sort(out_radial_ordered_clouds->at(i).begin(), out_radial_ordered_clouds->at(i).end(),
              [](const PointXYZIRTColor& a, const PointXYZIRTColor& b) { return a.radius < b.radius; });  // NOLINT
  }
}

其中颜色的上色是根据cv::generateColors(colors_, color_num_); 计算得到的，这个函数是个非常方便的辅助函数，当我们需要生成固定数目的不同颜色时，可以考虑使用它。

0.18度是VLP32C雷达的水平光束发散间隔。

通过判断射线中前后两点的坡度是否大于我们事先设定的坡度阈值，从而判定是否为地面点。

/*!
 * Classifies Points in the PointCoud as Ground and Not Ground
 * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud ordered by radial distance from the origin
 * @param out_ground_indices Returns the indices of the points classified as ground in the original PointCloud
 * @param out_no_ground_indices Returns the indices of the points classified as not ground in the original PointCloud
 */
void RayGroundFilter::ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor>& in_radial_ordered_clouds,
                                         const pcl::PointIndices::Ptr& out_ground_indices,
                                         const pcl::PointIndices::Ptr& out_no_ground_indices)
{
  out_ground_indices->indices.clear();
  out_no_ground_indices->indices.clear();
#pragma omp for
  for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++)  // sweep through each radial division
  {
    float prev_radius = 0.f;
    float prev_height = 0.f;
    bool prev_ground = false;
    bool current_ground = false;
    for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++)  // loop through each point in the radial div
    {
      float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;
      float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;
      float current_height = in_radial_ordered_clouds[i][j].point.z;
      float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;

      // for points which are very close causing the height threshold to be tiny, set a minimum value
      if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_)
      {
        height_threshold = min_height_threshold_;
      }

      // check current point height against the LOCAL threshold (previous point)
      if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold))
      {
        // Check again using general geometry (radius from origin) if previous points wasn't ground
        if (!prev_ground)
        {
          if (current_height <= general_height_threshold && current_height >= -general_height_threshold)
          {
            current_ground = true;
          }
          else
          {
            current_ground = false;
          }
        }
        else
        {
          current_ground = true;
        }
      }
      else
      {
        // check if previous point is too far from previous one, if so classify again
        if (points_distance > reclass_distance_threshold_ &&
            (current_height <= height_threshold && current_height >= -height_threshold))
        {
          current_ground = true;
        }
        else
        {
          current_ground = false;
        }
      }

      if (current_ground)
      {
        out_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
        prev_ground = true;
      }
      else
      {
        out_no_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
        prev_ground = false;
      }

      prev_radius = in_radial_ordered_clouds[i][j].radius;
      prev_height = in_radial_ordered_clouds[i][j].point.z;
    }
  }
}

这里有两个重要参数，一个是local_max_slope_，是我们设定的同条射线上邻近两点的坡度阈值，一个是general_max_slope_，表示整个地面的坡度阈值，这两个坡度阈值的单位为度（degree），我们通过这两个坡度阈值以及当前点的半径（到lidar的水平距离）求得高度阈值，通过判断当前点的高度（即点的z值）是否在地面加减高度阈值范围内来判断当前点是为地面。

该算法的整体流程为：通过当前点和上一个点的距离计算得出高度阈值，判断当前点的z值是否之前点加减高度阈值内：如果不在，则判断是否和上一个点距离太远，如果距离太远则比较当前z点的高度和高度阈值的比较，如果在范围内则设定为地面点，如果不在则为非地面点。 如果判断当前点的z值是否之前点加减高度阈值内，若上一个点是地面点，则当前点也为地面点。如果不是，则判断当前点高度相对于全局高度差的范围内，若在则为地面点，否则为非地面点。
ring_ground_filter: 地面滤波

其中关于地面滤除算法：LeGO-LOAM中也有代码是通过将点云投影到Rang Image进行地面滤除–imageProjection.cpp。
整体思路：copyPointCloud(laserCloudMsg)->将点云信息转化为pcl格式；-> findStartEndAngle()->找到点云的起始角和终止角；->projectPointCloud()->将点云投影到Range Image;-> groundRemoval()-> 去除地面点。

    void findStartEndAngle(){
        // atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
        // segMsg.startOrientation范围为(-PI,PI]
        // segMsg.endOrientation范围为(PI,3PI]
        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);//第一个点的起始角度
        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                     laserCloudIn->points[laserCloudIn->points.size() - 2].x) + 2 * M_PI;//最后一个点的角度
		// 开始和结束的角度差一般是多少？
		// 一个velodyne 雷达数据包转过的角度多大？
		// segMsg.endOrientation - segMsg.startOrientation范围为(0,4PI)
        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
            segMsg.endOrientation -= 2 * M_PI;
        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
            segMsg.endOrientation += 2 * M_PI;
		// segMsg.orientationDiff的范围为(PI,3PI),一圈大小为2PI，应该在2PI左右
        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;//整个激光扫描的水平角度
    }

对于雷达旋转一圈，其实并不是从0度按某一方向旋转到360度，而是在起始时有一定偏差，而旋转也不是严格的360度，一般情况下要比360度稍微多几度。注意，我们的雷达坐标系为右前上（xyz），所以我们要找到点云的起始角和结束角。这里是以x轴为基准，采用的办法是使用收到的第一个点云点和结束的点云点的角度作为该帧点云的起始角和结束角。
点云重投影projectPointCloud

void projectPointCloud(){
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize; 
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();
        fullCloud->points.resize(N_SCAN*Horizon_SCAN);
        visit_flag_= std::vector<bool>(N_SCAN*Horizon_SCAN, false);
        maxhorizon_num_ = 0, minhorizon_num_ = Horizon_SCAN;
        for (size_t i = 0; i < cloudSize; ++i){

            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            thisPoint.intensity=laserCloudIn->points[i].intensity;
           //verticalAngle，垂直方向的角度
            verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
			// rowIdn计算出该点是激光雷达的第几线的， 从下往上计数，-15度记为初始线为第0线，一共16线(N_SCAN=16)
            rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;
            //horizonAngle水平方向的角度
            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

			// round函数进行四舍五入取整
			// 这边确定不是减去180度???不是
			// 雷达水平方向上某个角度和水平第几线的关联关系???关系如下：
			// horizonAngle:(-PI,PI],columnIdn:[H/4,5H/4]-->[0,H] (H:Horizon_SCAN)
			// 下面是把坐标系绕z轴旋转顺时针旋转90度，
			// x+==>3*Horizon_SCAN/4,x-==>Horizon_SCAN/4
			// y+==>Horizon_SCAN/2,y-==>0
            //水平的一圈有1800个点，分辨率为0.2
            //columnIdn为水平的哪一个点
            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;

            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < 0.1)
                continue;
            if(maxhorizon_num_ < columnIdn)maxhorizon_num_ = columnIdn;
            if(minhorizon_num_ > columnIdn)minhorizon_num_ = columnIdn;
            //rangeMat存的是，垂直上的那根线，水平上的那个点，这个位置的长度
            rangeMat.at<float>(rowIdn, columnIdn) = range;

			// columnIdn:[0,H] (H:Horizon_SCAN)==>[0,1800]
           // thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            index = columnIdn  + rowIdn * Horizon_SCAN;//index存成一行，这一整个激光的那个点
            fullCloud->points[index] = thisPoint;
            visit_flag_[index] = true;

            fullInfoCloud->points[index].intensity =  range;
        }
    }

将点云投影到16*1800的图片上，16线，水平的角分辨率是0.2，因此360/0.2=1800。Rang Image中存储点云的深度信息，将点云信息存入fullCloud和fullInfoCloud，两者的区别在于第一个中的强度信息填入了行号和列号相关的信息，而后者填入的是深度信息。
提取地面点groundRemoval：
关于groundMat的说明：
=-1：无效值，表示无法判断是否为地面点；
=0： 代表初始值，不是地面点；
=1： 表示是地面点。

关于labelMat的说明：
=-1：无效值，不进行点云分割；
=0： 初始值；
=labelCount： 平面点；
=999999： 舍弃点。

关于rangeMat的说明：
FLT_MAX： 初始化值；
range： 点云的深度值。

void groundRemoval(){
        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;

        for (size_t j = 0; j < Horizon_SCAN; ++j){
        // for (size_t j = minhorizon_num_+5; j < maxhorizon_num_-5; ++j){
            groundMat.at<int8_t>(0,j) = 1;
            for (size_t i = 1; i < 16; ++i){//groundScanInd=7 只看下面的7根线
           
                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i-1)*Horizon_SCAN;
                if(j == 0 || j == Horizon_SCAN - 1)
                {
                    groundMat.at<int8_t>(i,j) = 1;
                    continue;
                }
           
                if(!visit_flag_[lowerInd] || !visit_flag_[upperInd])
                {
                    groundMat.at<int8_t>(i,j) = 1;
                    continue;
                }
         
				// 由上下两线之间点的XYZ位置得到两线之间的俯仰角
				// 如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1
				// 否则，则不是地面点，进行后续操作
                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

                
                if(fabs(diffX) > 1 || fabs(diffY) > 1)
                {
                    groundMat.at<int8_t>(i,j) = 1;
                    continue;
                }
                if(fabs(fullCloud->points[upperInd].x) > 12 || fabs(fullCloud->points[upperInd].x) > 12)
                {
                    groundMat.at<int8_t>(i,j) = 1;
                    continue;
                }
                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

    
                if (abs(angle) <= segment_dis_){//10度以内认为是地面点
                //ROS_INFO("I=%d",i);
                    groundMat.at<int8_t>(i,j) = 1;//不仅仅把当前点认为是地面点，还会把当前点下一根线上的点也认为是地面点
                    // groundMat.at<int8_t>(i+1,j) = 1;
               }
               else
               if(isnan(angle))
                   { //ROS_INFO("angle=%f",angle);
                         groundMat.at<int8_t>(i,j) = 1;//不仅仅把当前点认为是地面点，还会把当前点下一根线上的点也认为是地面点
                    groundMat.at<int8_t>(i+1,j) = 1; 
                   }else if(i==0)
                   {
                    groundMat.at<int8_t>(i,j) = 1;//不仅仅把当前点认为是地面点，还会把当前点下一根线上的点也认为是地面点
                    groundMat.at<int8_t>(i+1,j) = 1; 
                   }else if(i==14)
                   {
                    groundMat.at<int8_t>(i,j) = 1;//不仅仅把当前点认为是地面点，还会把当前点下一根线上的点也认为是地面点
                    groundMat.at<int8_t>(i+1,j) = 1;
                   }else
                   {
                      //ROS_INFO("angle=%f",angle);
                   }
            }
            int obs_num = 0;
            int obs_line_num = -1;
            for (size_t i = 0; i < 16; ++i)
            {
                if(groundMat.at<int8_t>(i,j) != 1)
                {
                    obs_num ++;
                    obs_line_num = i;
                }
            }
            if(obs_num == 1 && obs_line_num < 5)
            {
                for (size_t i = 0; i < 16; ++i)
                {
                    groundMat.at<int8_t>(i,j) = 1;
                }
            }
        }

		// 找到所有点中的地面点，并在labelMat将他们标记为-1
		// rangeMat[i][j]==FLT_MAX??? 判定为地面点的另一个条件
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
                    labelMat.at<int>(i,j) = -1;
                }
            }
        }
		
        if (pubGroundCloud.getNumSubscribers() != 0){	// 如果有节点订阅groundCloud，那么就需要把地面点发布出来
            for (size_t i = 0; i <= groundScanInd; ++i){
                for (size_t j = 0; j < Horizon_SCAN; ++j){
                    if (groundMat.at<int8_t>(i,j) == 1)
                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);// 具体实现过程：把点放到groundCloud队列中去
                }
            }
        }
    }

在提取地面点之前首先需要明确的一点，在激光雷达安装时是水平的，一般认为最中间的激光线束与水平面平行，所以对于一个16线的激光雷达而言，地面点在前7束激光线之中提取。即groundScanInd=7，遍历前7束所有点云：
对于地面点的判定：对于i 束中的激光点，在i + 1 束找到对应点，这两点与雷达的连线所形成的向量的差向量如下图如所示，若该差向量与水平面所形成的夹角小于10度，则认为是一个地面点。

还有一种常见的地面滤除算法：GPF
1.对于每个点云数据段，会先提取一组具有低高度值的种子点，然后用这些种子点来估计出一个地面的初始模型。

2.根据估计出的平面模型，点云段P中的每个点都要参与计算，计算从该点到其在候选平面上的正交投影的距离（简单理解成垂线段的长度）。
3.将该距离与用户定义的阈值dist 进行比较，该阈值决定该点是否属于地面。
4.属于地面的点被用作新模型的种子，该过程重复N次。
5.最后，由该算法产生的每个点云段的地面点连接起来，就是整个地面。

我们选择初始种子点的方法叫做最低点代表（LPR），该点定义为点云的N个最低高度值点的平均值。LPR保证了噪声不会影响平面估计结果。一旦计算出了LPR，则将其视为点云P的最低高度值点，并且高度阈值Thseeds内的点会成为初始种子点，用作后续的平面模型估计。

space_filter: 点云裁切 通过y方向和z方向的限定值进行裁减
另外地面滤除还有patchwork++算法 等后续更新。