Apollo 10.0
自动驾驶开放平台
apollo::perception::camera::LaneCameraPerception类 参考final

#include <lane_camera_perception.h>

类 apollo::perception::camera::LaneCameraPerception 继承关系图:
apollo::perception::camera::LaneCameraPerception 的协作图:

Public 类型

template<typename T , class EigenType >
using EigenMap = apollo::common::EigenMap< T, EigenType >
 

Public 成员函数

 LaneCameraPerception ()=default
 
 ~LaneCameraPerception ()=default
 
bool Init (const CameraPerceptionInitOptions &options) override
 
void InitLane (base::BaseCameraModelPtr &model, const app::PerceptionParam &perception_param)
 
void InitCalibrationService (const base::BaseCameraModelPtr model, const app::PerceptionParam &perception_param)
 
void SetCameraHeightAndPitch (const std::map< std::string, float > name_camera_ground_height_map, const std::map< std::string, float > name_camera_pitch_angle_diff_map, const float &pitch_angle_calibrator_working_sensor)
 
void SetIm2CarHomography (const Eigen::Matrix3d &homography_im2car)
 
bool GetCalibrationService (BaseCalibrationService **calibration_service)
 
bool Perception (const CameraPerceptionOptions &options, CameraFrame *frame) override
 
std::string Name () const override
 
- Public 成员函数 继承自 apollo::perception::camera::BaseCameraPerception
 BaseCameraPerception ()=default
 
virtual ~BaseCameraPerception ()=default
 
 DISALLOW_COPY_AND_ASSIGN (BaseCameraPerception)
 

详细描述

在文件 lane_camera_perception.h40 行定义.

成员类型定义说明

◆ EigenMap

template<typename T , class EigenType >
using apollo::perception::camera::LaneCameraPerception::EigenMap = apollo::common::EigenMap<T, EigenType>

在文件 lane_camera_perception.h43 行定义.

构造及析构函数说明

◆ LaneCameraPerception()

apollo::perception::camera::LaneCameraPerception::LaneCameraPerception ( )
default

◆ ~LaneCameraPerception()

apollo::perception::camera::LaneCameraPerception::~LaneCameraPerception ( )
default

成员函数说明

◆ GetCalibrationService()

bool apollo::perception::camera::LaneCameraPerception::GetCalibrationService ( BaseCalibrationService **  calibration_service)

在文件 lane_camera_perception.cc187 行定义.

188 {
189 *calibration_service = calibration_service_.get();
190 return true;
191}

◆ Init()

bool apollo::perception::camera::LaneCameraPerception::Init ( const CameraPerceptionInitOptions options)
overridevirtual

实现了 apollo::perception::camera::BaseCameraPerception.

在文件 lane_camera_perception.cc46 行定义.

46 {
47 std::string config_file =
48 GetConfigFile(options.config_path, options.config_file);
49 ACHECK(cyber::common::GetProtoFromFile(config_file, &perception_param_))
50 << "Read config failed: " << config_file;
52
53 lane_calibration_working_sensor_name_ =
54 options.lane_calibration_working_sensor_name;
55
56 // Init lane
58 InitLane(model, perception_param_);
59
60 // Init calibration service
61 InitCalibrationService(model, perception_param_);
62
63 return true;
64}
void InitCalibrationService(const base::BaseCameraModelPtr model, const app::PerceptionParam &perception_param)
void InitLane(base::BaseCameraModelPtr &model, const app::PerceptionParam &perception_param)
static bool set_device_id(int device_id)
#define ACHECK(cond)
Definition log.h:80
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...
Definition file.cc:132
std::shared_ptr< BaseCameraModel > BaseCameraModelPtr
Definition camera.h:75
std::string GetConfigFile(const std::string &config_path, const std::string &config_file)
Definition util.cc:80

◆ InitCalibrationService()

void apollo::perception::camera::LaneCameraPerception::InitCalibrationService ( const base::BaseCameraModelPtr  model,
const app::PerceptionParam perception_param 
)

在文件 lane_camera_perception.cc137 行定义.

139 {
140 // Init calibration service
141 ACHECK(perception_param.has_calibration_service_param())
142 << "Failed to include calibration_service_param";
143
144 auto calibration_service_param = perception_param.calibration_service_param();
145 CalibrationServiceInitOptions calibration_service_init_options;
146 calibration_service_init_options.calibrator_working_sensor_name =
147 lane_calibration_working_sensor_name_;
148 calibration_service_init_options.name_intrinsic_map = name_intrinsic_map_;
149 calibration_service_init_options.calibrator_method =
150 calibration_service_param.calibrator_method();
151 calibration_service_init_options.image_height =
152 static_cast<int>(model->get_height());
153 calibration_service_init_options.image_width =
154 static_cast<int>(model->get_width());
155
156 calibration_service_.reset(
157 BaseCalibrationServiceRegisterer::GetInstanceByName(
158 calibration_service_param.plugin_param().name()));
159 CHECK_NOTNULL(calibration_service_);
160 ACHECK(calibration_service_->Init(calibration_service_init_options))
161 << "Failed to init " << calibration_service_param.plugin_param().name();
162 AINFO << "Calibration service: " << calibration_service_->Name();
163}
#define AINFO
Definition log.h:42

◆ InitLane()

void apollo::perception::camera::LaneCameraPerception::InitLane ( base::BaseCameraModelPtr model,
const app::PerceptionParam perception_param 
)

在文件 lane_camera_perception.cc66 行定义.

68 {
69 // Init lane
70 CHECK_GT(perception_param.lane_param_size(), 0)
71 << "Failed to include lane_param";
72 for (int i = 0; i < perception_param.lane_param_size(); ++i) {
73 // Initialize lane detector
74 const auto &lane_param = perception_param.lane_param(i);
75 ACHECK(lane_param.has_lane_detector_param())
76 << "Failed to include lane_detector_param.";
77 LaneDetectorInitOptions lane_detector_init_options;
78 const auto &lane_detector_param = lane_param.lane_detector_param();
79 const auto &lane_detector_plugin_param = lane_detector_param.plugin_param();
80 lane_detector_init_options.config_file =
81 lane_detector_plugin_param.config_file();
82 lane_detector_init_options.config_path =
83 lane_detector_plugin_param.config_path();
84 model = algorithm::SensorManager::Instance()->GetUndistortCameraModel(
85 lane_detector_param.camera_name());
86 ACHECK(model) << "Can't find " << lane_detector_param.camera_name()
87 << " in data/conf/sensor_meta.pb.txt";
88 auto pinhole = static_cast<base::PinholeCameraModel *>(model.get());
89 name_intrinsic_map_.insert(std::pair<std::string, Eigen::Matrix3f>(
90 lane_detector_param.camera_name(), pinhole->get_intrinsic_params()));
91 lane_detector_init_options.gpu_id = perception_param.gpu_id();
92 lane_detector_init_options.base_camera_model = model;
93 AINFO << "lane_detector_name: " << lane_detector_plugin_param.name();
94
95 // Initialize lane detector
96 lane_detector_.reset(BaseLaneDetectorRegisterer::GetInstanceByName(
97 lane_detector_plugin_param.name()));
98 CHECK_NOTNULL(lane_detector_);
99 ACHECK(lane_detector_->Init(lane_detector_init_options))
100 << "Failed to init: " << lane_detector_plugin_param.name();
101 AINFO << "Detector: " << lane_detector_->Name();
102
103 // Initialize lane postprocessor
104 const auto &lane_postprocessor_param =
105 lane_param.lane_postprocessor_param();
106 LanePostprocessorInitOptions postprocessor_init_options;
107 postprocessor_init_options.config_path =
108 lane_postprocessor_param.config_path();
109 postprocessor_init_options.config_file =
110 lane_postprocessor_param.config_file();
111
112 lane_postprocessor_.reset(
113 BaseLanePostprocessorRegisterer::GetInstanceByName(
114 lane_postprocessor_param.name()));
115 CHECK_NOTNULL(lane_postprocessor_);
116 ACHECK(lane_postprocessor_->Init(postprocessor_init_options))
117 << "Failed to init: " << lane_postprocessor_param.name();
118 AINFO << "lane_postprocessor: " << lane_postprocessor_->Name();
119
120 // Init output file folder
121 if (perception_param.has_debug_param() &&
122 perception_param.debug_param().has_lane_out_dir()) {
123 write_out_lane_file_ = true;
124 out_lane_dir_ = perception_param.debug_param().lane_out_dir();
125 EnsureDirectory(out_lane_dir_);
126 }
127
128 if (perception_param.has_debug_param() &&
129 perception_param.debug_param().has_calibration_out_dir()) {
130 write_out_calib_file_ = true;
131 out_calib_dir_ = perception_param.debug_param().calibration_out_dir();
132 EnsureDirectory(out_calib_dir_);
133 }
134 }
135}
bool EnsureDirectory(const std::string &directory_path)
Check if a specified directory specified by directory_path exists.
Definition file.cc:299

◆ Name()

std::string apollo::perception::camera::LaneCameraPerception::Name ( ) const
inlineoverridevirtual

实现了 apollo::perception::camera::BaseCameraPerception.

在文件 lane_camera_perception.h63 行定义.

63{ return "LaneCameraPerception"; }

◆ Perception()

bool apollo::perception::camera::LaneCameraPerception::Perception ( const CameraPerceptionOptions options,
CameraFrame frame 
)
overridevirtual

实现了 apollo::perception::camera::BaseCameraPerception.

在文件 lane_camera_perception.cc193 行定义.

194 {
196 inference::CudaUtil::set_device_id(perception_param_.gpu_id());
198
199 if (frame->calibration_service == nullptr) {
200 AERROR << "Calibraion service is not available";
201 return false;
202 }
203
204 // Lane detector and postprocessor: work on front_6mm only
205 if (lane_calibration_working_sensor_name_ ==
206 frame->data_provider->sensor_name()) {
207 frame->camera_k_matrix =
208 name_intrinsic_map_.at(frame->data_provider->sensor_name());
209 LaneDetectorOptions lane_detetor_options;
210 LanePostprocessorOptions lane_postprocessor_options;
211 if (!lane_detector_->Detect(lane_detetor_options, frame)) {
212 AERROR << "Failed to detect lane.";
213 return false;
214 }
215 PERF_BLOCK_END_WITH_INDICATOR(frame->data_provider->sensor_name(),
216 "LaneDetector");
217
218 if (!lane_postprocessor_->Process2D(lane_postprocessor_options, frame)) {
219 AERROR << "Failed to postprocess lane 2D.";
220 return false;
221 }
222 PERF_BLOCK_END_WITH_INDICATOR(frame->data_provider->sensor_name(),
223 "LanePostprocessor2D");
224
225 // Calibration service
226 frame->calibration_service->Update(frame);
227 PERF_BLOCK_END_WITH_INDICATOR(frame->data_provider->sensor_name(),
228 "CalibrationService");
229
230 if (!lane_postprocessor_->Process3D(lane_postprocessor_options, frame)) {
231 AERROR << "Failed to postprocess lane 3D.";
232 return false;
233 }
234 PERF_BLOCK_END_WITH_INDICATOR(frame->data_provider->sensor_name(),
235 "LanePostprocessor3D");
236
237 if (write_out_lane_file_) {
238 std::string lane_file_path =
239 absl::StrCat(out_lane_dir_, "/", frame->frame_id, ".txt");
240 WriteLanelines(write_out_lane_file_, lane_file_path, frame->lane_objects);
241 }
242 } else {
243 AINFO << "Skip lane detection & calibration due to sensor mismatch.";
244 AINFO << "Will use service sync from obstacle camera instead.";
245 // fill the frame using previous estimates
246 frame->calibration_service->Update(frame);
247 PERF_BLOCK_END_WITH_INDICATOR(frame->data_provider->sensor_name(),
248 "CalibrationService");
249 }
250
251 if (write_out_calib_file_) {
252 std::string calib_file_path =
253 absl::StrCat(out_calib_dir_, "/", frame->frame_id, ".txt");
254 WriteCalibrationOutput(write_out_calib_file_, calib_file_path, frame);
255 }
256 return true;
257}
#define AERROR
Definition log.h:44
int WriteCalibrationOutput(bool enabled, const std::string &out_path, const CameraFrame *frame)
int WriteLanelines(const bool enabled, const std::string &save_path, const std::vector< base::LaneLine > &lane_objects)
Definition debug_info.cc:28
#define PERF_BLOCK_END_WITH_INDICATOR(indicator, msg)
Definition perf_util.h:128
#define PERF_BLOCK_START()
Definition perf_util.h:126
#define PERF_FUNCTION(...)
Definition profiler.h:54

◆ SetCameraHeightAndPitch()

void apollo::perception::camera::LaneCameraPerception::SetCameraHeightAndPitch ( const std::map< std::string, float >  name_camera_ground_height_map,
const std::map< std::string, float >  name_camera_pitch_angle_diff_map,
const float &  pitch_angle_calibrator_working_sensor 
)

在文件 lane_camera_perception.cc165 行定义.

168 {
169 if (calibration_service_ == nullptr) {
170 AERROR << "Calibraion service is not available";
171 return;
172 }
173 calibration_service_->SetCameraHeightAndPitch(
174 name_camera_ground_height_map, name_camera_pitch_angle_diff_map,
175 pitch_angle_calibrator_working_sensor);
176}

◆ SetIm2CarHomography()

void apollo::perception::camera::LaneCameraPerception::SetIm2CarHomography ( const Eigen::Matrix3d &  homography_im2car)

在文件 lane_camera_perception.cc178 行定义.

179 {
180 if (calibration_service_ == nullptr) {
181 AERROR << "Calibraion service is not available";
182 return;
183 }
184 lane_postprocessor_->SetIm2CarHomography(homography_im2car);
185}

该类的文档由以下文件生成: