add comments and update test file (#483)

* trim code

* trim code and add comments

* add comments

* add comments for hdmap_roi_filter.h

* update roi filter test config path

* update roi test

modules/perception/obstacle/lidar/roi_filter/hdmap_roi_filter/bitmap2d.h

@@ -25,6 +25,10 @@
 namespace apollo {
 namespace perception {
 
+/**
+ * @class Bitmap2D
+ *
+ */
 class Bitmap2D {
 public:
   enum DirectionMajor{XMAJOR = 0, YMAJOR =1};
@@ -46,7 +50,6 @@ public:
   bool IsExist(const Eigen::Vector2d& p) const;
   bool Check(const Eigen::Vector2d& p) const;
 
-  void Set(const Eigen::Vector2d& p);
   void Set(double x, double min_y, double max_y);
   void Set(uint64_t x_id, uint64_t min_y_id, uint64_t max_y_id);
 

modules/perception/obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

@@ -17,49 +17,7 @@
 namespace apollo {
 namespace perception {
 
-bool Bitmap2dFilter(const pcl::PointCloud<pcl_util::Point>::ConstPtr in_cloud_ptr,
-                    const Bitmap2D &bitmap,
-                    pcl_util::PointIndices* roi_indices_ptr) {
-  roi_indices_ptr->indices.reserve(in_cloud_ptr->size());
-  for (size_t i = 0; i < in_cloud_ptr->size(); ++i) {
-    const auto &pt = in_cloud_ptr->points[i];
-    Eigen::Vector2d p(pt.x, pt.y);
-    if (bitmap.IsExist(p) && bitmap.Check(p)) {
-      roi_indices_ptr->indices.push_back(i);
-    }
-  }
-  return true;
-}
-
-void MergeRoadBoundariesToPolygons(const std::vector<RoadBoundary>& road_boundaries,
-                                   std::vector<PolygonDType>* polygons) {
-  polygons->resize(road_boundaries.size());
-  for (size_t i = 0; i < road_boundaries.size(); ++i) {
-    const PolygonDType& left_boundary = road_boundaries[i].left_boundary;
-    const PolygonDType& right_boundary = road_boundaries[i].right_boundary;
 
-    auto& polygon = (*polygons)[i];
-    polygon.reserve(left_boundary.size() + right_boundary.size());
-    polygon.insert(polygon.end(), left_boundary.begin(), left_boundary.end());
-
-    CHECK(right_boundary.size() > 0);
-    for (size_t j = right_boundary.size() - 1; j != 0; --j) {
-      polygon.push_back(right_boundary[j]);
-    }
-  }
-}
-
-void MergeHdmapStructToPolygons(const HdmapStructConstPtr& hdmap_struct_ptr,
-                                std::vector<PolygonDType>* polygons) {
-  std::vector<PolygonDType> road_polygons;
-  MergeRoadBoundariesToPolygons(hdmap_struct_ptr->road_boundary, &road_polygons);
-
-  const std::vector<PolygonDType>& junction_polygons = hdmap_struct_ptr->junction;
-
-  polygons->reserve(road_polygons.size() + junction_polygons.size());
-  polygons->insert(polygons->end(), road_polygons.begin(), road_polygons.end());
-  polygons->insert(polygons->end(), junction_polygons.begin(), junction_polygons.end());
-}
 
 bool HdmapROIFilter::Filter(const pcl_util::PointCloudPtr& cloud,
                           const ROIFilterOptions &roi_filter_options,
@@ -134,6 +92,55 @@ MajorDirection HdmapROIFilter::GetMajorDirection(
       MajorDirection::XMAJOR : MajorDirection::YMAJOR;
 }
 
+bool HdmapROIFilter::Bitmap2dFilter(
+    const pcl::PointCloud<pcl_util::Point>::ConstPtr in_cloud_ptr,
+    const Bitmap2D &bitmap,
+    pcl_util::PointIndices* roi_indices_ptr) {
+
+  roi_indices_ptr->indices.reserve(in_cloud_ptr->size());
+  for (size_t i = 0; i < in_cloud_ptr->size(); ++i) {
+    const auto &pt = in_cloud_ptr->points[i];
+    Eigen::Vector2d p(pt.x, pt.y);
+    if (bitmap.IsExist(p) && bitmap.Check(p)) {
+      roi_indices_ptr->indices.push_back(i);
+    }
+  }
+  return true;
+}
+
+void HdmapROIFilter::MergeRoadBoundariesToPolygons(
+    const std::vector<RoadBoundary>& road_boundaries,
+    std::vector<PolygonDType>* polygons) {
+
+  polygons->resize(road_boundaries.size());
+  for (size_t i = 0; i < road_boundaries.size(); ++i) {
+    const PolygonDType& left_boundary = road_boundaries[i].left_boundary;
+    const PolygonDType& right_boundary = road_boundaries[i].right_boundary;
+
+    auto& polygon = (*polygons)[i];
+    polygon.reserve(left_boundary.size() + right_boundary.size());
+    polygon.insert(polygon.end(), left_boundary.begin(), left_boundary.end());
+
+    CHECK(right_boundary.size() > 0);
+    for (size_t j = right_boundary.size() - 1; j != 0; --j) {
+      polygon.push_back(right_boundary[j]);
+    }
+  }
+}
+
+void HdmapROIFilter::MergeHdmapStructToPolygons(
+    const HdmapStructConstPtr& hdmap_struct_ptr,
+    std::vector<PolygonDType>* polygons) {
+
+  std::vector<PolygonDType> road_polygons;
+  MergeRoadBoundariesToPolygons(hdmap_struct_ptr->road_boundary, &road_polygons);
+
+  const std::vector<PolygonDType>& junction_polygons = hdmap_struct_ptr->junction;
+
+  polygons->reserve(road_polygons.size() + junction_polygons.size());
+  polygons->insert(polygons->end(), road_polygons.begin(), road_polygons.end());
+  polygons->insert(polygons->end(), junction_polygons.begin(), junction_polygons.end());
+}
 
 bool HdmapROIFilter::Init() {
   //FLAGS_work_root = "modules/perception";
@@ -199,6 +206,6 @@ void HdmapROIFilter::TransformFrame(
   }
 }
 
-} // perception
-} // apollo
+} // namespace perception
+} // namespace apollo
 

modules/perception/obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.h

@@ -36,6 +36,14 @@ namespace apollo {
 namespace perception {
 
 typedef typename Bitmap2D::DirectionMajor MajorDirection;
+
+/**
+ * @class HdmapROIFilter
+ * @brief This is ROI(Region of Interest) Filter based on HD map, which can
+ * figure out which point is in the regions what we focus on(Almost in the road).
+ * This is an optional process, the most advantage of ROI filter is to filter
+ * out some points to reduce calculation in the following process.
+ */
 class HdmapROIFilter: public BaseROIFilter {
 public:
   HdmapROIFilter() : BaseROIFilter() {}
@@ -48,8 +56,9 @@ public:
               const ROIFilterOptions &roi_filter_options,
               pcl_util::PointIndices* roi_indices) override;
 
-  /*
-   * @brief: Draw polygons bitmap and check each whether is in polygons
+protected:
+  /**
+   * @brief: Draw polygons bitmap and check each point whether is in polygons
    * @params[In] cloud: point cloud with local coordinates
    * @params[In] map_polygons: polygons after transformed to scan polygon type
    * @params[Out] roi_indices: Indices of point in roi
@@ -59,8 +68,7 @@ public:
                              const std::vector<PolygonType>& map_polygons,
                              pcl_util::PointIndices* roi_indices);
 
-protected:
-  /*
+  /**
    * @brief: transform polygon and cloud to local corrdinates
    * @params[In] cloud: point cloud with world coordinates
    * @params[In] vel_pose: TODO
@@ -75,7 +83,7 @@ protected:
           std::vector<PolygonType>& polygons_local,
           pcl_util::PointCloudPtr& cloud_local);
 
-  /*
+  /**
    * @brief: Get major direction. Transform polygons type to what we want.
    * @params[In] map_polygons: polygons with local coordinates
    * @params[In] polygons: polygons with type of vector<Matrix<double, 2, 1>>
@@ -85,6 +93,16 @@ protected:
       const std::vector<PolygonType>& map_polygons,
       std::vector<PolygonScanConverter::Polygon>* polygons);
 
+  void MergeRoadBoundariesToPolygons(const std::vector<RoadBoundary>& road_boundaries,
+                                     std::vector<PolygonDType>* polygons);
+
+  void MergeHdmapStructToPolygons(const HdmapStructConstPtr& hdmap_struct_ptr,
+                                std::vector<PolygonDType>* polygons);
+
+  bool Bitmap2dFilter(const pcl::PointCloud<pcl_util::Point>::ConstPtr in_cloud_ptr,
+                      const Bitmap2D &bitmap,
+                      pcl_util::PointIndices* roi_indices_ptr);
+
   // We only filter point with local coordinates x, y in [-range, range] in meters
   double range_;
 

modules/perception/obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter_test.cc

@@ -62,20 +62,16 @@ bool LoadPolygonFile(const std::string& absolute_file_name,
   return true;
 }
 
-class HdmapROIFilterTest : public testing::Test {
+class HdmapROIFilterTest : public testing::Test, HdmapROIFilter {
  public:
-  HdmapROIFilterTest() : _hdmap_roi_filter_ptr(new HdmapROIFilter),
-  _pts_cloud_ptr(new pcl_util::PointCloud){};
+  HdmapROIFilterTest() : _pts_cloud_ptr(new pcl_util::PointCloud){};
  protected:
   void SetUp() {
     LoadPolygonFile(polygon_file_name, &_polygons);
     pcl::io::loadPCDFile(pcd_file_name, *_pts_cloud_ptr);
-    //_hdmap_roi_filter_ptr = std::std::unique_ptr<HdmapROIFilter>(new HdmapROIFilter);
   }
 
   void TearDown() {
-    //delete _hdmap_roi_filter_ptr;
-    //_hdmap_roi_filter_ptr = nullptr;
   }
 
  protected:
@@ -90,14 +86,14 @@ class HdmapROIFilterTest : public testing::Test {
 
 void HdmapROIFilterTest::init() {
   FLAGS_work_root = "modules/perception/data";
-  FLAGS_config_manager_path = "./config_manager_test/config_manager.config";
-  ASSERT_TRUE(_hdmap_roi_filter_ptr->Init());
+  FLAGS_config_manager_path = "config_manager_test/config_manager.config";
+  ASSERT_TRUE(Init());
 }
 
 void HdmapROIFilterTest::filter() {
   pcl_util::PointIndices indices;
 
-  _hdmap_roi_filter_ptr->FilterWithPolygonMask(_pts_cloud_ptr, _polygons, &indices);
+  ASSERT_TRUE(FilterWithPolygonMask(_pts_cloud_ptr, _polygons, &indices));
 
   size_t points_num = _pts_cloud_ptr->size();
   std::vector<bool> is_in_roi(points_num, false);
@@ -129,16 +125,16 @@ void HdmapROIFilterTest::filter() {
     }
   }
 
-  /*
+
   AERROR << "True positive: " << true_positive
         << ", False positive: " << false_positive
         << ", True negitive: " << true_negitive
         << ", False negative: " << false_negitive;
-      */
 }
 
 TEST_F(HdmapROIFilterTest, test_filter) {
   init();
+  std::cout << "Successfully init." << std::endl;
   filter();
 }