Autoware感知瞎学笔记（二）roi_object_filter

目录

无人驾驶中，我们在对激光雷达数据进行聚类得到车身周边的目标，但是很多场景下我们其实并不关心道路以外（比如建筑物）的目标信息，所以在我们聚类前就应该想办法滤除掉额外点云。因此在雷达数据预处理的时候，最好是从高精地图拿到道路信息，将我们不关心的区域过滤掉。正好最近学习autoware，我们看下autoware是怎么做的。

代码分析：

打开autoware/core_perception/launch/lidar_euclidean_cluster_detect.launch:
可以看到，在这个欧式聚类节点的launch文件中，如果使用vector_map(autoware里的“高精地图”)，则会启动一个叫做roi_object_filter的包，因此我们可以大胆猜测一下，这个应该是融合高精地图信息做的，不然它设立这个标志位干什么=、=。

 <group if="$(arg use_vector_map)">
    <node name="object_roi_filter_clustering" pkg="roi_object_filter" type="roi_object_filter"
          output="screen" ns="/detection/lidar_detector">
      <param name="objects_src_topic" value="/objects"/>
      <param name="wayarea_gridmap_layer" value="$(arg wayarea_gridmap_layer)"/>
      <param name="sync_topics" value="false"/>
      <param name="exception_list" value="[person, bicycle]"/>
    </node>
    <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_01"
          output="screen" ns="/detection/lidar_detector">
      <param name="objects_src_topic" value="/objects_filtered"/>
    </node>
  </group>
  <group unless="$(arg use_vector_map)">
    <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_01"
          output="screen" ns="/detection/lidar_detector">
      <param name="objects_src_topic" value="/objects"/>
    </node>
  </group>

1. roi_object_filter_node.cpp

找到core_perception下面的roi_object_filter包，并且打开roi_object_filter_node.cpp。这里便是节点的入口，RosRoiObjectFilterApp 实例化一个类，并调用其Run()方法

#include "roi_object_filter/roi_object_filter.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, __APP_NAME__);

    RosRoiObjectFilterApp app;

    app.Run();

    return 0;
}

2. roi_object_filter.cpp

2.1 构造函数

初始化一些标志位，这里可以看到gridmap网格地图的字眼，会让人联想到occupancy grid map，这里先往下继续分析

RosRoiObjectFilterApp::RosRoiObjectFilterApp()
{
  gridmap_ready_ = false;
  detections_ready_ = false;
  processing_ = false;
}

2.2 Run()函数

void RosRoiObjectFilterApp::Run() {
  ros::NodeHandle private_node_handle("~");
  tf::TransformListener transform_listener;
  transform_listener_ = &transform_listener;
  InitializeRosIo(private_node_handle);
  ROS_INFO("[%s] Ready. Waiting for data...", __APP_NAME__);
  ros::spin();
  ROS_INFO("[%s] END", __APP_NAME__);
}

这里是声明一个局部tf的监听器变量，然后赋值给类内全局变量。tf监听器在类内声明的时候并没有初始化，因此在主线程开始前将其初始化一下赋予一个值。下面打开主要功能函数InitializeRosIo()

void RosRoiObjectFilterApp::InitializeRosIo(ros::NodeHandle &in_private_handle) {
  std::string exception_list;

  ros_namespace_ = ros::this_node::getNamespace();

  if (ros_namespace_.substr(0, 2) == "//") {
    ros_namespace_.erase(ros_namespace_.begin());
  }

  objects_src_topic_ = ros_namespace_ + "/objects";
  wayarea_gridmap_topic_= "/grid_map_wayarea";
  std::string roi_topic_str = ros_namespace_+ "/objects_filtered";

  ROS_INFO("[%s] objects_src_topic: %s", __APP_NAME__, objects_src_topic_.c_str());
  ROS_INFO("[%s] wayarea_gridmap_topic: %s", __APP_NAME__, wayarea_gridmap_topic_.c_str());

  in_private_handle.param<bool>("sync_topics", sync_topics_, false);
  ROS_INFO("[%s] sync_topics: %d", __APP_NAME__, sync_topics_);

  in_private_handle.param<std::string>("wayarea_gridmap_layer", gridmap_layer_, "wayarea");
  ROS_INFO("[%s] wayarea_gridmap_layer: %s", __APP_NAME__, gridmap_layer_.c_str());

  in_private_handle.param<int>("wayarea_no_road_value", gridmap_no_road_value_, GRID_MAX_VALUE);
  ROS_INFO("[%s] wayarea_no_road_value: %ds", __APP_NAME__, gridmap_no_road_value_);

  in_private_handle.param<std::string>("exception_list", exception_list, "[person, bicycle]");
  ROS_INFO("[%s] exception_list: %s", __APP_NAME__, exception_list.c_str());

  YAML::Node exception_list_yaml;
  try {
    exception_list_yaml = YAML::Load(exception_list);
    for (const auto &exception : exception_list_yaml) {
      exception_list_.push_back(exception.as<std::string>());
    }
  } catch (const std::exception &e) {
    ROS_INFO("[%s] Incorrect format for Exception List
    						(exception_list) param, using default.",__APP_NAME__);
    exception_list_ = {"person", "bycicle"};
  }

  if (!sync_topics_) {
    detections_range_subscriber_ = in_private_handle.subscribe(objects_src_topic_, 1,
                                     &RosRoiObjectFilterApp::DetectionsCallback, this);

    wayarea_gridmap_subscriber_ = in_private_handle.subscribe(wayarea_gridmap_topic_, 1,
                                  &RosRoiObjectFilterApp::WayAreaGridMapCallback, this);
  } else {
    detections_filter_subscriber_ = new message_filters::Subscriber<
    					autoware_msgs::DetectedObjectArray(node_handle_, objects_src_topic_,1);
    					
    gridmap_filter_subscriber_ = new message_filters::Subscriber<
    							grid_map_msgs::GridMap>(node_handle_, wayarea_gridmap_topic_, 1);
    							
    detections_synchronizer_ = new message_filters::Synchronizer<SyncPolicyT>(SyncPolicyT(10),
                                *detections_filter_subscriber_, *gridmap_filter_subscriber_);
                                
    detections_synchronizer_->registerCallback(boost::bind(
    							&RosRoiObjectFilterApp::SyncedDetectionsCallback, this, _1, _2));
  }
  
  roi_objects_publisher_ = node_handle_.advertise<autoware_msgs::DetectedObjectArray>(roi_topic_str, 1);
  ROS_INFO("[%s] Publishing filtered objects in %s", __APP_NAME__, roi_topic_str.c_str());
}

前面的部分都是ros通过参数服务器加载参数，我们直接看到后面，这里面有一个ros关于同步的知识点，如果不需要ros同步的话，则分别订阅两个topic，一个是关于雷达目标的topic，一个是关于wayarea_gridmap_topic。 同步的话，则利用ros自带的库message_filters::Synchronizer, 将多个topic数据放到一个callback回调函数里面处理，具体用法可以看ros.wiki页面

回到autoware这里，我们可以看到关于WayAreaGridMap的回调函数，将ros消息数据传到全局变量wayarea_gridmap_中，然后重置标志位。

void RosRoiObjectFilterApp::WayAreaGridMapCallback(const grid_map_msgs::GridMap::ConstPtr &in_message)
{
  if (!processing_)
  {
    processing_ = true;
    wayarea_gridmap_ = in_message;
    processing_ = false;
  }
}

再看关于雷达聚类检测出目标的回调函数：

void RosRoiObjectFilterApp::DetectionsCallback(const autoware_msgs::DetectedObjectArray::ConstPtr &in_detections)
{
  if (!processing_ && !in_detections->objects.empty())
  {
    processing_ = true;
    object_detections_ = in_detections;
    SyncedDetectionsCallback(in_detections, wayarea_gridmap_);
    processing_ = false;
  }
}

这里先是将ros消息传入到全局变量，然后调用同步方式里的两个消息同步SyncedDetectioncallback来处理两个ros数据消息，那其实这里可以猜到雷达聚类目标的这个topic频率应该是高于wayarea_gridmap_的。
然后我们继续打开SyncedDetectionsCallback（）同步callback：

void RosRoiObjectFilterApp::SyncedDetectionsCallback(
		const autoware_msgs::DetectedObjectArray::ConstPtr &in_detections,
		const grid_map_msgs::GridMap::ConstPtr &in_gridmap) {
  if (nullptr == in_detections || nullptr == in_gridmap) {
    ROS_INFO("[%s] Empty input messages, for details check the topics : %s and %s.", __APP_NAME__,
             wayarea_gridmap_topic_.c_str(), objects_src_topic_.c_str());
    return;
  }

  grid_map::GridMap current_grid;
  grid_map::GridMapRosConverter::fromMessage(*in_gridmap, current_grid);
  autoware_msgs::DetectedObjectArray in_roi_objects;
  autoware_msgs::DetectedObjectArray out_roi_objects;

  in_roi_objects.header = in_detections->header;
  out_roi_objects.header = in_detections->header;

  if (current_grid.exists(gridmap_layer_))
  {
    // check if centroids are inside the drivable area
    cv::Mat grid_image;
    grid_map::GridMapCvConverter::toImage<unsigned char, 1>(
    			current_grid, gridmap_layer_, CV_8UC1, GRID_MIN_VALUE, GRID_MAX_VALUE, grid_image);
    			
    #pragma omp for
    for (unsigned int i = 0; i < in_detections->objects.size(); i++) {
      geometry_msgs::Point original_centroid_point, final_centroid_point;
      original_centroid_point = in_detections->objects[i].pose.position;

      if (current_grid.getFrameId() != in_detections->header.frame_id) {
        tf::StampedTransform grid_sensor_tf = FindTransform(
        	current_grid.getFrameId(), 	in_detections->header.frame_id);
        final_centroid_point = TransformPoint(original_centroid_point, grid_sensor_tf);
      } else {
        final_centroid_point = original_centroid_point;
      }

      bool point_in_grid = CheckPointInGrid(current_grid, grid_image, final_centroid_point);

      std::vector<std::string>::const_iterator found = std::find(exception_list_.begin(), exception_list_.end(),
                                                                 in_detections->objects[i].label);
      bool is_exception = (found != exception_list_.end());

      if (is_exception || point_in_grid) {
        in_roi_objects.objects.push_back(in_detections->objects[i]);
      } else {
        out_roi_objects.objects.push_back(in_detections->objects[i]);
      }
    }
  } else {
    ROS_INFO("[%s] %s layer not contained in the OccupancyGrid", __APP_NAME__, gridmap_layer_.c_str());
  }

  roi_objects_publisher_.publish(in_roi_objects);

  wayarea_gridmap_ = nullptr;
  object_detections_ = nullptr;
}

具体的函数流程如下：

• 第一步检测ros数据是否都有，没有就退出。这是一个非常好的编程习惯，即在函数入口处检查数据的正常性。我们可以在google开源的各种代码中看到这样的check机制。
• 第二步将gridmap消息转换成grid_map::GridMap，这里是用的ros自带的grid_map包
• 第三步判断grid_map中是否存在gridmap_layer这一层
  • 先将gridmap转换成图像数据cv::Mat grid_image
  • 使用openmp进行并行加速for循环
  • 遍历检测到的目标：
    • 拿到每一个目标的位置坐标值
    • 判断当前占有格地图的frame_id 和检测目标的frame_id是否一致
      • 如果不一致，则将目标转换到地图坐标系下，这样是便于比较两者的值
      • 如果一致，则将目标点直接赋值给final_centroid_point
    • 使用CheckPointInGrid方法来检测目标点是否在目标栅格地图gridmap中
    • 通过将目标的标签来和期望目标类别（车子和行人）比较
    • 如果目标是行人或者车子 || 目标点在gridmap中即在我们关心的网格区域内，就把对象存到in_roi_objects中，并且通过/objects_filtered发布出来。

以上流程中，FindTransform()和TransformPoint()方法比较简单，不在赘述。我们打开CheckPointInGrid方法来看一下：

bool RosRoiObjectFilterApp::CheckPointInGrid(const grid_map::GridMap &in_grid_map,
                                        const cv::Mat &in_grid_image,
                                        const geometry_msgs::Point &in_point)
{
  // calculate out_grid_map position
  grid_map::Position map_pos = in_grid_map.getPosition();
  double origin_x_offset = in_grid_map.getLength().x() / 2.0 - map_pos.x();
  double origin_y_offset = in_grid_map.getLength().y() / 2.0 - map_pos.y();
  // coordinate conversion for cv image
  double cv_x = (in_grid_map.getLength().y() - origin_y_offset - in_point.y) / in_grid_map.getResolution();
  double cv_y = (in_grid_map.getLength().x() - origin_x_offset - in_point.x) / in_grid_map.getResolution();

  // check coords are inside the gridmap
  if (cv_x < 0 || cv_x > in_grid_image.cols || cv_y < 0 || cv_y > in_grid_image.rows)
  {
    return false;
  }

  //_gridmap_no_road_value if road
  if (in_grid_image.at<uchar>(cv_y, cv_x) != gridmap_no_road_value_)
  {
    return true;
  }

  return false;
}

• 首先拿到in_grid_map的位置
• 拿到地图左下角x和y相对于地图中心的偏移，这里其实是负的偏移，因此下面会减去这个值
• 将输入点的坐标转换成图像坐标系下的像素坐标值，像素坐标系和地图坐标系这里是有区别的，同时x和y好像这里也是互相调换的。
• 拿到目标点在图像坐标系下的像素值，如果有的话，则证明目标点是处于grid_map内的，则返回true。反之返回false。

最后我们可以发现这个功能节点还有一个很重要的依赖输入topic便是/grid_map_wayarea，它到底是多长多宽，如何生成的，是动态变化的还是固定就是整个大地图场景，我们暂时都不知道，因此我们需要在后面继续学习这个topic是如何配置的。

总结

最后看下来，并不是和我想的一样，autoware是聚类加上使用占有格地图的方式来滤除我们行驶区域以外的目标。因此前面聚类的计算量依然是挺庞大的。