【apollo7.0】感知模块(2): 基于激光雷达的障碍物感知算法及流程

【apollo7.0】感知模块(2): 基于激光雷达的障碍物感知算法及流程

Step 1 DetectionComponent::Proc()

该函数位于detection_component.cc，是一个回调函数，输入为驱动传来的点云信息，然后调用InternalProc()处理每一帧点云，获得处理后的结果，发布结果信息。

bool DetectionComponent::Proc(
    const std::shared_ptr<drivers::PointCloud>& message) {
  // 回调函数
  // 传入参数为驱动传来的点云数据
  AINFO << std::setprecision(16)
        << "Enter detection component, message timestamp: "
        << message->measurement_time()
        << " current timestamp: " << Clock::NowInSeconds();
  // 创建输出信息
  auto out_message = std::make_shared<LidarFrameMessage>();
  // 处理每帧点云
  bool status = InternalProc(message, out_message);
  if (status) {
    // 发布点云经过detector处理后的消息
    writer_->Write(out_message);
    AINFO << "Send lidar detect output message.";
  }
  return status;
}

Step 2 DetectionComponent::InternalProc(）

该函数位于detection_component.cc，主要用来构建LidarFrame，同时计算点云转世界坐标系的姿态，然后调用具体的检测方法对点云进行障碍物感知。

bool DetectionComponent::InternalProc(
    const std::shared_ptr<const drivers::PointCloud>& in_message,
    const std::shared_ptr<LidarFrameMessage>& out_message) {
  uint32_t seq_num = seq_num_.fetch_add(1);
  const double timestamp = in_message->measurement_time();
  const double cur_time = Clock::NowInSeconds();
  const double start_latency = (cur_time - timestamp) * 1e3;
  AINFO << std::setprecision(16) << "FRAME_STATISTICS:Lidar:Start:msg_time["
        << timestamp << "]:sensor[" << sensor_name_ << "]:cur_time[" << cur_time
        << "]:cur_latency[" << start_latency << "]";
  // 确定当前消息的时间戳
  out_message->timestamp_ = timestamp;
  // 确定当前点云的时间戳
  out_message->lidar_timestamp_ = in_message->header().lidar_timestamp();
  // 处理帧的序号
  out_message->seq_num_ = seq_num;
  out_message->process_stage_ = ProcessStage::LIDAR_DETECTION;
  out_message->error_code_ = apollo::common::ErrorCode::OK;

  // 获取lidar_frame的引用
  auto& frame = out_message->lidar_frame_;
  frame = lidar::LidarFramePool::Instance().Get();
  frame->cloud = base::PointFCloudPool::Instance().Get();
  frame->timestamp = timestamp;
  // 传感器信息，来源于配置文件
  frame->sensor_info = sensor_info_;

  Eigen::Affine3d pose = Eigen::Affine3d::Identity();
  Eigen::Affine3d pose_novatel = Eigen::Affine3d::Identity();

  // 根据时间戳，确定pose及pose_novatel，分别指的是传感器到world的姿态，及组合导航到world的姿态
  const double lidar_query_tf_timestamp =
      timestamp - lidar_query_tf_offset_ * 0.001;
  if (!lidar2world_trans_.GetSensor2worldTrans(lidar_query_tf_timestamp, &pose,
                                               &pose_novatel)) {
    out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;
    AERROR << "Failed to get pose at time : " << lidar_query_tf_timestamp;
    return false;
  }

  // 确定雷达转world的位姿
  frame->lidar2world_pose = pose;
  // 确定组合导航转world的位姿。不过，通常novatel被定为世界坐标系。
  frame->novatel2world_pose = pose_novatel;

  // 创建检测相关的属性器，如传感器名称
  lidar::LidarObstacleDetectionOptions detect_opts;
  detect_opts.sensor_name = sensor_name_;
  // 给属性器添加传感器到组合导航的外参
  lidar2world_trans_.GetExtrinsics(&detect_opts.sensor2novatel_extrinsics);

  // 点云处理入口
  lidar::LidarProcessResult ret =
      detector_->Process(detect_opts, in_message, frame.get());

  if (ret.error_code != lidar::LidarErrorCode::Succeed) {
    out_message->error_code_ =
        apollo::common::ErrorCode::PERCEPTION_ERROR_PROCESS;
    AERROR << "Lidar detection process error, " << ret.log;
    return false;
  }

  return true;
}

Step 3 LidarObstacleDetection::Process()

该函数位于lidar_obstacle_detection.cc，主要对点云进行预处理，然后调用ProcessCommon()对每帧点云进行处理。该函数比较简单，代码不再显示，具体说下点云的预处理。
点云的预处理是对原始点云进行检查，去除nan值及其他限制的无效值；根据lidar2world的姿态，确定世界坐标系下的点云。

bool PointCloudPreprocessor::Preprocess(
    const PointCloudPreprocessorOptions& options,
    const std::shared_ptr<apollo::drivers::PointCloud const>& message,
    LidarFrame* frame) const 
{
  // 点云预处理
....省略
  // 设置点云时间戳
  frame->cloud->set_timestamp(message->measurement_time());
  // 将原始点云拷贝到frame中，同时计算world_cloud
  if (message->point_size() > 0) {
    frame->cloud->reserve(message->point_size());
    base::PointF point;
    for (int i = 0; i < message->point_size(); ++i) {
      const apollo::drivers::PointXYZIT& pt = message->point(i);
      // 去除nan值及无效值
      if (filter_naninf_points_) {
        if (std::isnan(pt.x()) || std::isnan(pt.y()) || std::isnan(pt.z())) {
          continue;
        }
        // kPointInfThreshold=1000，作最大值限制
        if (fabs(pt.x()) > kPointInfThreshold ||
            fabs(pt.y()) > kPointInfThreshold ||
            fabs(pt.z()) > kPointInfThreshold) {
          continue;
        }
      }
      // 点云原始数值
      Eigen::Vector3d vec3d_lidar(pt.x(), pt.y(), pt.z());
      // 点云到novatel坐标系的转换
      Eigen::Vector3d vec3d_novatel =
          options.sensor2novatel_extrinsics * vec3d_lidar;
      // 根据配置文件的设置进行过滤，在一个box范围内进行去除。通常设置为false
      if (filter_nearby_box_points_ && vec3d_novatel[0] < box_forward_x_ &&
          vec3d_novatel[0] > box_backward_x_ &&
          vec3d_novatel[1] < box_forward_y_ &&
          vec3d_novatel[1] > box_backward_y_) {
        continue;
      }
      // 高度限制
      if (filter_high_z_points_ && pt.z() > z_threshold_) {
        continue;
      }
      // 将原始点添加到frame中的点云中
      point.x = pt.x();
      point.y = pt.y();
      point.z = pt.z();
      point.intensity = static_cast<float>(pt.intensity());
      frame->cloud->push_back(point, static_cast<double>(pt.timestamp()) * 1e-9,
                              std::numeric_limits<float>::max(), i, 0);
    }
    // 根据lidar2world_pose，将cloud转换为world_cloud
    TransformCloud(frame->cloud, frame->lidar2world_pose, frame->world_cloud);
  }
  return true;
}

Step 4 LidarObstacleDetection::ProcessCommon()

该函数是lidar障碍物检测的主函数，会对高精地图进行解析，调用算法对点云进行处理，获得感知结果，然后再执行object build以及object filter。
object build以及object filter暂未详细看，后续再说。
解析高精地图是根据当前车辆的定位，以一定的半径，从高精地图中提取道路、交叉路口的信息，用于后续roi的提取。提取的道路、交叉路口的信息是以多边形的结构保存的。

bool MapManager::Update(const MapManagerOptions& options, LidarFrame* frame) {
 // check input
 ......省略
  //获取当前定位
  base::PointD point;
  point.x = frame->lidar2world_pose.translation()(0);
  point.y = frame->lidar2world_pose.translation()(1);
  point.z = frame->lidar2world_pose.translation()(2);
  // 获取高精地图中的道路信息，与当前定位的距离为roi_search_distance_
  // 这里是依据apollo的高精地图的格式来获取的，不同地图格式应当有不同的实现方法。
  if (!hdmap_input_->GetRoiHDMapStruct(point, roi_search_distance_,
                                       frame->hdmap_struct)) {
    frame->hdmap_struct->road_polygons.clear();//道路的多边形信息
    frame->hdmap_struct->road_boundary.clear();//道路边界
    frame->hdmap_struct->hole_polygons.clear();//洞的多边形？
    frame->hdmap_struct->junction_polygons.clear();//交叉路口的多边形
    AINFO << "Failed to get roi from hdmap.";
  }
  return true;
}

Step 5 NCutSegmentation

点云检测器以NCutSegmentation为例。
NCutSegmentation是一种基于聚类分析的点云分割方法。
下面对该算法的Detect()函数进行逐步说明。
该算法为了提升检测效率，使用了omp多线程技术。

// 设置omp并行计算的线程数，omp_threads_是修改添加的。
omp_set_num_threads(omp_threads_);
#pragma omp parallel
  { num_threads = omp_get_num_threads(); }

关键步骤1：roi的提取与地面的去除
根据前面高精地图提取的道路与路口信息，对点云进行roi提取，只关心那些位于道路及路口区域的点云，再根据点云信息，对地面进行拟合计算，然后再点云中去除地面，从而只保留了roi中的障碍物点云，此法可以对原始点云进行精减。

// 利用另外一个线程实现roi的提取与地面的去除
  if (remove_roi_) {
    AINFO << "remove roi and remove ground for ncut segmentation";
    worker_.WakeUp();
    worker_.Join();
  }

代码中写的提示是remove roi and remove ground，实际应该是extract roi and remove ground。
这里采用另一个线程来处理该项任务，因此需要先找到该线程内定义的任务内容，其位于init()函数中。

worker_.Bind([&]() {
    ROIFilterOptions roi_filter_options;
    // 根据高精地图的道路与交叉路口信息，提取点云中的ROI 
    // 如果没有高精地图，则无法提取ROI
    // ROI的表示是以点云索引来的
    if (lidar_frame_ref_->hdmap_struct != nullptr &&
        roi_filter_->Filter(roi_filter_options, lidar_frame_ref_)) 
    {
        // 拷贝点云
        roi_cloud_->CopyPointCloud(*lidar_frame_ref_->cloud,
                                 lidar_frame_ref_->roi_indices);
        roi_world_cloud_->CopyPointCloud(*lidar_frame_ref_->world_cloud,
                                       lidar_frame_ref_->roi_indices);
    } 
    else ......省略
    // 将roi的点云赋给frame
    lidar_frame_ref_->cloud = roi_cloud_;
    lidar_frame_ref_->world_cloud = roi_world_cloud_;
    ......省略
    // 进行地面检测与去除
    GroundDetectorOptions ground_detector_options;
    ground_detector_->Detect(ground_detector_options, lidar_frame_ref_);
    return true;
  });
  worker_.Start();

地面检测采用spatio_temporal_ground_detector，该算法确定了点云中不为地面的点的索引。
其主要代码为

bool SpatioTemporalGroundDetector::Detect(const GroundDetectorOptions& options,
                                          LidarFrame* frame) {
  // check input
  ......省略
  // 获取点云中心坐标
  cloud_center_(0) = frame->lidar2world_pose(0, 3);
  cloud_center_(1) = frame->lidar2world_pose(1, 3);
  cloud_center_(2) = frame->lidar2world_pose(2, 3);

  // check output
  frame->non_ground_indices.indices.clear();

  if (frame->roi_indices.indices.empty()) {
    use_roi_ = false;
  }
  // 如果使用了roi，则直接对roi中点进行地面检测，反之采用所有点
  if (use_roi_) {
    num_points = frame->roi_indices.indices.size();
  } else {
    num_points = frame->world_cloud->size();
  }

  ADEBUG << "spatial temporal seg: use roi " << use_roi_ << " num points "
         << num_points;

  // reallocate memory if points num > the preallocated size
  num_points_all = num_points;
  // 提前开辟内存空间
  if (num_points_all > default_point_size_) {
    default_point_size_ = num_points * 2;
    point_indices_temp_.resize(default_point_size_);
    data_.resize(default_point_size_ * 3);
    ground_height_signed_.resize(default_point_size_);
  }

  // copy point data, filtering lower points under ground
  if (use_roi_) {
    for (i = 0; i < num_points; ++i) {
      index = frame->roi_indices.indices[i];
      const auto& pt = frame->world_cloud->at(index);
      point_indices_temp_[valid_point_num++] = index;
      data_[data_id++] = static_cast<float>(pt.x - cloud_center_(0));
      data_[data_id++] = static_cast<float>(pt.y - cloud_center_(1));
      data_[data_id++] = static_cast<float>(pt.z - cloud_center_(2));
    }
  } else {
    for (i = 0; i < num_points; ++i) {
      const auto& pt = frame->world_cloud->at(i);
      point_indices_temp_[valid_point_num++] = static_cast<int>(i);
      data_[data_id++] = static_cast<float>(pt.x - cloud_center_(0));
      data_[data_id++] = static_cast<float>(pt.y - cloud_center_(1));
      data_[data_id++] = static_cast<float>(pt.z - cloud_center_(2));
    }
  }

  valid_point_num_cur = valid_point_num;

  CHECK_EQ(data_id, valid_point_num * 3);
  base::PointIndices& non_ground_indices = frame->non_ground_indices;
  ADEBUG << "input of ground detector:" << valid_point_num;

  // 点云平面检测同时记录每个点到地面的距离
  if (!pfdetector_->Detect(data_.data(), ground_height_signed_.data(),
                           valid_point_num, nr_points_element)) {
    ADEBUG << "failed to call ground detector!";
    non_ground_indices.indices.insert(
        non_ground_indices.indices.end(), point_indices_temp_.begin(),
        point_indices_temp_.begin() + valid_point_num);
    return false;
  }
  // 给点云重新赋值去除地面后的高度
  for (i = 0; i < valid_point_num_cur; ++i) {
    z_distance = ground_height_signed_.data()[i];
    frame->cloud->mutable_points_height()->at(point_indices_temp_[i]) =
        z_distance;
    frame->world_cloud->mutable_points_height()->at(point_indices_temp_[i]) =
        z_distance;
    // 设置一个高度阈值，大于该阈值视为有效点，反之，视为地面，标记为GROUND
    if (common::IAbs(z_distance) > ground_thres_) {
      non_ground_indices.indices.push_back(point_indices_temp_[i]);
    } else {
      frame->cloud->mutable_points_label()->at(point_indices_temp_[i]) =
          static_cast<uint8_t>(LidarPointLabel::GROUND);
      frame->world_cloud->mutable_points_label()->at(point_indices_temp_[i]) =
          static_cast<uint8_t>(LidarPointLabel::GROUND);
    }
  }
 ......省略
  return true;
}

再继续看detect()函数，roi提取与地面检测后，需要拷贝新的点云。因为lidar_frame_ref_->secondary_indices并未在哪个环节中使用，因此为了检测到物体，必须使remove_ground_为true。
lidar_frame_ref_->non_ground_indices为上面地面检测中，确定的不为地面的点的索引。

// 根据是否进行地面去除拷贝点云！
  // 这里为了加快检测，加入了距离变量distance_x_,distance_y_
  if (remove_ground_) {
    AINFO << "remove ground";
    cloud_above_ground->CopyPointCloud(*lidar_frame_ref_->cloud,
                                       lidar_frame_ref_->non_ground_indices,
                                       distance_x_, distance_y_);
  } else {
    // if used as secondary segmentor, got from cloud directly
    cloud_above_ground->CopyPointCloud(*lidar_frame_ref_->cloud,
                                       lidar_frame_ref_->secondary_indices,
                                       distance_x_, distance_y_);
  }

关键步骤2：车辆与行人的检测
这一步是由ObstacleFilter()函数来实现的，通过不同类不同的滤波窗口，对车辆与行人作检测。但代码中对于类别的区分是有误的，无论什么类别均会输出—unkown，其分类程序在lr_classifier.h中，是通过尺寸进行的简单分类，但计算结果一直为0，输出的type对应的正是unkown，因此ObstacleFilter()并未发挥作用，可以注释掉。可能是apollo的开发者没有完善该处。

// .3 filter vehicle
  // 检测车辆并进行滤波
  base::PointFCloudPtr cloud_after_car_filter;
  ObstacleFilter(cloud_above_ground, vehicle_filter_cell_size_, false,
                 &cloud_after_car_filter, segments);
  
  // .4 filter pedestrian
  // 检测行人并进行滤波
  base::PointFCloudPtr cloud_after_people_filter;
  ObstacleFilter(cloud_after_car_filter, pedestrian_filter_cell_size_, true,
                  &cloud_after_people_filter, segments);

关键步骤3：点云聚类分割
因关键步骤2没有发挥作用，这里直接利用去除地面后的点云进行分割。采用的算法为FloodFill—洪水覆盖法。获得的结果是一个个划分区域后的点云集合。

std::vector<base::PointFCloudPtr> cloud_components;
  PartitionConnectedComponents(cloud_above_ground, partition_cell_size_,
                               &cloud_components);

void NCutSegmentation::PartitionConnectedComponents(
    const base::PointFCloudPtr& in_cloud, float cell_size,
    std::vector<base::PointFCloudPtr>* out_clouds) {
  std::vector<base::PointFCloudPtr>& temp_clouds = *out_clouds;
  // 创建洪水覆盖法
  FloodFill FFfilter(grid_radius_, cell_size);
  // 每个区域包含点的索引
  std::vector<std::vector<int>> component_points;
  // 每个区域包含cell的数目
  std::vector<int> num_cells_per_components;
  // 利用洪水覆盖法对点云进行连通区域的分割
  FFfilter.GetSegments(in_cloud, &component_points, &num_cells_per_components);
  temp_clouds.resize(component_points.size());
  for (size_t i = 0; i < component_points.size(); ++i) {
    temp_clouds[i] = base::PointFCloudPtr(
        new base::PointFCloud(*in_cloud, component_points[i]));
  }
}

关键步骤4：NCut
对分割后的点云集合，执行NCut算法。这里程序在前面还进行了外点的去除，用于NCut算法的是有效分割区域。

// cloud_tbd表示有效分割部分的索引
    // 对cloud_components的有效分割部分进行检测
    for (size_t i = 0; i < cloud_tbd.size(); ++i) {
      // 对分割后的点云执行NCut算法
      my_ncut->Segment(cloud_components[cloud_tbd[i]]);
      ADEBUG << "after segment with num segments" << my_ncut->NumSegments();
      for (int j = 0; j < my_ncut->NumSegments(); ++j) {
        base::PointFCloudPtr pc = my_ncut->GetSegmentPointCloud(j);
        // 这里的Label计算是有问题的，均是输出unknown
        std::string label = my_ncut->GetSegmentLabel(j);
        if (IsOutlier(pc)) {
          my_outlier_pcs.push_back(pc);
        } else {
          my_segment_pcs.push_back(pc);
          my_segment_labels.push_back(label);
        }
      }
    }

最后，确定检测结果。

#pragma omp parallel for
  for (int i = 0; i < num_threads; ++i) {
    int offset = segment_offset[i];
    for (size_t j = 0; j < threads_segment_pcs[i].size(); ++j) {
      // 输出点云与标签
      base::ObjectPtr& obj_ptr = (*segments)[offset + j];
      obj_ptr.reset(new base::Object());
      obj_ptr->lidar_supplement.cloud = *threads_segment_pcs[i][j];

      if (do_classification_) {
        obj_ptr->type = Label2Type(threads_segment_labels[i][j]);
      }
    }
  }

检测结果