From 55f291efcf0a0476c9505e890b9d832ed49541b0 Mon Sep 17 00:00:00 2001 From: Tae Eun Choe Date: Fri, 28 Jun 2019 15:07:17 -0700 Subject: [PATCH] Perception: Make visualizer full screen; Changed parameter of CIPO (#9051) Percpetion: typo --- modules/perception/camera/app/cipv_camera.cc | 18 +++++++++++------- modules/perception/camera/app/cipv_camera.h | 10 ++++++++-- modules/perception/camera/common/lane_object.h | 2 +- .../camera/tools/offline/visualizer.cc | 10 ++++++++-- 4 files changed, 28 insertions(+), 12 deletions(-) diff --git a/modules/perception/camera/app/cipv_camera.cc b/modules/perception/camera/app/cipv_camera.cc index 036b62e8e1..e4bd1bee7d 100644 --- a/modules/perception/camera/app/cipv_camera.cc +++ b/modules/perception/camera/app/cipv_camera.cc @@ -208,7 +208,7 @@ bool Cipv::ElongateEgoLane(const std::vector &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 = half_virtual_egolane_width_in_meter_; + float offset_distance = half_vehicle_width_in_meter_; // When left lane line is available if (b_left_valid && b_right_valid) { // elongate both lanes or do nothing @@ -676,13 +676,16 @@ bool Cipv::IsObjectInTheLaneImage(const std::shared_ptr &object, bool Cipv::IsObjectInTheLaneGround(const std::shared_ptr &object, const EgoLane &egolane_ground, const Eigen::Affine3d world2camera, + const bool b_virtual, float *object_distance) { LineSegment2Df closted_object_edge; bool b_left_lane_clear = false; bool b_right_lane_clear = false; float shortest_distance = 0.0f; float distance = 0.0f; - + float max_dist_object_to_lane_in_meter = + b_virtual ? max_dist_object_to_virtual_lane_in_meter_ : + max_dist_object_to_lane_in_meter_; int closest_index = -1; // Find closest edge of a given object bounding box float b_valid_object = FindClosestObjectGround( @@ -734,7 +737,7 @@ bool Cipv::IsObjectInTheLaneGround(const std::shared_ptr &object, closted_object_edge.end_point, egolane_ground.left_line.line_point[closest_index], egolane_ground.left_line.line_point[closest_index + 1]) && - shortest_distance < max_dist_object_to_lane_in_meter_) { + shortest_distance < max_dist_object_to_lane_in_meter) { b_left_lane_clear = true; } } @@ -776,7 +779,7 @@ bool Cipv::IsObjectInTheLaneGround(const std::shared_ptr &object, closted_object_edge.start_point, egolane_ground.right_line.line_point[closest_index], egolane_ground.right_line.line_point[closest_index + 1]) && - shortest_distance < max_dist_object_to_lane_in_meter_) { + shortest_distance < max_dist_object_to_lane_in_meter) { b_right_lane_clear = true; } } @@ -789,12 +792,13 @@ bool Cipv::IsObjectInTheLane(const std::shared_ptr &object, const EgoLane &egolane_image, const EgoLane &egolane_ground, const Eigen::Affine3d world2camera, + const bool b_virtual, float *distance) { if (b_image_based_cipv_) { return IsObjectInTheLaneImage(object, egolane_image, distance); } return IsObjectInTheLaneGround(object, egolane_ground, world2camera, - distance); + b_virtual, distance); } // ===================================================================== @@ -840,9 +844,9 @@ bool Cipv::DetermineCipv(const std::vector &lane_objects, AINFO << "objects[" << i << "]->track_id: " << (*objects)[i]->track_id; } if (IsObjectInTheLane((*objects)[i], egolane_image, egolane_ground, - world2camera, &distance) || + world2camera, false, &distance) || IsObjectInTheLane((*objects)[i], egolane_image, virtual_egolane_ground, - world2camera, &distance)) { + world2camera, true, &distance)) { if (cipv_index < 0 || distance < min_distance) { cipv_index = i; cipv_track_id = (*objects)[i]->track_id; diff --git a/modules/perception/camera/app/cipv_camera.h b/modules/perception/camera/app/cipv_camera.h index ca9a595660..ffbbeb1d44 100644 --- a/modules/perception/camera/app/cipv_camera.h +++ b/modules/perception/camera/app/cipv_camera.h @@ -41,8 +41,9 @@ struct CipvOptions { float yaw_angle = 0.0f; }; -constexpr float kMinVelocity = 15.0f; // in m/s +constexpr float kMinVelocity = 10.0f; // in m/s constexpr float kMaxDistObjectToLaneInMeter = 70.0f; +constexpr float kMaxDistObjectToVirtualLaneInMeter = 10.0f; constexpr float kMaxDistObjectToLaneInPixel = 10.0f; const std::size_t kDropsHistorySize = 20; const std::size_t kMaxObjectNum = 100; @@ -141,13 +142,16 @@ class Cipv { bool IsObjectInTheLaneGround(const std::shared_ptr &object, const EgoLane &egolane_ground, const Eigen::Affine3d world2camera, + const bool b_virtual, float *distance); // Check if the object is in the lane in ego-ground space bool IsObjectInTheLane(const std::shared_ptr &object, const EgoLane &egolane_image, const EgoLane &egolane_ground, - const Eigen::Affine3d world2camera, float *distance); + const Eigen::Affine3d world2camera, + const bool b_virtual, + float *distance); // Check if a point is left of a line segment bool IsPointLeftOfLine(const Point2Df &point, @@ -186,6 +190,8 @@ class Cipv { single_virtual_egolane_width_in_meter_ * 0.5f; float half_vehicle_width_in_meter_ = kMaxVehicleWidthInMeter * 0.5f; float max_dist_object_to_lane_in_meter_ = kMaxDistObjectToLaneInMeter; + float max_dist_object_to_virtual_lane_in_meter_ = + kMaxDistObjectToVirtualLaneInMeter; float max_dist_object_to_lane_in_pixel_ = kMaxDistObjectToLaneInPixel; float MAX_VEHICLE_WIDTH_METER = 5.0f; float EPSILON = 1.0e-6f; diff --git a/modules/perception/camera/common/lane_object.h b/modules/perception/camera/common/lane_object.h index 9ca55460b2..9b84c497d6 100644 --- a/modules/perception/camera/common/lane_object.h +++ b/modules/perception/camera/common/lane_object.h @@ -36,7 +36,7 @@ constexpr uint32_t kMinLaneLineLengthForCIPV = 2; // Average width of lane constexpr float kAverageLaneWidthInMeter = 3.7f; // Maximum vehicle width -constexpr float kMaxVehicleWidthInMeter = 2.5f; +constexpr float kMaxVehicleWidthInMeter = 1.87f; // Margin from a virtual car lane to actual lane constexpr float kMarginVehicleToLane = (kAverageLaneWidthInMeter - kMaxVehicleWidthInMeter) / 2.0f; diff --git a/modules/perception/camera/tools/offline/visualizer.cc b/modules/perception/camera/tools/offline/visualizer.cc index 727c993d0c..3c071ebe10 100644 --- a/modules/perception/camera/tools/offline/visualizer.cc +++ b/modules/perception/camera/tools/offline/visualizer.cc @@ -919,7 +919,10 @@ void Visualizer::ShowResult(const cv::Mat &img, const CameraFrame &frame) { } if (cv_imshow_img_) { - cv::imshow("", bigimg); + cv::namedWindow("Apollo Visualizer", CV_WINDOW_NORMAL); + cv::setWindowProperty("Apollo Visualizer", CV_WND_PROP_FULLSCREEN, + CV_WINDOW_FULLSCREEN); + cv::imshow("Apollo Visualizer", bigimg); int key = cvWaitKey(30); key_handler(camera_name, key); } @@ -1364,7 +1367,10 @@ void Visualizer::ShowResult_all_info_single_camera(const cv::Mat &img, } if (cv_imshow_img_) { - cv::imshow("", bigimg); + cv::namedWindow("Apollo Visualizer", CV_WINDOW_NORMAL); + cv::setWindowProperty("Apollo Visualizer", CV_WND_PROP_FULLSCREEN, + CV_WINDOW_FULLSCREEN); + cv::imshow("Apollo Visualizer", bigimg); int key = cvWaitKey(30); key_handler(camera_name, key); } -- GitLab