diff --git a/modules/common/util/util.h b/modules/common/util/util.h index 93e34854ed59a25c435281caa3fa33cdcedbe775..060955fcb1d4602224b6f494f4692a371405b924 100644 --- a/modules/common/util/util.h +++ b/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 +auto erase_where(Container& c, F&& f) { + return c.erase(std::remove_if(c.begin(), c.end(), std::forward(f)), + c.end()); +} + } // namespace util } // namespace common } // namespace apollo diff --git a/modules/perception/obstacle/camera/cipv/cipv.cc b/modules/perception/obstacle/camera/cipv/cipv.cc index 45952b0d332c002a7d9c597f383a5ead1b93a387..6814ace4a5107dfc52311042e78aa3cf294e2c65 100644 --- a/modules/perception/obstacle/camera/cipv/cipv.cc +++ b/modules/perception/obstacle/camera/cipv/cipv.cc @@ -14,8 +14,10 @@ * limitations under the License. *****************************************************************************/ -#include #include "modules/perception/obstacle/camera/cipv/cipv.h" + +#include + #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 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 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(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 sensor_objects, return true; } -std::string Cipv::name() const { - return "Cipv"; -} +std::string Cipv::name() const { return "Cipv"; } // Register plugin. // REGISTER_CIPV(Cipv); diff --git a/modules/perception/obstacle/camera/cipv/cipv.h b/modules/perception/obstacle/camera/cipv/cipv.h index b5d8bedc6bee98be673097e8657e6c080c16250f..1fb6e02b3b3ad866869ad0d85ba5d68dc83ffeb5 100644 --- a/modules/perception/obstacle/camera/cipv/cipv.h +++ b/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 -#include -#include #include #include #include + +#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_ diff --git a/modules/perception/obstacle/camera/visualizer/frame_content.cc b/modules/perception/obstacle/camera/visualizer/frame_content.cc index 181c70421b6c0598ec588002242307255ecf1f7f..0367edaac659becaf541c930a1d98523084b6170 100644 --- a/modules/perception/obstacle/camera/visualizer/frame_content.cc +++ b/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 #include #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::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::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::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::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::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::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; } } } diff --git a/modules/perception/obstacle/camera/visualizer/frame_content.h b/modules/perception/obstacle/camera/visualizer/frame_content.h index d92050fcb7dd0f4482e110f6eb3475030112b045..f0dbe27ef5cb16f65d7307c664d0ef3a8aae9c5c 100644 --- a/modules/perception/obstacle/camera/visualizer/frame_content.h +++ b/modules/perception/obstacle/camera/visualizer/frame_content.h @@ -20,11 +20,11 @@ #include #include #include +#include #include #include #include #include -#include #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 get_camera_objects(); std::vector 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; }*/