Perception: code changes.

modules/common/util/util.h

@@ -32,11 +32,12 @@
 
 #include "google/protobuf/util/message_differencer.h"
 
-#include "modules/common/math/vec2d.h"
 #include "modules/common/proto/geometry.pb.h"
 #include "modules/common/proto/pnc_point.pb.h"
 #include "modules/perception/proto/perception_obstacle.pb.h"
 
+#include "modules/common/math/vec2d.h"
+
 /**
  * @namespace apollo::common::util
  * @brief apollo::common::util
@@ -161,6 +162,14 @@ PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
                                             const PathPoint& p2,
                                             const double w1, const double w2);
 
+// a wrapper template function for remove_if (notice that remove_if cannot
+// change the Container size)
+template <class Container, class F>
+auto erase_where(Container& c, F&& f) {
+  return c.erase(std::remove_if(c.begin(), c.end(), std::forward<F>(f)),
+                 c.end());
+}
+
 }  // namespace util
 }  // namespace common
 }  // namespace apollo

modules/perception/obstacle/camera/cipv/cipv.cc

@@ -14,8 +14,10 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include <math.h>
 #include "modules/perception/obstacle/camera/cipv/cipv.h"
+
+#include <math.h>
+
 #include "modules/common/log.h"
 
 namespace apollo {
@@ -80,9 +82,9 @@ bool Cipv::distance_from_point_to_line_segment(
   *distance = sqrt(dx * dx + dy * dy);
   if (_debug_level >= 2) {
     AINFO << "distance_from_point (" << point[0] << ", " << point[1]
-               << ") _to_line_segment (" << line_seg_start_point[0] << ", "
-               << line_seg_start_point[1] << ")->(" << line_seg_end_point[0]
-               << ", " << line_seg_end_point[1] << "): " << *distance << "m";
+          << ") _to_line_segment (" << line_seg_start_point[0] << ", "
+          << line_seg_start_point[1] << ")->(" << line_seg_end_point[0] << ", "
+          << line_seg_end_point[1] << "): " << *distance << "m";
   }
   return true;
 }
@@ -95,7 +97,7 @@ bool Cipv::get_egolane(const LaneObjectsPtr lane_objects,
     if ((*lane_objects)[i].spatial == L_0) {
       if (_debug_level >= 2) {
         AINFO << "[get_egolane]LEFT(*lane_objects)[i].image_pos.size(): "
-                   << (*lane_objects)[i].image_pos.size();
+              << (*lane_objects)[i].image_pos.size();
       }
       if ((*lane_objects)[i].image_pos.size() <
           MIN_LANE_LINE_LENGTH_FOR_CIPV_DETERMINATION) {
@@ -116,7 +118,7 @@ bool Cipv::get_egolane(const LaneObjectsPtr lane_objects,
     } else if ((*lane_objects)[i].spatial == R_0) {
       if (_debug_level >= 2) {
         AINFO << "[get_egolane]RIGHT(*lane_objects)[i].image_pos.size(): "
-                   << (*lane_objects)[i].image_pos.size();
+              << (*lane_objects)[i].image_pos.size();
       }
       if ((*lane_objects)[i].image_pos.size() <
           MIN_LANE_LINE_LENGTH_FOR_CIPV_DETERMINATION) {
@@ -210,11 +212,10 @@ bool Cipv::make_virtual_ego_lane_from_yaw_rate(const float yaw_rate,
 }
 
 // Elongate lane line
-bool Cipv::elongate_egolane(
-    const LaneObjectsPtr lane_objects,
-    const bool b_left_valid, const bool b_right_valid,
-    const float yaw_rate, const float velocity,
-    EgoLane *egolane_image, EgoLane *egolane_ground) {
+bool Cipv::elongate_egolane(const LaneObjectsPtr lane_objects,
+                            const bool b_left_valid, const bool b_right_valid,
+                            const float yaw_rate, const float velocity,
+                            EgoLane *egolane_image, EgoLane *egolane_ground) {
   float offset_distance = EGO_CAR_HALF_VIRTUAL_LANE;
   // When left lane line is available
   if (b_left_valid && b_right_valid) {
@@ -427,9 +428,9 @@ bool Cipv::find_closest_edge_of_object_ground(
 
   if (_debug_level >= 2) {
     AINFO << "start(" << closted_object_edge->start_point[0] << ", "
-               << closted_object_edge->start_point[1] << ")->";
+          << closted_object_edge->start_point[1] << ")->";
     AINFO << "end(" << closted_object_edge->end_point[0] << ", "
-               << closted_object_edge->end_point[1] << ")";
+          << closted_object_edge->end_point[1] << ")";
   }
   return true;
 }
@@ -443,28 +444,28 @@ bool Cipv::are_distances_sane(const float distance_start_point_to_right_lane,
   if (distance_start_point_to_right_lane > MAX_DIST_OBJECT_TO_LANE_METER) {
     if (_debug_level >= 1) {
       AINFO << "distance from start to right lane(" << distance
-                 << " m) is too long";
+            << " m) is too long";
     }
     return false;
   }
   if (distance_start_point_to_left_lane > MAX_DIST_OBJECT_TO_LANE_METER) {
     if (_debug_level >= 1) {
       AINFO << "distance from start to left lane(" << distance
-                 << " m) is too long";
+            << " m) is too long";
     }
     return false;
   }
   if (distance_end_point_to_right_lane > MAX_DIST_OBJECT_TO_LANE_METER) {
     if (_debug_level >= 1) {
       AINFO << "distance from end to right lane(" << distance
-                 << " m) is too long";
+            << " m) is too long";
     }
     return false;
   }
   if (distance_end_point_to_left_lane > MAX_DIST_OBJECT_TO_LANE_METER) {
     if (_debug_level >= 1) {
       AINFO << "distance from end to left lane(" << distance
-                 << " m) is too long";
+            << " m) is too long";
     }
     return false;
   }
@@ -504,19 +505,19 @@ bool Cipv::is_point_left_of_line(const Point2Df &point,
   if (cross_product > 0.0f) {
     if (_debug_level >= 2) {
       AINFO << "point (" << point[0] << ", " << point[1]
-                 << ") is left of line_segment (" << line_seg_start_point[0]
-                 << ", " << line_seg_start_point[1] << ")->("
-                 << line_seg_end_point[0] << ", " << line_seg_end_point[1]
-                 << "), cross_product: " << cross_product;
+            << ") is left of line_segment (" << line_seg_start_point[0] << ", "
+            << line_seg_start_point[1] << ")->(" << line_seg_end_point[0]
+            << ", " << line_seg_end_point[1]
+            << "), cross_product: " << cross_product;
     }
     return true;
   } else {
     if (_debug_level >= 2) {
       AINFO << "point (" << point[0] << ", " << point[1]
-                 << ") is right of line_segment (" << line_seg_start_point[0]
-                 << ", " << line_seg_start_point[1] << ")->("
-                 << line_seg_end_point[0] << ", " << line_seg_end_point[1]
-                 << "), cross_product: " << cross_product;
+            << ") is right of line_segment (" << line_seg_start_point[0] << ", "
+            << line_seg_start_point[1] << ")->(" << line_seg_end_point[0]
+            << ", " << line_seg_end_point[1]
+            << "), cross_product: " << cross_product;
     }
     return false;
   }
@@ -561,7 +562,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
 
   if (_debug_level >= 3) {
     AINFO << "egolane_ground.left_line.line_point.size(): "
-               << egolane_ground.left_line.line_point.size();
+          << egolane_ground.left_line.line_point.size();
   }
   if (egolane_ground.left_line.line_point.size() <= 1) {
     if (_debug_level >= 1) {
@@ -591,7 +592,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
     // Check if the end point is on the right of the line segment
     if (_debug_level >= 3) {
       AINFO << "[Left] closest_index: " << closest_index
-                 << ", shortest_distance: " << shortest_distance;
+            << ", shortest_distance: " << shortest_distance;
     }
     if (is_point_left_of_line(
             closted_object_edge.end_point,
@@ -603,7 +604,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
 
   if (_debug_level >= 3) {
     AINFO << "egolane_ground.right_line.line_point.size(): "
-               << egolane_ground.right_line.line_point.size();
+          << egolane_ground.right_line.line_point.size();
   }
   // Check start_point and right lane
   if (egolane_ground.right_line.line_point.size() <= 1) {
@@ -631,7 +632,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
   if (closest_index >= 0) {
     if (_debug_level >= 3) {
       AINFO << "[right] closest_index: " << closest_index
-                 << ", shortest_distance: " << shortest_distance;
+            << ", shortest_distance: " << shortest_distance;
     }
     // Check if the end point is on the right of the line segment
     if (is_point_left_of_line(
@@ -672,10 +673,9 @@ bool Cipv::is_object_in_the_lane(const ObjectPtr &object,
 bool Cipv::determine_cipv(std::shared_ptr<SensorObjects> sensor_objects,
                           CipvOptions *options) {
   if (_debug_level >= 3) {
-    AINFO << "Cipv Got SensorObjects size "
-               << sensor_objects->objects.size();
+    AINFO << "Cipv Got SensorObjects size " << sensor_objects->objects.size();
     AINFO << "Cipv Got lane object size "
-               << sensor_objects->lane_objects->size();
+          << sensor_objects->lane_objects->size();
   }
   float yaw_rate = options->yaw_rate;
   float velocity = options->yaw_rate;
@@ -693,16 +693,14 @@ bool Cipv::determine_cipv(std::shared_ptr<SensorObjects> sensor_objects,
   // Get ego lanes (in both image and ground coordinate)
   get_egolane(sensor_objects->lane_objects, &egolane_image, &egolane_ground,
               &b_left_valid, &b_right_valid);
-  elongate_egolane(sensor_objects->lane_objects,
-                   b_left_valid, b_right_valid,
-                   yaw_rate, velocity,
-                   &egolane_image, &egolane_ground);
+  elongate_egolane(sensor_objects->lane_objects, b_left_valid, b_right_valid,
+                   yaw_rate, velocity, &egolane_image, &egolane_ground);
 
   for (int32_t i = 0; i < static_cast<int32_t>(sensor_objects->objects.size());
        i++) {
     if (_debug_level >= 2) {
       AINFO << "sensor_objects->objects[i]->track_id: "
-                 << sensor_objects->objects[i]->track_id;
+            << sensor_objects->objects[i]->track_id;
     }
     if (is_object_in_the_lane(sensor_objects->objects[i], egolane_image,
                               egolane_ground) == true) {
@@ -761,9 +759,7 @@ bool Cipv::determine_cipv(std::shared_ptr<SensorObjects> sensor_objects,
   return true;
 }
 
-std::string Cipv::name() const {
-  return "Cipv";
-}
+std::string Cipv::name() const { return "Cipv"; }
 
 // Register plugin.
 // REGISTER_CIPV(Cipv);

modules/perception/obstacle/camera/cipv/cipv.h

@@ -14,15 +14,17 @@
  * limitations under the License.
  *****************************************************************************/
 
-#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
-#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
+#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_
+#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_
 
-#include <Eigen/Dense>
-#include <Eigen/Eigen>
-#include <Eigen/Geometry>
 #include <array>
 #include <memory>
 #include <string>
+
+#include "Eigen/Dense"
+#include "Eigen/Eigen"
+#include "Eigen/Geometry"
+
 #include "modules/perception/obstacle/base/object.h"
 #include "modules/perception/obstacle/camera/common/lane_object.h"
 #include "modules/perception/obstacle/camera/lane_post_process/common/type.h"
@@ -75,9 +77,9 @@ class Cipv {
                                            float *distance);
 
   // Determine CIPV among multiple objects
-  bool get_egolane(const LaneObjectsPtr lane_objects,
-                   EgoLane *egolane_image, EgoLane *egolane_ground,
-                   bool *b_left_valid, bool *b_right_valid);
+  bool get_egolane(const LaneObjectsPtr lane_objects, EgoLane *egolane_image,
+                   EgoLane *egolane_ground, bool *b_left_valid,
+                   bool *b_right_valid);
 
   // Elongate lane line
   bool elongate_egolane(const LaneObjectsPtr lane_objects,
@@ -132,8 +134,8 @@ class Cipv {
                          LaneLine *virtual_lane_line);
 
   float vehicle_dynamics(const uint32_t tick, const float yaw_rate,
-                         const float velocity, const float time_unit,
-                         float *x, float *y);
+                         const float velocity, const float time_unit, float *x,
+                         float *y);
   // Make a virtual lane line using a yaw_rate
   bool make_virtual_ego_lane_from_yaw_rate(const float yaw_rate,
                                            const float velocity,
@@ -141,13 +143,13 @@ class Cipv {
                                            LaneLine *left_lane_line,
                                            LaneLine *right_lane_line);
   // Member variables
-  bool _b_image_based_cipv;
-  int32_t _debug_level;
-  float _time_unit;
+  bool _b_image_based_cipv = false;
+  int32_t _debug_level = 0;
+  float _time_unit = 0.0f;
 };
 
 }  // namespace obstacle
 }  // namespace perception
 }  // namespace apollo
 
-#endif  // MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
+#endif  // MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_

modules/perception/obstacle/camera/visualizer/frame_content.cc

@@ -14,10 +14,10 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "modules/perception/obstacle/camera/visualizer/frame_content.h"
 #include <Eigen/LU>
 #include <map>
 #include "modules/common/log.h"
+#include "modules/perception/obstacle/camera/visualizer/frame_content.h"
 
 namespace apollo {
 namespace perception {
@@ -143,7 +143,7 @@ void FrameContent::update_timestamp(double ref) {
     }
     best_ts = FLT_MAX;
     for (std::map<double, ImageContent>::iterator it = _image_caches.begin();
-         it != _image_caches.end(); it++) {
+         it != _image_caches.end(); ++it) {
       double it_ts = it->first;
       if (it_ts < best_ts && _current_image_timestamp != it_ts) {
         best_ts = it_ts;
@@ -157,7 +157,7 @@ void FrameContent::update_timestamp(double ref) {
     if (it->first < _current_image_timestamp) {
       _image_caches.erase(it++);
     } else {
-      it++;
+      ++it;
     }
   }
 
@@ -171,7 +171,7 @@ void FrameContent::update_timestamp(double ref) {
   best_ts = -1;
 
   for (std::map<double, RadarContent>::iterator it = _radar_caches.begin();
-       it != _radar_caches.end(); it++) {
+       it != _radar_caches.end(); ++it) {
     double it_ts = it->first;
     double delta = fabs(it_ts - ref);
 
@@ -186,13 +186,13 @@ void FrameContent::update_timestamp(double ref) {
     if (it->first < best_ts) {
       _radar_caches.erase(it++);
     } else {
-      it++;
+      ++it;
     }
   }
   best_delta = FLT_MAX;
   best_ts = -1;
   for (std::map<double, FusionContent>::iterator it = _fusion_caches.begin();
-       it != _fusion_caches.end(); it++) {
+       it != _fusion_caches.end(); ++it) {
     double it_ts = it->first;
     double delta = fabs(it_ts - ref);
 
@@ -207,7 +207,7 @@ void FrameContent::update_timestamp(double ref) {
     if (it->first < best_ts) {
       _fusion_caches.erase(it++);
     } else {
-      it++;
+      ++it;
     }
   }
 
@@ -215,7 +215,7 @@ void FrameContent::update_timestamp(double ref) {
   best_delta = FLT_MAX;
   best_ts = -1;
   for (std::map<double, CameraContent>::iterator it = _camera_caches.begin();
-       it != _camera_caches.end(); it++) {
+       it != _camera_caches.end(); ++it) {
     double it_ts = it->first;
     double delta = fabs(it_ts - ref);
 
@@ -230,14 +230,14 @@ void FrameContent::update_timestamp(double ref) {
     if (it->first < best_ts) {
       _camera_caches.erase(it++);
     } else {
-      it++;
+      ++it;
     }
   }
 
   best_delta = FLT_MAX;
   best_ts = -1;
   for (std::map<double, GroundTruthContent>::iterator it = _gt_caches.begin();
-       it != _gt_caches.end(); it++) {
+       it != _gt_caches.end(); ++it) {
     double it_ts = it->first;
     double delta = fabs(it_ts - ref);
     if (delta < best_delta) {
@@ -251,7 +251,7 @@ void FrameContent::update_timestamp(double ref) {
     if (it->first < best_ts) {
       _gt_caches.erase(it++);
     } else {
-      it++;
+      ++it;
     }
   }
 
@@ -259,7 +259,7 @@ void FrameContent::update_timestamp(double ref) {
   best_delta = FLT_MAX;
   best_ts = -1;
   for (std::map<double, MotionContent>::iterator it = _motion_caches.begin();
-       it != _motion_caches.end(); it++) {
+       it != _motion_caches.end(); ++it) {
     double it_ts = it->first;
     double delta = fabs(it_ts - ref);
 
@@ -274,7 +274,7 @@ void FrameContent::update_timestamp(double ref) {
     if (it->first < best_ts) {
       _motion_caches.erase(it++);
     } else {
-      it++;
+      ++it;
     }
   }
 }

modules/perception/obstacle/camera/visualizer/frame_content.h

@@ -20,11 +20,11 @@
 #include <boost/shared_ptr.hpp>
 #include <deque>
 #include <iomanip>
+#include <map>
 #include <sstream>
 #include <string>
 #include <thread>
 #include <vector>
-#include <map>
 
 #include "modules/perception/obstacle/base/object.h"
 #include "modules/perception/obstacle/base/object_supplement.h"
@@ -129,27 +129,19 @@ class FrameContent {
   Eigen::Matrix4d get_pose_v2w();
   cv::Mat get_camera_image();
 
-  int get_pose_type() {
-    return _continuous_type;
-  }
+  int get_pose_type() { return _continuous_type; }
 
-  void set_pose_type(int type) {
-    _continuous_type = type;
-  }
+  void set_pose_type(int type) { _continuous_type = type; }
 
   std::vector<ObjectPtr> get_camera_objects();
   std::vector<ObjectPtr> get_radar_objects();
   double get_visualization_timestamp();
 
-  inline bool has_radar_data() {
-    return _radar_caches.size();
-  }
+  inline bool has_radar_data() { return _radar_caches.size(); }
 
   CameraFrameSupplementPtr get_camera_frame_supplement();
 
-  inline bool has_camera_data() {
-    return _camera_caches.size();
-  }
+  inline bool has_camera_data() { return _camera_caches.size(); }
   /*   inline void set_camera2velo_pose(const Eigen::Matrix4d& pose) {
          _pose_camera2velo = pose;
      }*/