Perception: avoiding using bubble sort.

modules/perception/BUILD

@@ -15,13 +15,13 @@ cc_library(
         "//modules/perception/common",
         "//modules/perception/lib/base",
         "//modules/perception/obstacle/onboard:camera_subnode",
+        "//modules/perception/obstacle/onboard:cipv_subnode",
         "//modules/perception/obstacle/onboard:fusion_subnode",
         "//modules/perception/obstacle/onboard:lane_post_processing_subnode",
         "//modules/perception/obstacle/onboard:lidar_subnode",
         "//modules/perception/obstacle/onboard:perception_obstacle_shared_data",
         "//modules/perception/obstacle/onboard:radar_subnode",
         "//modules/perception/obstacle/onboard:visualization_subnode",
-        "//modules/perception/obstacle/onboard:cipv_subnode",
         "//modules/perception/onboard",
         "//modules/perception/proto:perception_proto",
         "//modules/perception/traffic_light/onboard",

modules/perception/model/traffic_light/reviser.config

@@ -5,7 +5,7 @@ model_configs {
         name: "param_file"
         value: "data/models/reviser/reviser.prototxt"
     }
-} 
+}
 model_configs {
     name: "ColorReviser"
     version: "1.0.0"

modules/perception/obstacle/fusion/probabilistic_fusion/BUILD

 load("//tools:cpplint.bzl", "cpplint")
-load("//tools:cpplint.bzl", "cpplint")
 
 package(default_visibility = ["//visibility:public"])
 

modules/perception/obstacle/fusion/probabilistic_fusion/pbf_sensor.cc

@@ -24,27 +24,25 @@ namespace perception {
 
 size_t PbfSensor::s_max_cached_frame_number_ = 10;
 
-PbfSensor::PbfSensor(const SensorType &type, const std::string &sensor_id)
-    : sensor_id_(sensor_id), sensor_type_(type), latest_query_timestamp_(0.0) {}
+PbfSensor::PbfSensor(const std::string &sensor_id, const SensorType &type)
+    : sensor_id_(sensor_id), sensor_type_(type) {}
 
 PbfSensor::~PbfSensor() {}
 
-void PbfSensor::QueryLatestFrames(double time_stamp,
+void PbfSensor::QueryLatestFrames(const double time_stamp,
                                   std::vector<PbfSensorFramePtr> *frames) {
-  if (frames == nullptr) {
-    return;
-  }
+  CHECK_NOTNULL(frames);
   frames->clear();
   for (size_t i = 0; i < frames_.size(); ++i) {
     if (frames_[i]->timestamp > latest_query_timestamp_ &&
         frames_[i]->timestamp <= time_stamp) {
-      (*frames).push_back(frames_[i]);
+      frames->push_back(frames_[i]);
     }
   }
   latest_query_timestamp_ = time_stamp;
 }
 
-PbfSensorFramePtr PbfSensor::QueryLatestFrame(double time_stamp) {
+PbfSensorFramePtr PbfSensor::QueryLatestFrame(const double time_stamp) {
   PbfSensorFramePtr latest_frame = nullptr;
   for (size_t i = 0; i < frames_.size(); ++i) {
     if (frames_[i]->timestamp > latest_query_timestamp_ &&
@@ -80,21 +78,19 @@ void PbfSensor::AddFrame(const SensorObjects &frame) {
   frames_.push_back(pbf_frame);
 }
 
-bool PbfSensor::GetPose(double time_stamp, Eigen::Matrix4d *pose) {
-  if (pose == nullptr) {
-    AERROR << "parameter pose is nullptr for output";
-    return false;
-  }
+bool PbfSensor::GetPose(const double time_stamp, const double time_range,
+                        Eigen::Matrix4d *pose) {
+  CHECK_NOTNULL(pose);
+  CHECK_GE(time_range, 0.0);
 
-  for (int i = frames_.size() - 1; i >= 0; i--) {
-    double time_diff = time_stamp - frames_[i]->timestamp;
-    if (fabs(time_diff) < 1.0e-3) {
-      *pose = frames_[i]->sensor2world_pose;
+  for (auto rit = frames_.rbegin(); rit != frames_.rend(); ++rit) {
+    const double time_diff = time_stamp - (*rit)->timestamp;
+    if (fabs(time_diff) < time_range) {
+      *pose = (*rit)->sensor2world_pose;
       return true;
     }
   }
   AERROR << "Failed to find velodyne2world pose for timestamp: " << time_stamp;
-
   return false;
 }
 

modules/perception/obstacle/fusion/probabilistic_fusion/pbf_sensor.h

@@ -16,9 +16,11 @@
 
 #ifndef MODULES_PERCEPTION_OBSTACLE_FUSION_PROBABILISTIC_FUSION_PBF_SENSOR_H_
 #define MODULES_PERCEPTION_OBSTACLE_FUSION_PROBABILISTIC_FUSION_PBF_SENSOR_H_
+
 #include <deque>
 #include <string>
 #include <vector>
+
 #include "modules/common/log.h"
 #include "modules/common/macro.h"
 #include "modules/perception/obstacle/base/object.h"
@@ -29,26 +31,31 @@ namespace perception {
 
 class PbfSensor {
  public:
-  explicit PbfSensor(const SensorType &type, const std::string &sensor_id);
+  explicit PbfSensor(const std::string &sensor_id, const SensorType &type);
   ~PbfSensor();
 
-  /* @brief query frames whose time stamp is in range (latest_fused_time_stamp_,
-   * time_stamp)
+  /*
+   * @brief query frames whose time stamp is in range (latest_fused_time_stamp_,
+   * time_stamp); update latest_query_timestamp_ by time_stamp.
    */
   void QueryLatestFrames(double time_stamp,
                          std::vector<PbfSensorFramePtr> *frames);
 
-  /**@brief query latest frame whose time stamp is in range
-   * (latest_fused_time_stamp_, time_stamp]*/
-  PbfSensorFramePtr QueryLatestFrame(double time_stamp);
+  /*
+   * @brief query latest frame whose time stamp is in range
+   * (latest_fused_time_stamp_, time_stamp]; update latest_query_timestamp_ by
+   * time_stamp
+   */
+  PbfSensorFramePtr QueryLatestFrame(const double time_stamp);
 
   /**@brief add a frame objects*/
   void AddFrame(const SensorObjects &frame);
 
   /**@brief query pose at time_stamp, return false if not found*/
-  bool GetPose(double time_stamp, Eigen::Matrix4d *pose);
+  bool GetPose(const double time_stamp, const double time_range,
+               Eigen::Matrix4d *pose);
 
-  static void SetMaxCachedFrameNumber(int number) {
+  static void SetMaxCachedFrameNumber(const int number) {
     s_max_cached_frame_number_ = number;
   }
 
@@ -62,7 +69,7 @@ class PbfSensor {
   /**@brief max size of frames_*/
   static size_t s_max_cached_frame_number_;
 
-  double latest_query_timestamp_;
+  double latest_query_timestamp_ = 0.0;
 
  private:
   PbfSensor();

modules/perception/obstacle/fusion/probabilistic_fusion/pbf_sensor_manager.cc

@@ -16,6 +16,8 @@
 
 #include "modules/perception/obstacle/fusion/probabilistic_fusion/pbf_sensor_manager.h"
 
+#include <algorithm>
+
 #include "modules/common/log.h"
 
 namespace apollo {
@@ -29,8 +31,7 @@ PbfSensorManager *PbfSensorManager::Instance() {
 PbfSensorManager::PbfSensorManager() {}
 
 PbfSensorManager::~PbfSensorManager() {
-  std::map<std::string, PbfSensor *>::iterator it = sensors_.begin();
-  for (; it != sensors_.end(); ++it) {
+  for (auto it = sensors_.begin(); it != sensors_.end(); ++it) {
     if (it->second != nullptr) {
       delete (it->second);
       it->second = nullptr;
@@ -45,12 +46,12 @@ bool PbfSensorManager::Init() {
   std::string sensor_id = GetSensorType(SensorType::VELODYNE_64);
   SensorType type = SensorType::VELODYNE_64;
 
-  PbfSensor *velodyne_64 = new PbfSensor(type, sensor_id);
+  PbfSensor *velodyne_64 = new PbfSensor(sensor_id, type);
   sensors_[sensor_id] = velodyne_64;
 
   sensor_id = GetSensorType(SensorType::RADAR);
   type = SensorType::RADAR;
-  PbfSensor *radar = new PbfSensor(type, sensor_id);
+  PbfSensor *radar = new PbfSensor(sensor_id, type);
   if (radar == nullptr) {
     AERROR << "Fail to create PbfSensor. sensor_id = " << sensor_id;
     return false;
@@ -74,7 +75,7 @@ void PbfSensorManager::AddSensorMeasurements(const SensorObjects &objects) {
   if (it == sensors_.end()) {
     AWARN << "Cannot find sensor " << sensor_id
           << " and create one in SensorManager";
-    sensor = new PbfSensor(type, sensor_id);
+    sensor = new PbfSensor(sensor_id, type);
     if (sensor == nullptr) {
       AERROR << "Fail to create PbfSensor. sensor_id = " << sensor_id;
       return;
@@ -87,12 +88,12 @@ void PbfSensorManager::AddSensorMeasurements(const SensorObjects &objects) {
 }
 
 void PbfSensorManager::GetLatestSensorFrames(
-    double time_stamp, const std::string &sensor_id,
+    const double time_stamp, const std::string &sensor_id,
     std::vector<PbfSensorFramePtr> *frames) {
   if (frames == nullptr) {
     return;
   }
-  std::map<std::string, PbfSensor *>::iterator it = sensors_.find(sensor_id);
+  auto it = sensors_.find(sensor_id);
   if (it == sensors_.end()) {
     return;
   }
@@ -105,24 +106,17 @@ void PbfSensorManager::GetLatestFrames(const double time_stamp,
     return;
   }
   frames->clear();
-  for (std::map<std::string, PbfSensor *>::iterator it = sensors_.begin();
-       it != sensors_.end(); ++it) {
+  for (auto it = sensors_.begin(); it != sensors_.end(); ++it) {
     PbfSensor *sensor = it->second;
     PbfSensorFramePtr frame = sensor->QueryLatestFrame(time_stamp);
     if (frame != nullptr) {
       frames->push_back(frame);
     }
   }
-  int frame_size = static_cast<int>(frames->size());
-  for (int i = 0; i < frame_size - 1; i++) {
-    for (int j = i + 1; j < frame_size; j++) {
-      if ((*frames)[j]->timestamp < (*frames)[i]->timestamp) {
-        PbfSensorFramePtr tf = (*frames)[i];
-        (*frames)[i] = (*frames)[j];
-        (*frames)[j] = tf;
-      }
-    }
-  }
+  std::sort(frames->begin(), frames->end(),
+            [](const PbfSensorFramePtr &p1, const PbfSensorFramePtr &p2) {
+              return p1->timestamp < p2->timestamp;
+            });
 }
 
 PbfSensor *PbfSensorManager::GetSensor(const std::string &sensor_id) {
@@ -135,7 +129,7 @@ PbfSensor *PbfSensorManager::GetSensor(const std::string &sensor_id) {
 }
 
 bool PbfSensorManager::GetPose(const std::string &sensor_id, double time_stamp,
-                               Eigen::Matrix4d *pose) {
+                               const double time_range, Eigen::Matrix4d *pose) {
   if (pose == nullptr) {
     AERROR << "output parameter pose is nullptr";
     return false;
@@ -148,7 +142,7 @@ bool PbfSensorManager::GetPose(const std::string &sensor_id, double time_stamp,
   }
 
   PbfSensor *sensor = it->second;
-  return sensor->GetPose(time_stamp, pose);
+  return sensor->GetPose(time_stamp, time_range, pose);
 }
 
 }  // namespace perception

modules/perception/obstacle/fusion/probabilistic_fusion/pbf_sensor_manager.h

@@ -35,19 +35,22 @@ class PbfSensorManager {
 
   void AddSensorMeasurements(const SensorObjects &objects);
 
-  void GetLatestSensorFrames(double time_stamp, const std::string &sensor_id,
+  void GetLatestSensorFrames(const double time_stamp,
+                             const std::string &sensor_id,
                              std::vector<PbfSensorFramePtr> *frames);
 
-  /**@brief query one closest sensor frame for each sensor between last query
-     timestamp and
-     current timestamp, stored in ascending order of the frame timestamp */
+  /*
+   * @brief query one closest sensor frame for each sensor between last query
+   * timestamp and current timestamp, stored in ascending order of the frame
+   * timestamp
+   */
   void GetLatestFrames(const double time_stamp,
                        std::vector<PbfSensorFramePtr> *frames);
 
   PbfSensor *GetSensor(const std::string &sensor_id);
 
   bool GetPose(const std::string &sensor_id, double time_stamp,
-               Eigen::Matrix4d *pose);
+               const double time_range, Eigen::Matrix4d *pose);
 
  protected:
   bool Init();
@@ -65,4 +68,6 @@ class PbfSensorManager {
 }  // namespace perception
 }  // namespace apollo
 
+/* clang-format off */
 #endif  // MODULES_PERCEPTION_OBSTACLE_FUSION_PROBABILISTIC_FUSION_PBF_SENSOR_MANAGER_H_ // NOLINT
+/* clang-format on */

modules/perception/obstacle/fusion/probabilistic_fusion/pbf_sensor_manager_test.cc

@@ -40,8 +40,9 @@ TEST(PbfSensorManagerTest, pbf_sensor_frame_manage_test) {
   sensor_manager->AddSensorMeasurements(lidar_frame);
   EXPECT_TRUE(sensor_manager->GetSensor(lidar_name) != nullptr);
   Eigen::Matrix4d pose;
-  EXPECT_TRUE(
-      sensor_manager->GetPose(lidar_name, lidar_frame.timestamp, &pose));
+  const double kEpsilon = 1e-3;
+  EXPECT_TRUE(sensor_manager->GetPose(lidar_name, lidar_frame.timestamp,
+                                      kEpsilon, &pose));
   sensor_manager->AddSensorMeasurements(radar_frame);
   std::vector<PbfSensorFramePtr> frames;
   sensor_manager->GetLatestFrames(radar_frame.timestamp, &frames);