From 49647ab0f8ff0e8349f7099b8487f5245a10e293 Mon Sep 17 00:00:00 2001 From: SeasoulChris <274224843@qq.com> Date: Tue, 18 May 2021 20:53:03 +0800 Subject: [PATCH] Perception: add gflag to make lidar height file could be specified (#13745) * Revert "Perception: make lidar height file could be specified (#13226)" This reverts commit 210996789bd564aa5fb2c223d5c976dcf1455dfa. * add gflag to make lidar height could be specified --- .../perception/common/perception_gflags.cc | 4 + modules/perception/common/perception_gflags.h | 3 + .../fusion_camera_detection_component.cc | 80 +++++++++---------- .../fusion_camera_detection_component.h | 4 - .../component/lane_detection_component.cc | 11 ++- .../fusion_camera_detection_component.proto | 3 +- 6 files changed, 52 insertions(+), 53 deletions(-) diff --git a/modules/perception/common/perception_gflags.cc b/modules/perception/common/perception_gflags.cc index 7b9225d43d..405d63ef02 100644 --- a/modules/perception/common/perception_gflags.cc +++ b/modules/perception/common/perception_gflags.cc @@ -117,5 +117,9 @@ DEFINE_string(torch_detector_model, "/apollo/modules/perception/camera" "/lib/obstacle/detector/yolov4/model/yolov4.pt", "The torch model file for emergency detection"); + +// lidar sensor name +DEFINE_string(lidar_sensor_name, "velodyne128", + "lidar sensor name"); } // namespace perception } // namespace apollo diff --git a/modules/perception/common/perception_gflags.h b/modules/perception/common/perception_gflags.h index bb2578b39f..8131b8812d 100644 --- a/modules/perception/common/perception_gflags.h +++ b/modules/perception/common/perception_gflags.h @@ -67,5 +67,8 @@ DECLARE_int32(num_classes); // emergency detection libtorch DECLARE_string(torch_detector_model); + +// lidar sensor name +DECLARE_string(lidar_sensor_name); } // namespace perception } // namespace apollo diff --git a/modules/perception/onboard/component/fusion_camera_detection_component.cc b/modules/perception/onboard/component/fusion_camera_detection_component.cc index 40f193f569..ebf676b22b 100644 --- a/modules/perception/onboard/component/fusion_camera_detection_component.cc +++ b/modules/perception/onboard/component/fusion_camera_detection_component.cc @@ -65,6 +65,39 @@ static int GetGpuId(const camera::CameraPerceptionInitOptions &options) { return perception_param.gpu_id(); } +bool SetCameraHeight(const std::string &sensor_name, + const std::string ¶ms_dir, + const std::string &lidar_sensor_name, + float default_camera_height, + float *camera_height) { + float base_h = default_camera_height; + float camera_offset = 0.0f; + try { + YAML::Node lidar_height = + YAML::LoadFile( + params_dir + "/" + lidar_sensor_name + "_height.yaml"); + base_h = lidar_height["vehicle"]["parameters"]["height"].as(); + AINFO << base_h; + YAML::Node camera_ex = + YAML::LoadFile(params_dir + "/" + sensor_name + "_extrinsics.yaml"); + camera_offset = camera_ex["transform"]["translation"]["z"].as(); + AINFO << camera_offset; + *camera_height = base_h + camera_offset; + } catch (YAML::InvalidNode &in) { + AERROR << "load camera extrisic file error, YAML::InvalidNode exception"; + return false; + } catch (YAML::TypedBadConversion &bc) { + AERROR << "load camera extrisic file error, " + << "YAML::TypedBadConversion exception"; + return false; + } catch (YAML::Exception &e) { + AERROR << "load camera extrisic file " + << " error, YAML exception:" << e.what(); + return false; + } + return true; +} + // @description: load camera extrinsics from yaml file bool LoadExtrinsics(const std::string &yaml_file, Eigen::Matrix4d *camera_extrinsic) { @@ -202,9 +235,9 @@ bool FusionCameraDetectionComponent::Init() { // load in lidar to imu extrinsic Eigen::Matrix4d ex_lidar2imu; LoadExtrinsics(FLAGS_obs_sensor_intrinsic_path + "/" + - "velodyne128_novatel_extrinsics.yaml", + FLAGS_lidar_sensor_name + "_novatel_extrinsics.yaml", &ex_lidar2imu); - AINFO << "velodyne128_novatel_extrinsics: " << ex_lidar2imu; + AINFO << FLAGS_lidar_sensor_name + "_novatel_extrinsics.yaml" << ex_lidar2imu; ACHECK(visualize_.Init_all_info_single_camera( camera_names_, visual_camera_, intrinsic_map_, extrinsic_map_, @@ -522,7 +555,8 @@ int FusionCameraDetectionComponent::InitCameraFrames() { for (const auto &camera_name : camera_names_) { float height = 0.0f; SetCameraHeight(camera_name, FLAGS_obs_sensor_intrinsic_path, - default_camera_height_, &height); + FLAGS_lidar_sensor_name, default_camera_height_, + &height); camera_height_map_[camera_name] = height; } @@ -1111,46 +1145,6 @@ int FusionCameraDetectionComponent::MakeCameraDebugMsg( return cyber::SUCC; } -bool FusionCameraDetectionComponent::SetCameraHeight( - const std::string &sensor_name, - const std::string ¶ms_dir, float default_camera_height, - float *camera_height) { - float base_height = default_camera_height; - float camera_offset = 0.0f; - - apollo::perception::onboard::FusionCameraDetection - fusion_camera_detection_param; - if (!GetProtoConfig(&fusion_camera_detection_param)) { - AINFO << "load fusion camera detection component proto param failed"; - return false; - } - std::string lidar_type = - fusion_camera_detection_param.lidar_type(); - try { - YAML::Node lidar_height = - YAML::LoadFile(params_dir + "/" + lidar_type + "_height.yaml"); - base_height = lidar_height["vehicle"]["parameters"]["height"].as(); - AINFO << base_height; - YAML::Node camera_ex = - YAML::LoadFile(params_dir + "/" + sensor_name + "_extrinsics.yaml"); - camera_offset = camera_ex["transform"]["translation"]["z"].as(); - AINFO << camera_offset; - *camera_height = base_height + camera_offset; - } catch (YAML::InvalidNode &in) { - AERROR << "load camera extrisic file error, YAML::InvalidNode exception"; - return false; - } catch (YAML::TypedBadConversion &bc) { - AERROR << "load camera extrisic file error, " - << "YAML::TypedBadConversion exception"; - return false; - } catch (YAML::Exception &e) { - AERROR << "load camera extrisic file " - << " error, YAML exception:" << e.what(); - return false; - } - return true; -} - } // namespace onboard } // namespace perception } // namespace apollo diff --git a/modules/perception/onboard/component/fusion_camera_detection_component.h b/modules/perception/onboard/component/fusion_camera_detection_component.h index c98d1981aa..4b500a3ab1 100644 --- a/modules/perception/onboard/component/fusion_camera_detection_component.h +++ b/modules/perception/onboard/component/fusion_camera_detection_component.h @@ -76,10 +76,6 @@ class FusionCameraDetectionComponent : public apollo::cyber::Component<> { int InitMotionService(); void SetCameraHeightAndPitch(); void OnMotionService(const MotionServiceMsgType& message); - bool SetCameraHeight( - const std::string &sensor_name, - const std::string ¶ms_dir, float default_camera_height, - float *camera_height); int InternalProc( const std::shared_ptr& in_message, diff --git a/modules/perception/onboard/component/lane_detection_component.cc b/modules/perception/onboard/component/lane_detection_component.cc index 6cb1f2a270..015a90cc13 100644 --- a/modules/perception/onboard/component/lane_detection_component.cc +++ b/modules/perception/onboard/component/lane_detection_component.cc @@ -73,12 +73,13 @@ static int GetGpuId(const camera::CameraPerceptionInitOptions &options) { static bool SetCameraHeight(const std::string &sensor_name, const std::string ¶ms_dir, + const std::string &lidar_sensor_name, float default_camera_height, float *camera_height) { float base_h = default_camera_height; float camera_offset = 0.0f; try { YAML::Node lidar_height = - YAML::LoadFile(params_dir + "/" + "velodyne128_height.yaml"); + YAML::LoadFile(params_dir + "/" + lidar_sensor_name + "_height.yaml"); base_h = lidar_height["vehicle"]["parameters"]["height"].as(); AINFO << base_h; YAML::Node camera_ex = @@ -208,9 +209,10 @@ bool LaneDetectionComponent::Init() { // load in lidar to imu extrinsic Eigen::Matrix4d ex_lidar2imu; LoadExtrinsics(FLAGS_obs_sensor_intrinsic_path + "/" + - "velodyne128_novatel_extrinsics.yaml", + FLAGS_lidar_sensor_name + "_novatel_extrinsics.yaml", &ex_lidar2imu); - AINFO << "velodyne128_novatel_extrinsics: " << ex_lidar2imu; + AINFO << FLAGS_lidar_sensor_name + "_novatel_extrinsics.yaml: " + << ex_lidar2imu; ACHECK(visualize_.Init_all_info_single_camera( camera_names_, visual_camera_, intrinsic_map_, extrinsic_map_, @@ -511,7 +513,8 @@ int LaneDetectionComponent::InitCameraFrames() { for (const auto &camera_name : camera_names_) { float height = 0.0f; SetCameraHeight(camera_name, FLAGS_obs_sensor_intrinsic_path, - default_camera_height_, &height); + FLAGS_lidar_sensor_name, default_camera_height_, + &height); camera_height_map_[camera_name] = height; } diff --git a/modules/perception/onboard/proto/fusion_camera_detection_component.proto b/modules/perception/onboard/proto/fusion_camera_detection_component.proto index a6f4da6f08..59577f389f 100644 --- a/modules/perception/onboard/proto/fusion_camera_detection_component.proto +++ b/modules/perception/onboard/proto/fusion_camera_detection_component.proto @@ -45,5 +45,4 @@ message FusionCameraDetection { optional bool image_based_cipv = 30 [default = false]; optional int32 debug_level = 31 [default = 0]; optional bool enable_cipv = 32 [default = false]; - optional string lidar_type = 33 [default = "velodyne128"]; -} +} \ No newline at end of file -- GitLab