[Perception] (1) add 'non_mask' feature; (2) modify view range for lane proto

modules/perception/model/camera/lane_post_process.config

@@ -25,6 +25,21 @@ model_configs {
         values: 640
     }
 
+    bool_params {
+        name: "use_non_mask"
+        value: false
+    }
+
+    array_float_params {
+        name: "non_mask"
+        values: 960   # x coordinate of point
+        values: 1440  # y coordinate of point
+        values: 1260
+        values: 1079
+        values: 660
+        values: 1079
+    }
+
     float_params {
         name: "lane_map_confidence_thresh"
         value: 0.5

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc

@@ -94,6 +94,31 @@ bool CCLanePostProcessor::Init() {
           << roi_.x + roi_.width - 1 << ", " << roi_.y + roi_.height - 1 << "]";
   }
 
+  if (!model_config->GetValue("use_non_mask", &options_.frame.use_non_mask)) {
+    AERROR << "the use_non_mask parameter not found.";
+    return false;
+  }
+
+  std::vector<float> non_mask_polygon_points;
+  if (!model_config->GetValue("non_mask", &non_mask_polygon_points)) {
+    AERROR << "non_mask points not found.";
+    return false;
+  }
+
+  AINFO << "[Debug] non_mask_polygon_points.size() = " << non_mask_polygon_points.size();
+
+  if (non_mask_polygon_points.size() % 2 != 0) {
+    AERROR << "the number of point coordinate values should be even.";
+    return false;
+  }
+  size_t non_mask_polygon_point_num = non_mask_polygon_points.size() / 2;
+
+  non_mask_.reset(new NonMask(non_mask_polygon_point_num));
+  for (size_t i = 0; i < non_mask_polygon_point_num; ++i) {
+    non_mask_->AddPolygonPoint(non_mask_polygon_points.at(2 * i),
+                               non_mask_polygon_points.at(2 * i + 1));
+  }
+
   if (!model_config->GetValue("lane_map_confidence_thresh",
                               &options_.lane_map_conf_thresh)) {
     AERROR << "the confidence threshold of label map not found.";
@@ -573,9 +598,9 @@ bool CCLanePostProcessor::GenerateLaneInstances(const cv::Mat &lane_map) {
   cur_frame_.reset(new LaneFrame);
 
   if (options_.frame.space_type == SpaceType::IMAGE) {
-    cur_frame_->Init(cc_list, options_.frame);
+    cur_frame_->Init(cc_list, non_mask_, options_.frame);
   } else if (options_.frame.space_type == SpaceType::VEHICLE) {
-    cur_frame_->Init(cc_list, projector_, options_.frame);
+    cur_frame_->Init(cc_list, non_mask_, projector_, options_.frame);
   } else {
     AERROR << "unknown space type: " << options_.frame.space_type;
     return false;

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h

@@ -53,9 +53,9 @@ struct CCLanePostProcessorOptions {
 class CCLanePostProcessor : public BaseCameraLanePostProcessor {
  public:
   CCLanePostProcessor() : BaseCameraLanePostProcessor() {
-    max_distance_to_see_ = 200.0;
-    vis_ = false;
-    is_init_ = false;
+    //max_distance_to_see_ = 200.0;
+    //vis_ = false;
+    //is_init_ = false;
   }
 
   ~CCLanePostProcessor() {}
@@ -102,23 +102,25 @@ class CCLanePostProcessor : public BaseCameraLanePostProcessor {
  private:
   CCLanePostProcessorOptions options_;
 
-  double time_stamp_;
-  int frame_id_;
+  std::shared_ptr<NonMask> non_mask_;
+
+  double time_stamp_ = 0.0;
+  int frame_id_ = -1;
   std::shared_ptr<ConnectedComponentGenerator> cc_generator_;
   std::shared_ptr<LaneFrame> cur_frame_;
   LaneInstancesPtr cur_lane_instances_;
 
-  ScalarType max_distance_to_see_;
-  int image_width_;
-  int image_height_;
+  ScalarType max_distance_to_see_ = 500.0;
+  int image_width_ = 1080;
+  int image_height_ = 1920;
   cv::Rect roi_;
 
-  bool is_x_longitude_;
+  bool is_x_longitude_ = true;
 
   std::shared_ptr<Projector<ScalarType>> projector_;
 
-  bool is_init_;
-  bool vis_;
+  bool is_init_ = false;
+  bool vis_ = false;
 
   DISALLOW_COPY_AND_ASSIGN(CCLanePostProcessor);
 };

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.cc

@@ -150,6 +150,7 @@ ScalarType LaneFrame::ComputeMarkerPairDistance(const Marker& ref,
 }
 
 bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
+                     const shared_ptr<NonMask>& non_mask,
                      const LaneFrameOptions& options) {
   if (options.space_type != SpaceType::IMAGE) {
     AERROR << "the space type is not IMAGE.";
@@ -172,22 +173,38 @@ bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
         Marker marker;
         marker.shape_type = MarkerShapeType::LINE_SEGMENT;
         marker.space_type = opts_.space_type;
+
         marker.pos = cc_ptr->GetVertex(edge_ptr->end_vertex_id);
         marker.image_pos = marker.pos;
+        if (opts_.use_non_mask &&
+            non_mask->IsInsideMask(marker.image_pos)) {
+          ADEBUG << "the marker with end point ("
+                 << marker.image_pos.x() << ", "
+                 << marker.image_pos.y() << ") is filtered by non_mask.";
+          continue;
+        }
         marker.vis_pos = cv::Point(static_cast<int>(marker.pos.x()),
                                    static_cast<int>(marker.pos.y()));
+
         marker.start_pos = cc_ptr->GetVertex(edge_ptr->start_vertex_id);
         marker.image_start_pos = marker.start_pos;
+        if (opts_.use_non_mask &&
+            non_mask->IsInsideMask(marker.image_start_pos)) {
+          ADEBUG << "the marker with start point ("
+                 << marker.image_start_pos.x() << ", "
+                 << marker.image_start_pos.y() << ") is filtered by non_mask.";
+          continue;
+        }
         marker.vis_start_pos =
             cv::Point(static_cast<int>(marker.start_pos.x()),
                       static_cast<int>(marker.start_pos.y()));
+
         marker.angle = edge_ptr->orie;
         if (marker.angle < -static_cast<ScalarType>(M_PI) ||
             marker.angle > static_cast<ScalarType>(M_PI)) {
           AERROR << "marker.angle is out range of [-pi, pi]: " << marker.angle;
           return false;
         }
-
         marker.orie(0) = std::cos(marker.angle);
         marker.orie(1) = std::sin(marker.angle);
         marker.original_id = static_cast<int>(markers_.size());
@@ -277,6 +294,7 @@ bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
 }
 
 bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
+                     const shared_ptr<NonMask>& non_mask,
                      const shared_ptr<Projector<ScalarType>>& projector,
                      const LaneFrameOptions& options) {
   if (options.space_type != SpaceType::VEHICLE) {
@@ -307,12 +325,21 @@ bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
         marker.shape_type = MarkerShapeType::LINE_SEGMENT;
         marker.space_type = opts_.space_type;
 
+        if (opts_.use_non_mask && non_mask->IsInsideMask(pos)) {
+          ADEBUG << "the marker with end point ("
+                 << pos(0) << ", "
+                 << pos(1) << ") is filtered by non_mask.";
+          continue;
+        }
+        marker.image_pos = pos;
         if (!projector_->UvToXy(static_cast<ScalarType>(pos(0)),
                                 static_cast<ScalarType>(pos(1)),
                                 &(marker.pos))) {
+          ADEBUG << "the marker with end point ("
+                 << pos(0) << ", "
+                 << pos(1) << ") is filtered by projector.";
           continue;
         }
-        marker.image_pos = pos;
         if (projector_->is_vis()) {
           if (!projector_->UvToXyImagePoint(static_cast<ScalarType>(pos(0)),
                                             static_cast<ScalarType>(pos(1)),
@@ -321,12 +348,21 @@ bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
           }
         }
 
+        if (opts_.use_non_mask && non_mask->IsInsideMask(start_pos)) {
+          ADEBUG << "the marker with start point ("
+                 << start_pos(0) << ", "
+                 << start_pos(1) << ") is filtered by non_mask.";
+          continue;
+        }
+        marker.image_start_pos = start_pos;
         if (!projector_->UvToXy(static_cast<ScalarType>(start_pos(0)),
                                 static_cast<ScalarType>(start_pos(1)),
                                 &(marker.start_pos))) {
+          ADEBUG << "the marker with start point ("
+                 << start_pos(0) << ", "
+                 << start_pos(1) << ") is filtered by projector.";
           continue;
         }
-        marker.image_start_pos = start_pos;
         if (projector_->is_vis()) {
           if (!projector_->UvToXyImagePoint(
                   static_cast<ScalarType>(start_pos(0)),
@@ -343,7 +379,6 @@ bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
           AERROR << "marker.angle is out range of [-pi, pi]: " << marker.angle;
           return false;
         }
-
         marker.orie(0) = std::cos(marker.angle);
         marker.orie(1) = std::sin(marker.angle);
         marker.original_id = static_cast<int>(markers_.size());
@@ -397,13 +432,13 @@ bool LaneFrame::Init(const vector<ConnectedComponentPtr>& input_cc,
           y_max = std::max(y_max, markers_[j].pos(1));
         }
         if (x_max - x_min < 0.5 && y_max - y_min < 0.5) {
-          AINFO << "x_min = " << x_min << ", "
+          ADEBUG << "x_min = " << x_min << ", "
                 << "x_max = " << x_max << ", "
                 << "width = " << x_max - x_min << ", "
                 << "y_min = " << y_min << ", "
                 << "y_max = " << y_max << ", "
                 << "height = " << y_max - y_min;
-          AINFO << "this cc is too small, ignore it.";
+          ADEBUG << "this cc is too small, ignore it.";
           is_small_cc = true;
         }
       }

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h

@@ -48,6 +48,8 @@ struct LaneFrameOptions {
   int min_cc_pixel_num = 10;  // minimum number of pixels of CC
   int min_cc_size = 5;        // minimum size of CC
 
+  bool use_non_mask = false;  // indicating whether use non_mask or not
+
   // used for greedy search association method
   // maximum number of markers used for matching for each CC
   int max_cc_marker_match_num = 1;
@@ -78,9 +80,11 @@ struct LaneFrameOptions {
 class LaneFrame {
  public:
   bool Init(const std::vector<ConnectedComponentPtr>& input_cc,
+            const std::shared_ptr<NonMask>& non_mask,
             const LaneFrameOptions& options);
 
   bool Init(const std::vector<ConnectedComponentPtr>& input_cc,
+            const std::shared_ptr<NonMask>& non_mask,
             const std::shared_ptr<Projector<ScalarType>>& projector,
             const LaneFrameOptions& options);
 

modules/perception/obstacle/camera/lane_post_process/common/BUILD

@@ -16,6 +16,7 @@ cc_library(
 
 cc_library(
     name = "util",
+    srcs = ["util.cc"],
     hdrs = ["util.h"],
     deps = [
         ":type",

modules/perception/obstacle/camera/lane_post_process/common/type.h

@@ -60,6 +60,10 @@ namespace perception {
 
 typedef float ScalarType;
 
+#ifndef INF_NON_MASK_POINT_X
+#define INF_NON_MASK_POINT_X 10000
+#endif
+
 constexpr ScalarType kEpsilon = 1e-5;
 
 // define colors for visualization (Blue, Green, Red)
@@ -253,7 +257,9 @@ struct LaneObject {
     lane_marker->set_c1_heading_angle(model(1));
     lane_marker->set_c2_curvature(model(2));
     lane_marker->set_c3_curvature_derivative(model(3));
-    lane_marker->set_view_range(longitude_end - longitude_start);
+    lane_marker->set_view_range(std::max(longitude_end, 0));
+    lane_marker->set_longitude_start(longitude_start);
+    lane_marker->set_longitude_end(longitude_end);
   }
 
   std::string GetSpatialLabel() const {

modules/perception/obstacle/camera/lane_post_process/common/util.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/perception/obstacle/camera/lane_post_process/common/util.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace apollo {
+namespace perception {
+
+void RectAngle(ScalarType *theta) {
+  if (theta == NULL) {
+    return;
+  }
+  if (*theta < 0) {
+    (*theta) += static_cast<ScalarType>(2 * M_PI);
+  }
+}
+
+void NonMask::AddPolygonPoint(const ScalarType &x, const ScalarType &y) {
+  polygon_.push_back(Vector2D(x, y));
+}
+
+int NonMask::ComputeOrientation(const Vector2D &p1, const Vector2D &p2,
+                                const Vector2D &q) const {
+  ScalarType cross = (q.x() - p1.x()) * (p2.y() - p1.y()) -
+                     (p2.x() - p1.x()) * (q.y() - p1.y());
+  if (std::fabs(cross) < kEpsilon) {
+    return 0;  // coliner
+  }
+  return cross > 0 ? 1 : -1;  // 1: clockwise, -1: anti-clockwise
+}
+
+bool NonMask::IsColinear(const Vector2D &p1, const Vector2D &p2,
+                         const Vector2D &q) const {
+  ScalarType cross = (q.x() - p1.x()) * (p2.y() - p1.y()) -
+                     (p2.x() - p1.x()) * (q.y() - p1.y());
+  return std::fabs(cross) < kEpsilon;
+}
+
+bool NonMask::IsOnLineSegmentWhenColinear(const Vector2D &p1,
+                                          const Vector2D &p2,
+                                          const Vector2D &q) const {
+  return q.x() <= std::max(p1.x(), p2.x()) &&
+         q.x() >= std::min(p1.x(), p2.x()) &&
+         q.y() <= std::max(p1.y(), p2.y()) && q.y() >= std::min(p1.y(), p2.y());
+}
+
+bool NonMask::IsLineSegmentIntersect(const Vector2D &p1, const Vector2D &p2,
+                                     const Vector2D &p3,
+                                     const Vector2D &p4) const {
+  int relative_orientation_123 = ComputeOrientation(p1, p2, p3);
+  int relative_orientation_124 = ComputeOrientation(p1, p2, p4);
+  int relative_orientation_341 = ComputeOrientation(p3, p4, p1);
+  int relative_orientation_342 = ComputeOrientation(p3, p4, p2);
+
+  if (relative_orientation_123 == 0 &&
+      IsOnLineSegmentWhenColinear(p1, p2, p3)) {
+    return true;
+  }
+  if (relative_orientation_124 == 0 &&
+      IsOnLineSegmentWhenColinear(p1, p2, p4)) {
+    return true;
+  }
+  if (relative_orientation_341 == 0 &&
+      IsOnLineSegmentWhenColinear(p3, p4, p1)) {
+    return true;
+  }
+  if (relative_orientation_342 == 0 &&
+      IsOnLineSegmentWhenColinear(p3, p4, p2)) {
+    return true;
+  }
+
+  return (relative_orientation_123 != relative_orientation_124) &&
+         (relative_orientation_341 != relative_orientation_342);
+}
+
+bool NonMask::IsInsideMask(const Vector2D &p) const {
+  int n = static_cast<int>(polygon_.size());
+  if (n < 3) return false;
+
+  Vector2D extreme_p(ScalarType(INF_NON_MASK_POINT_X), p.y());
+
+  int intersec_count = 0, curt = 0, next = 1;
+  while (curt < n) {
+    // Check if the line segment 'p'->'extreme_p' intersects with
+    // 'polygon_[curt]'->'polygon_[next]'
+    if (IsLineSegmentIntersect(polygon_[curt], polygon_[next], p, extreme_p)) {
+      // If the point is colinear with the boundary polygon,
+      // check if it lies on the boundary.
+      if (IsColinear(polygon_[curt], polygon_[next], p)) {
+        return IsOnLineSegmentWhenColinear(polygon_[curt], polygon_[next], p);
+      }
+
+      intersec_count++;
+    }
+
+    ++curt;
+    if (++next == n) {
+      next = 0;
+    }
+  }
+
+  return intersec_count % 2 != 0;
+}
+
+}  // namespace perception
+}  // namespace apollo

modules/perception/obstacle/camera/lane_post_process/common/util.h

@@ -31,20 +31,13 @@ namespace apollo {
 namespace perception {
 
 // @brief: convert angle from the range of [-pi, pi] to [0, 2*pi]
-inline void RectAngle(ScalarType* theta) {
-  if (theta == NULL) {
-    return;
-  }
-  if (*theta < 0) {
-    (*theta) += static_cast<ScalarType>(2 * M_PI);
-  }
-}
+void RectAngle(ScalarType *theta);
 
 // @brief: fit polynomial function with QR decomposition (using Eigen 3)
 template <typename T = ScalarType>
-bool PolyFit(const std::vector<Eigen::Matrix<T, 2, 1>>& pos_vec,
-             const int& order, Eigen::Matrix<T, MAX_POLY_ORDER + 1, 1>* coeff,
-             const bool& is_x_axis = true) {
+bool PolyFit(const std::vector<Eigen::Matrix<T, 2, 1>> &pos_vec,
+             const int &order, Eigen::Matrix<T, MAX_POLY_ORDER + 1, 1> *coeff,
+             const bool &is_x_axis = true) {
   if (coeff == NULL) {
     AERROR << "The coefficient pointer is NULL.";
     return false;
@@ -89,8 +82,8 @@ bool PolyFit(const std::vector<Eigen::Matrix<T, 2, 1>>& pos_vec,
 
 // @brief: evaluate y value of given x for a polynomial function
 template <typename T = ScalarType>
-T PolyEval(const T& x, const int& order,
-           const Eigen::Matrix<T, MAX_POLY_ORDER + 1, 1>& coeff) {
+T PolyEval(const T &x, const int &order,
+           const Eigen::Matrix<T, MAX_POLY_ORDER + 1, 1> &coeff) {
   int poly_order = order;
   if (order > MAX_POLY_ORDER) {
     AERROR << "the order of polynomial function must be smaller than "
@@ -122,6 +115,32 @@ T GetPolyValue(T a, T b, T c, T d, T x) {
   return y;
 }
 
+// @brief: non mask class which is used for filtering out the markers inside the
+// polygon mask
+class NonMask {
+ public:
+  NonMask() {}
+  NonMask(size_t n) {
+    polygon_.reserve(n);
+  }
+
+  void AddPolygonPoint(const ScalarType &x, const ScalarType &y);
+  bool IsInsideMask(const Vector2D &p) const;
+
+ protected:
+  int ComputeOrientation(const Vector2D &p1, const Vector2D &p2,
+                         const Vector2D &q) const;
+  bool IsColinear(const Vector2D &p1, const Vector2D &p2,
+                  const Vector2D &q) const;
+  bool IsOnLineSegmentWhenColinear(const Vector2D &p1, const Vector2D &p2,
+                                   const Vector2D &q) const;
+  bool IsLineSegmentIntersect(const Vector2D &p1, const Vector2D &p2,
+                              const Vector2D &p3, const Vector2D &p4) const;
+
+ private:
+  std::vector<Vector2D> polygon_;
+};
+
 }  // namespace perception
 }  // namespace apollo
 

modules/perception/proto/perception_obstacle.proto

@@ -68,6 +68,8 @@ message LaneMarker {
   optional double c2_curvature = 6;
   optional double c3_curvature_derivative = 7;
   optional double view_range = 8;
+  optional double longitude_start = 9;
+  optional double longitude_end = 10;
 }
 
 message LaneMarkers {