From b79331ad05fece53a1cbf715a9943639da390353 Mon Sep 17 00:00:00 2001 From: BISTU-gzy <269895055+BISTU-gzy@users.noreply.github.com> Date: Sun, 12 Apr 2026 21:10:04 +0800 Subject: [PATCH] Perception: initialize static camera location refinement calibration --- .../camera_location_refinement/BUILD | 13 +++ .../camera_location_refinement_component.cc | 33 ++++++- .../camera_location_refinement_component.h | 10 ++ ...mera_location_refinement_component_test.cc | 96 +++++++++++++++++++ .../camera_location_refinement_config.pb.txt | 2 + ...ra_location_refinement_front_config.pb.txt | 2 + ...era_location_refinement_rear_config.pb.txt | 2 + .../proto/camera_location_refinement.proto | 4 +- 8 files changed, 158 insertions(+), 4 deletions(-) create mode 100644 modules/perception/camera_location_refinement/camera_location_refinement_component_test.cc diff --git a/modules/perception/camera_location_refinement/BUILD b/modules/perception/camera_location_refinement/BUILD index 51fa135a07..62a4851f33 100644 --- a/modules/perception/camera_location_refinement/BUILD +++ b/modules/perception/camera_location_refinement/BUILD @@ -61,6 +61,19 @@ apollo_component( "//modules/perception/common/camera:apollo_perception_common_camera", "//modules/perception/common/lib:apollo_perception_common_lib", "//modules/perception/common/onboard:apollo_perception_common_onboard", + "@com_google_googletest//:gtest", + ], +) + +apollo_cc_test( + name = "camera_location_refinement_component_test", + size = "small", + srcs = [ + "camera_location_refinement_component_test.cc", + ], + deps = [ + ":libcamera_location_refinement_component_camera.so", + "@com_google_googletest//:gtest_main", ], ) diff --git a/modules/perception/camera_location_refinement/camera_location_refinement_component.cc b/modules/perception/camera_location_refinement/camera_location_refinement_component.cc index b555f85dda..57ebf2bd0b 100644 --- a/modules/perception/camera_location_refinement/camera_location_refinement_component.cc +++ b/modules/perception/camera_location_refinement/camera_location_refinement_component.cc @@ -89,6 +89,33 @@ void CameraLocationRefinementComponent::SetCameraHeightAndPitch( pitch_angle_calibrator_working_sensor); } +bool CameraLocationRefinementComponent::InitStaticCalibration( + const CameraLocationRefinement& location_refinement_param) { + if (calibration_service_ == nullptr) { + AERROR << "Calibration service is not available"; + return false; + } + const float camera_height = location_refinement_param.default_camera_height(); + if (camera_height <= 0.0f) { + AERROR << "default_camera_height must be positive, got: " + << camera_height; + return false; + } + std::map name_camera_ground_height_map = { + {location_refinement_param.camera_name(), camera_height}}; + std::map name_camera_pitch_angle_diff_map = { + {location_refinement_param.camera_name(), 0.0f}}; + SetCameraHeightAndPitch(name_camera_ground_height_map, + name_camera_pitch_angle_diff_map, + location_refinement_param.default_camera_pitch()); + if (!calibration_service_->BuildIndex()) { + AERROR << "Failed to build calibration service index for " + << location_refinement_param.camera_name(); + return false; + } + return true; +} + bool CameraLocationRefinementComponent::Init() { CameraLocationRefinement location_refinement_param; if (!GetProtoConfig(&location_refinement_param)) { @@ -97,9 +124,9 @@ bool CameraLocationRefinementComponent::Init() { } InitPostprocessor(location_refinement_param); - - // todo(daohu527): need complete - // SetCameraHeightAndPitch(); + if (!InitStaticCalibration(location_refinement_param)) { + return false; + } writer_ = node_->CreateWriter( location_refinement_param.channel().output_obstacles_channel_name()); diff --git a/modules/perception/camera_location_refinement/camera_location_refinement_component.h b/modules/perception/camera_location_refinement/camera_location_refinement_component.h index af33b84c1a..ece68d69fc 100644 --- a/modules/perception/camera_location_refinement/camera_location_refinement_component.h +++ b/modules/perception/camera_location_refinement/camera_location_refinement_component.h @@ -19,6 +19,8 @@ #include #include +#include "gtest/gtest_prod.h" + #include "modules/perception/camera_location_refinement/proto/camera_location_refinement.pb.h" #include "cyber/cyber.h" @@ -57,12 +59,20 @@ class CameraLocationRefinementComponent final void InitPostprocessor( const CameraLocationRefinement& location_refinement_param); + bool InitStaticCalibration( + const CameraLocationRefinement& location_refinement_param); + void SetCameraHeightAndPitch( const std::map& name_camera_ground_height_map, const std::map& name_camera_pitch_angle_diff_map, const float& pitch_angle_calibrator_working_sensor); private: + FRIEND_TEST(CameraLocationRefinementComponentTest, + init_static_calibration_builds_ground_plane_test); + FRIEND_TEST(CameraLocationRefinementComponentTest, + init_static_calibration_rejects_non_positive_height_test); + std::shared_ptr postprocessor_; std::shared_ptr calibration_service_; diff --git a/modules/perception/camera_location_refinement/camera_location_refinement_component_test.cc b/modules/perception/camera_location_refinement/camera_location_refinement_component_test.cc new file mode 100644 index 0000000000..2255e64502 --- /dev/null +++ b/modules/perception/camera_location_refinement/camera_location_refinement_component_test.cc @@ -0,0 +1,96 @@ +/****************************************************************************** + * Copyright 2026 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the License); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/perception/camera_location_refinement/camera_location_refinement_component.h" + +#include "gtest/gtest.h" + +namespace apollo { +namespace perception { +namespace camera { + +class RecordingCalibrationService : public BaseCalibrationService { + public: + bool Init(const CalibrationServiceInitOptions &options = + CalibrationServiceInitOptions()) override { + return true; + } + + bool BuildIndex() override { + build_index_called = true; + return build_index_result; + } + + void SetCameraHeightAndPitch( + const std::map &name_camera_ground_height_map, + const std::map &name_camera_pitch_angle_diff_map, + const float &pitch_angle_master_sensor) override { + camera_ground_height_map = name_camera_ground_height_map; + camera_pitch_angle_diff_map = name_camera_pitch_angle_diff_map; + master_sensor_pitch = pitch_angle_master_sensor; + } + + std::string Name() const override { return "RecordingCalibrationService"; } + + bool build_index_called = false; + bool build_index_result = true; + float master_sensor_pitch = 0.0f; + std::map camera_ground_height_map; + std::map camera_pitch_angle_diff_map; +}; + +class CameraLocationRefinementComponentTest : public ::testing::Test {}; + +TEST_F(CameraLocationRefinementComponentTest, + init_static_calibration_builds_ground_plane_test) { + CameraLocationRefinementComponent component; + auto calibration_service = std::make_shared(); + component.calibration_service_ = calibration_service; + + CameraLocationRefinement config; + config.set_camera_name("front_6mm"); + config.set_default_camera_pitch(0.12f); + config.set_default_camera_height(1.8f); + + EXPECT_TRUE(component.InitStaticCalibration(config)); + EXPECT_TRUE(calibration_service->build_index_called); + ASSERT_EQ(calibration_service->camera_ground_height_map.size(), 1u); + ASSERT_EQ(calibration_service->camera_pitch_angle_diff_map.size(), 1u); + EXPECT_FLOAT_EQ( + calibration_service->camera_ground_height_map.at("front_6mm"), 1.8f); + EXPECT_FLOAT_EQ( + calibration_service->camera_pitch_angle_diff_map.at("front_6mm"), 0.0f); + EXPECT_FLOAT_EQ(calibration_service->master_sensor_pitch, 0.12f); +} + +TEST_F(CameraLocationRefinementComponentTest, + init_static_calibration_rejects_non_positive_height_test) { + CameraLocationRefinementComponent component; + auto calibration_service = std::make_shared(); + component.calibration_service_ = calibration_service; + + CameraLocationRefinement config; + config.set_camera_name("front_6mm"); + config.set_default_camera_height(0.0f); + + EXPECT_FALSE(component.InitStaticCalibration(config)); + EXPECT_FALSE(calibration_service->build_index_called); + EXPECT_TRUE(calibration_service->camera_ground_height_map.empty()); +} + +} // namespace camera +} // namespace perception +} // namespace apollo diff --git a/modules/perception/camera_location_refinement/conf/camera_location_refinement_config.pb.txt b/modules/perception/camera_location_refinement/conf/camera_location_refinement_config.pb.txt index b053dd14be..3a3ff54bec 100644 --- a/modules/perception/camera_location_refinement/conf/camera_location_refinement_config.pb.txt +++ b/modules/perception/camera_location_refinement/conf/camera_location_refinement_config.pb.txt @@ -1,4 +1,6 @@ camera_name: "front_6mm" +default_camera_pitch: 0.0 +default_camera_height: 1.5 postprocessor_param { plugin_param { name: "LocationRefinerPostprocessor" diff --git a/modules/perception/camera_location_refinement/conf/camera_location_refinement_front_config.pb.txt b/modules/perception/camera_location_refinement/conf/camera_location_refinement_front_config.pb.txt index b053dd14be..3a3ff54bec 100644 --- a/modules/perception/camera_location_refinement/conf/camera_location_refinement_front_config.pb.txt +++ b/modules/perception/camera_location_refinement/conf/camera_location_refinement_front_config.pb.txt @@ -1,4 +1,6 @@ camera_name: "front_6mm" +default_camera_pitch: 0.0 +default_camera_height: 1.5 postprocessor_param { plugin_param { name: "LocationRefinerPostprocessor" diff --git a/modules/perception/camera_location_refinement/conf/camera_location_refinement_rear_config.pb.txt b/modules/perception/camera_location_refinement/conf/camera_location_refinement_rear_config.pb.txt index 034edb2d66..e15cf6bccf 100644 --- a/modules/perception/camera_location_refinement/conf/camera_location_refinement_rear_config.pb.txt +++ b/modules/perception/camera_location_refinement/conf/camera_location_refinement_rear_config.pb.txt @@ -1,4 +1,6 @@ camera_name: "front_12mm" +default_camera_pitch: 0.0 +default_camera_height: 1.5 postprocessor_param { plugin_param { name: "LocationRefinerPostprocessor" diff --git a/modules/perception/camera_location_refinement/proto/camera_location_refinement.proto b/modules/perception/camera_location_refinement/proto/camera_location_refinement.proto index af8e947f2b..22f5a8ac7f 100644 --- a/modules/perception/camera_location_refinement/proto/camera_location_refinement.proto +++ b/modules/perception/camera_location_refinement/proto/camera_location_refinement.proto @@ -8,6 +8,8 @@ message CameraLocationRefinement { optional PostprocessorParam postprocessor_param = 2; optional CalibrationServiceParam calibration_service_param = 3; optional RefinementChannel channel = 4; + optional float default_camera_pitch = 5 [default = 0.0]; + optional float default_camera_height = 6 [default = 1.5]; } message RefinementChannel { @@ -22,4 +24,4 @@ message PostprocessorParam { message CalibrationServiceParam { optional string calibrator_method = 2; optional perception.PluginParam plugin_param = 3; -} \ No newline at end of file +}