Apollo 6.0 perception 瞎学笔记（二）hdmap_roi_filter

目录

上篇博客作者瞎学了bitmap2d.cc，有那种它要干什么的感觉，但是并不清楚每一步的操作含义是什么，暂时先将那篇博文放着。我们继续来看关于地图bitmap2d的上一层部分：hdmap_roi_filter

hdmap_roi_filter.cc

打开/apollo-r6.0.0/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

初始化Init方法

bool HdmapROIFilter::Init(const ROIFilterInitOptions& options) {
  // load model config
  auto config_manager = lib::ConfigManager::Instance();
  const lib::ModelConfig* model_config = nullptr;
  ACHECK(config_manager->GetModelConfig(Name(), &model_config));
  const std::string work_root = config_manager->work_root();
  std::string config_file;
  std::string root_path;
  ACHECK(model_config->get_value("root_path", &root_path));
  config_file = GetAbsolutePath(work_root, root_path);
  config_file = GetAbsolutePath(config_file, "hdmap_roi_filter.conf");
  HDMapRoiFilterConfig config;
  ACHECK(apollo::cyber::common::GetProtoFromFile(config_file, &config));
  range_ = config.range();
  cell_size_ = config.cell_size();
  extend_dist_ = config.extend_dist();
  no_edge_table_ = config.no_edge_table();
  set_roi_service_ = config.set_roi_service();

  // reserve mem
  const size_t KPolygonMaxNum = 100;
  polygons_world_.reserve(KPolygonMaxNum);
  polygons_local_.reserve(KPolygonMaxNum);

  // init bitmap
  Eigen::Vector2d min_range(-range_, -range_);
  Eigen::Vector2d max_range(range_, range_);
  Eigen::Vector2d cell_size(cell_size_, cell_size_);
  bitmap_.Init(min_range, max_range, cell_size);

  // output input parameters
  AINFO << " HDMap Roi Filter Parameters: "
        << " range: " << range_ << " cell_size: " << cell_size_
        << " extend_dist: " << extend_dist_
        << " no_edge_table: " << no_edge_table_
        << " set_roi_service: " << set_roi_service_;
  return true;
}

函数传入ROIFilterInitOptions类型的options，但是函数内并没有关于options的操作。

• 实例化一个ConfigManager类型的对象
• 声明一个ModelConfig类型的空指针
• ACheck检查模型文件
• 通过config_manager获取根路径
• ACheck检查根路径
• 通过GetAbsolutePath获取路径
• 加上配置文件的名称hdmap_roi_filter.conf来获得具体路径
• 声明一个HDMapRoiFilterConfig类型的变量
• ACheck检测能否获得模型文件
• 将配置文件的参数都赋值给全局变量
• 将两个全局变量vector分别调整为100大小
• 设定好地图的尺寸参数，调用bitmap_.Init()方法来初始化地图
• 记录log

Filter方法

bool HdmapROIFilter::Filter(const ROIFilterOptions& options,
                            LidarFrame* frame) {
  if (frame->hdmap_struct == nullptr || frame->cloud == nullptr) {
    AERROR << " Input frame data error !";
    return false;
  }

  // get map polygon of roi
  auto& road_polygons = frame->hdmap_struct->road_polygons;
  auto& junction_polygons = frame->hdmap_struct->junction_polygons;
  size_t polygons_world_size = road_polygons.size() + junction_polygons.size();
  if (0 == polygons_world_size) {
    AINFO << " Polygon Empty.";
    return false;
  }

  polygons_world_.clear();
  polygons_world_.resize(polygons_world_size, nullptr);
  size_t i = 0;
  for (auto& polygon : road_polygons) {
    polygons_world_[i++] = &polygon;
  }
  for (auto& polygon : junction_polygons) {
    polygons_world_[i++] = &polygon;
  }

  // transform to local
  base::PointFCloudPtr cloud_local = base::PointFCloudPool::Instance().Get();
  TransformFrame(frame->cloud, frame->lidar2world_pose, polygons_world_,
                 &polygons_local_, &cloud_local);

  bool ret = FilterWithPolygonMask(cloud_local, polygons_local_,
                                   &(frame->roi_indices));

  // set roi points label
  if (ret) {
    for (auto index : frame->roi_indices.indices) {
      frame->cloud->mutable_points_label()->at(index) =
          static_cast<uint8_t>(LidarPointLabel::ROI);
      frame->world_cloud->mutable_points_label()->at(index) =
          static_cast<uint8_t>(LidarPointLabel::ROI);
    }
  }

  // set roi service
  if (set_roi_service_) {
    auto roi_service = SceneManager::Instance().Service("ROIService");
    if (roi_service != nullptr) {
      roi_service_content_.range_ = range_;
      roi_service_content_.cell_size_ = cell_size_;
      roi_service_content_.map_size_ = bitmap_.map_size();
      roi_service_content_.bitmap_ = bitmap_.bitmap();
      roi_service_content_.major_dir_ =
          static_cast<ROIServiceContent::DirectionMajor>(bitmap_.dir_major());
      roi_service_content_.transform_ = frame->lidar2world_pose.translation();
      if (!ret) {
        std::fill(roi_service_content_.bitmap_.begin(),
                  roi_service_content_.bitmap_.end(), -1);
      }
      roi_service->UpdateServiceContent(roi_service_content_);
    } else {
      AINFO << "Failed to find roi service and cannot update.";
    }
  }
  return ret;
}

这里入口参数，有一个LidarFrame类型的变量，我们下一讲细致看下

• 如果雷达和地图输入都是空，直接返回false
• 拿到road_polygons 和 junction_polygons 判断两者数量是不是大于0，即拿到了地图边界
• 将polygon_world_清空，然后将road_polygons和junction_polygons都存入到polygons_world_里面
• 拿到点云数据，通过雷达在世界中的位姿将雷达数据转换到世界坐标系
• 通过FilterWithPolygonMask方法来得到Roi_indices,即roi区域内的索引
• 如果可以拿到索引的话，分别改变点云和世界点云的标签为roi点云
• 下面的部分是apollo重写rosservice的形式，将bitmap的相关参数传出去

FilterWithPolygonMask方法

bool HdmapROIFilter::FilterWithPolygonMask(
    const base::PointFCloudPtr& cloud,
    const std::vector<PolygonDType>& map_polygons,
    base::PointIndices* roi_indices) {
  std::vector<Polygon<double>> raw_polygons;
  // convert and obtain the major direction
  raw_polygons.resize(map_polygons.size());
  double min_x = range_;
  double max_x = -min_x;
  double min_y = min_x;
  double max_y = max_x;

  for (size_t i = 0; i < map_polygons.size(); ++i) {
    auto& raw_polygon = raw_polygons[i];
    const auto& map_polygon = map_polygons[i];
    raw_polygon.resize(map_polygon.size());
    for (size_t j = 0; j < map_polygon.size(); ++j) {
      raw_polygon[j].x() = map_polygon[j].x;
      raw_polygon[j].y() = map_polygon[j].y;
      min_x = std::min(raw_polygon[j].x(), min_x);
      max_x = std::max(raw_polygon[j].x(), max_x);
      min_y = std::min(raw_polygon[j].y(), min_y);
      max_y = std::max(raw_polygon[j].y(), max_y);
    }
  }
  min_x = std::max(min_x, -range_);
  max_x = std::min(max_x, range_);
  min_y = std::max(min_y, -range_);
  max_y = std::min(max_y, range_);

  DirectionMajor major_dir = DirectionMajor::XMAJOR;
  if ((max_y - min_y) < (max_x - min_x)) {
    major_dir = DirectionMajor::YMAJOR;
  }
  bitmap_.SetUp(major_dir);

  return DrawPolygonsMask<double>(raw_polygons, &bitmap_, extend_dist_,
                                  no_edge_table_) &&
         Bitmap2dFilter(cloud, bitmap_, roi_indices);
}

上面Filter方法在其中调用了FilterWithPolygonMask方法，我们来看一下：

• 声明一个vector，类型是Polygon(double)
• 调整raw_polygons和map_polygons为一样的尺寸
• 声明并且初始化了局部最大最小值
• 遍历map_polygons
  • 拿到原始的raw_polygon 和 map_polygon元素
  • 调整raw_polygon的大小，是的raw_polygon的尺寸和map_polygons里面每个元素里的尺寸一样大小
  • 遍历map_polygon里面的点
    • 将map_polygon里面的顶点x，y传给raw_polygon，比较并更新最小最大的x，y值
• 将最大和最小值和边界值比较，取得较内的边界值
• XMAJOR值赋值给major_dir
• 如果Y方向变化比X方向变化值大，则Y方向为主方向。并且调用Setup方法设立为主方向
• 最后调用DrawPolygonsMask 和 Bitmap2dFilter方法，如果两者都是成功，返回true

TransformFrame方法

这里是transformFrame方法，将世界坐标系下的点云和polygon根据雷达在世界坐标系下的位姿转换到本地

void HdmapROIFilter::TransformFrame(
    const base::PointFCloudPtr& cloud, const Eigen::Affine3d& vel_pose,
    const std::vector<PolygonDType*>& polygons_world,
    std::vector<PolygonDType>* polygons_local,
    base::PointFCloudPtr* cloud_local) {
  Eigen::Vector3d vel_location = vel_pose.translation();
  Eigen::Matrix3d vel_rot = vel_pose.linear();
  Eigen::Vector3d x_axis = vel_rot.row(0);
  Eigen::Vector3d y_axis = vel_rot.row(1);

  // transform polygons
  polygons_local->clear();
  polygons_local->resize(polygons_world.size());
  for (size_t i = 0; i < polygons_local->size(); ++i) {
    const auto& polygon_world = *(polygons_world[i]);
    auto& polygon_local = (*polygons_local)[i];
    polygon_local.resize(polygon_world.size());
    for (size_t j = 0; j < polygon_local.size(); ++j) {
      polygon_local[j].x = polygon_world[j].x - vel_location.x();
      polygon_local[j].y = polygon_world[j].y - vel_location.y();
    }
  }

  // transform cloud
  (*cloud_local)->clear();
  (*cloud_local)->resize(cloud->size());
  for (size_t i = 0; i < (*cloud_local)->size(); ++i) {
    const auto& pt = cloud->at(i);
    auto& local_pt = (*cloud_local)->at(i);
    Eigen::Vector3d e_pt(pt.x, pt.y, pt.z);
    local_pt.x = static_cast<float>(x_axis.dot(e_pt));
    local_pt.y = static_cast<float>(y_axis.dot(e_pt));
  }
}

• 获得vel_pose的translation部分，赋值给vel_location
• 获取vel_pose的linear部分即旋转矩阵，赋值给vel_rot
• 第0行数据赋值给x_axis
• 第1行数据赋值给y_axis
• 将polygons_local清空，并且调整polygons_local大小和polygons_world一样
• 遍历polygons_local里的元素
  • 将polygon_world里面每一个点减去相应的x()和y()，得到局部的x()和y()，赋值给polygon_local，因此世界和局部之间的polygon就转换好了
• 将cloud_local清空
• 遍历每一个局部点云的坐标
  • 将函数传入的cloud的每一个点的坐标直接乘以相应的变换，得到局部坐标值

Bitmap2dFilter方法

输入点云和bitmap，输出roi点云的索引

bool HdmapROIFilter::Bitmap2dFilter(const base::PointFCloudPtr& in_cloud,
                                    const Bitmap2D& bitmap,
                                    base::PointIndices* roi_indices) {
  if (!bitmap.Check(Eigen::Vector2d(0.0, 0.0))) {
    AWARN << " Car is not in roi!!.";
    return false;
  }
  roi_indices->indices.clear();
  roi_indices->indices.reserve(in_cloud->size());
  for (size_t i = 0; i < in_cloud->size(); ++i) {
    const auto& pt = in_cloud->at(i);
    Eigen::Vector2d e_pt(pt.x, pt.y);
    if (!bitmap.IsExists(e_pt)) {
      continue;
    }
    if (bitmap.Check(e_pt)) {
      roi_indices->indices.push_back(static_cast<int>(i));
    }
  }
  return true;
}

• 如果check零点返回false，则认为车不在roi内，这里有点让人匪夷所思
• 清空roi_indices索引，调整roi_indices尺寸和输入量一样
• 遍历输入点云
  • 判断点是不是在地图内，如果不在则跳过
  • 如果此张地图可以Check该点，将该点的索引压入到索引内

因此这里主要还是调用的bitmap的底层接口来判断是否在roi区域内。

总结

这里主要是写好了Filter接口，用于滤除找到ROI区域内的点云索引点。