Perception: add lane history filter stradegy(first simple version) in...

Perception: add lane history filter stradegy(first simple version) in lane_subnode, cc_lane_post_processor (#3742)

modules/perception/obstacle/base/object_supplement.h

@@ -130,12 +130,13 @@ struct alignas(16) CameraSupplement {
 typedef std::shared_ptr<CameraSupplement> CameraSupplementPtr;
 typedef std::shared_ptr<const CameraSupplement> CameraSupplementConstPtr;
 
+typedef Eigen::Matrix3f MotionType;
 struct alignas(16) VehicleStatus {
   float yaw_rate;
   float velocity;
   double time_ts;            // time stamp
   double time_d;            // time stamp difference in image
-  Eigen::Matrix3f motion;  // Motion Matrix
+  MotionType motion;  // Motion Matrix
 };
 
 typedef boost::circular_buffer<VehicleStatus> MotionBuffer;

modules/perception/obstacle/camera/interface/base_lane_post_processor.h

@@ -56,12 +56,19 @@
 #include "modules/common/macro.h"
 #include "modules/perception/lib/base/registerer.h"
 #include "modules/perception/obstacle/camera/lane_post_process/common/type.h"
+#include "modules/perception/obstacle/base/object_supplement.h"
 
 namespace apollo {
 namespace perception {
 
 struct CameraLanePostProcessOptions {
   double timestamp;
+  bool use_lane_history = false;
+  int lane_history_size = 0;
+  VehicleStatus vehicle_status;
+  void SetMotion(const VehicleStatus &vs) {
+    vehicle_status = vs;
+  }
 };
 
 class BaseCameraLanePostProcessor {

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

@@ -509,6 +509,9 @@ bool CCLanePostProcessor::Process(const cv::Mat &lane_map,
   }
 
   time_stamp_ = options.timestamp;
+  if (options.use_lane_history && !use_history_) {
+    InitLaneHistory();
+  }
 
   cur_lane_instances_.reset(new vector<LaneInstance>);
   if (!GenerateLaneInstances(lane_map)) {
@@ -788,11 +791,64 @@ bool CCLanePostProcessor::Process(const cv::Mat &lane_map,
   //     return false;
   //   }
   // }
-  EnrichLaneInfo((*lane_objects));
 
+  EnrichLaneInfo((*lane_objects));
+  ADEBUG << "use_lane_history_: " << use_history_;
+  if (use_history_) {
+    FilterWithLaneHistory(*lane_objects);
+    auto vs = options.vehicle_status;
+    vs.motion = vs.motion.inverse();
+    motion_buffer_->push_back(vs);
+    lane_history_.push_back(*(*lane_objects));
+  }
   return true;
 }
 
+void CCLanePostProcessor::InitLaneHistory() {
+  use_history_ = true;
+  lane_history_.set_capacity(MAX_LANE_HISTORY);
+  motion_buffer_ = std::make_shared<MotionBuffer>(MAX_LANE_HISTORY);
+}
+
+void CCLanePostProcessor::FilterWithLaneHistory(LaneObjectsPtr lane_objects) {
+  std::vector<int> erase_idx;
+  for (size_t i = 0; i < lane_objects->size(); i++) {
+    Eigen::Vector3f start_pos;
+    start_pos <<  lane_objects->at(i).pos[0].x(),
+                  lane_objects->at(i).pos[0].y(),
+                  1.0;
+
+    for (size_t j = 0; j < lane_history_.size(); j++) {
+      // iter to find corresponding lane
+      size_t k;
+      for (k = 0; k < lane_history_[j].size(); k++) {
+        if (lane_history_[j][k].spatial == lane_objects->at(i).spatial) {
+          break;
+        }
+      }
+      // if not exist, skip
+      if (k == lane_history_[j].size()) {
+        continue;
+      }
+      // project start_pos to history, check lane stability
+      auto project_pos = motion_buffer_->at(j).motion * start_pos;
+      auto &lane_object =  lane_history_[j][k];
+      ScalarType delta_y =
+        project_pos.y() -PolyEval(project_pos.x(),
+                                  lane_object.order,
+                                  lane_object.model);
+      // delete if too far from polyline
+      if (std::abs(delta_y) > 3.7) {
+        erase_idx.push_back(i);
+        break;
+      }
+    }
+  }
+  for (size_t i = erase_idx.size()-1; i >= 0; i--) {
+    lane_objects->erase(lane_objects->begin() + erase_idx[i]);
+  }
+}
+
 bool CCLanePostProcessor::CompensateLaneObjects(LaneObjectsPtr lane_objects) {
   if (lane_objects == nullptr) {
     AERROR << "lane_objects is a null pointer.";

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

@@ -19,6 +19,8 @@
 #ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_H_
 #define MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_H_
 
+#include <boost/circular_buffer.hpp>
+
 #include <memory>
 #include <string>
 #include <utility>
@@ -34,6 +36,7 @@
 #include "modules/perception/obstacle/camera/common/util.h"
 #include "modules/perception/obstacle/camera/interface/base_lane_post_processor.h"
 #include "modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h"
+#include "modules/perception/obstacle/base/object_supplement.h"
 
 namespace apollo {
 namespace perception {
@@ -97,6 +100,10 @@ class CCLanePostProcessor : public BaseCameraLanePostProcessor {
 
   bool EnrichLaneInfo(LaneObjectsPtr lane_objects);
 
+  void InitLaneHistory();
+
+  void FilterWithLaneHistory(LaneObjectsPtr lane_objects);
+
  private:
   CCLanePostProcessorOptions options_;
 
@@ -122,6 +129,9 @@ class CCLanePostProcessor : public BaseCameraLanePostProcessor {
 
   lane_post_process_config::ModelConfigs config_;
 
+  bool use_history_ = false;
+  boost::circular_buffer<LaneObjects> lane_history_;
+  MotionBufferPtr motion_buffer_ = nullptr;
   DISALLOW_COPY_AND_ASSIGN(CCLanePostProcessor);
 };
 

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

@@ -34,6 +34,10 @@
 namespace apollo {
 namespace perception {
 
+#ifndef MAX_LANE_HISTORY
+#define MAX_LANE_HISTORY 10
+#endif
+
 #ifndef UF_BLOCK_WIDTH
 #define UF_BLOCK_WIDTH 32
 #endif

modules/perception/obstacle/onboard/BUILD

@@ -236,6 +236,8 @@ cc_library(
         "//modules/perception/obstacle/base",
         "//modules/perception/obstacle/camera/interface",
         "//modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor",
+        "//modules/perception/obstacle/camera/motion",
+        "//modules/perception/obstacle/onboard:motion_subnode",
         "@eigen",
         "@opencv2//:core",
     ],

modules/perception/obstacle/onboard/lane_post_processing_subnode.cc

@@ -54,7 +54,21 @@ bool LanePostProcessingSubnode::InitInternal() {
   if (fields.count("publish") && stoi(fields["publish"]) != 0) {
     publish_ = true;
   }
-
+  auto iter = fields.find("motion_event_id");
+  if (iter == fields.end()) {
+    motion_event_id_ = -1;
+    AWARN << "Failed to find motion_event_id_:" << reserve_;
+    AWARN << "Unable to project lane history information";
+  } else {
+    motion_event_id_ = static_cast<EventID>(atoi((iter->second).c_str()));
+    motion_service_ = dynamic_cast<MotionService*>(
+          DAGStreaming::GetSubnodeByName("MotionService"));
+    if (motion_service_ == nullptr) {
+      AWARN << "motion service should initialize before LanePostProcessing";
+    }
+    options_.use_lane_history = true;
+//    options_.ConfigLaneHistory(FLAGS_lane_history_size);
+  }
   // init shared data
   if (!InitSharedData()) {
     AERROR << "failed to init shared data.";
@@ -210,7 +224,19 @@ Status LanePostProcessingSubnode::ProcEvents() {
   CameraLanePostProcessOptions options;
   options.timestamp = event.timestamp;
   timestamp_ns_ = event.timestamp * 1e9;
-
+  if (motion_event_id_ != -1) {
+    if (motion_service_ == nullptr) {
+      motion_service_ = dynamic_cast<MotionService*>(
+          DAGStreaming::GetSubnodeByName("MotionService"));
+      if (motion_service_ == nullptr) {
+        AERROR << "motion service must initialize before LanePostProcessing";
+        return Status(ErrorCode::PERCEPTION_ERROR, "Failed to proc events.");
+      }
+    }
+
+    // TODO(gchen-apollo): add lock to read motion_buffer
+    options_.SetMotion(motion_service_->GetMotionBuffer()->back());
+  }
   lane_post_processor_->Process(lane_map, options, &lane_objects);
   for (size_t i = 0; i < lane_objects->size(); ++i) {
     (*lane_objects)[i].timestamp = event.timestamp;

modules/perception/obstacle/onboard/lane_post_processing_subnode.h

@@ -35,6 +35,8 @@
 #include "modules/perception/obstacle/onboard/object_shared_data.h"
 #include "modules/perception/onboard/subnode.h"
 #include "modules/perception/onboard/subnode_helper.h"
+#include "modules/perception/onboard/dag_streaming.h"
+#include "modules/perception/obstacle/onboard/motion_service.h"
 
 namespace apollo {
 namespace perception {
@@ -71,7 +73,10 @@ class LanePostProcessingSubnode : public Subnode {
   uint64_t min_processing_time_ = UINT64_MAX;
   uint64_t max_processing_time_ = 0;
   uint64_t tot_processing_time_ = 0;
+  CameraLanePostProcessOptions options_;
 
+  MotionService* motion_service_ = nullptr;
+  EventID motion_event_id_;
   DISALLOW_COPY_AND_ASSIGN(LanePostProcessingSubnode);
 };
 

modules/perception/obstacle/onboard/visualization_subnode.cc

@@ -104,7 +104,7 @@ bool VisualizationSubnode::InitInternal() {
   if (motion_event_id_ != -1) {
       motion_service_ = dynamic_cast<MotionService*>(
         DAGStreaming::GetSubnodeByName("MotionService"));
-    if (motion_service_ == NULL) {
+    if (motion_service_ == nullptr) {
       AERROR << "motion service not inited";
       return false;
     }
@@ -247,6 +247,7 @@ void VisualizationSubnode::SetFrameContent(const Event& event,
                                 (*(objs->camera_frame_supplement)));
   } else if (event.event_id == motion_event_id_) {
 //    AINFO << "Vis_subnode: motion_event_id_" << motion_event_id_;
+    // TODO(gchen-apollo): add lock to read motion_buffer
     MotionBufferPtr motion_buffer = motion_service_->GetMotionBuffer();
     if (motion_buffer == nullptr) {
       AINFO << "motion_buffer is null";