Apollo 10.0
自动驾驶开放平台
camera_detection_occupancy_component.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2024 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the License);
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an AS IS BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16#include "modules/perception/camera_detection_occupancy/camera_detection_occupancy_component.h"
17
18#include <string>
19
20#include "cyber/common/file.h"
21#include "cyber/common/log.h"
22#include "cyber/profiler/profiler.h"
23#include "cyber/time/clock.h"
24#include "modules/perception/common/algorithm/sensor_manager/sensor_manager.h"
25#include "modules/perception/common/base/camera.h"
26#include "modules/perception/common/perception_gflags.h"
27
28namespace apollo {
29namespace perception {
30namespace camera {
31
32bool CameraDetectionOccComponent::InitDetector(
33 const CameraDetectionBEV &detection_param) {
34 // Init conf file
35 auto detector_plugin_param = detection_param.detector_plugin_param();
36 init_options.config_path = detector_plugin_param.config_path();
37 init_options.config_file = detector_plugin_param.config_file();
38 init_options.gpu_id = detection_param.gpu_id();
39
40 // Init camera params
41 std::string camera_name = detection_param.camera_name();
42 base::BaseCameraModelPtr model =
43 algorithm::SensorManager::Instance()->GetUndistortCameraModel(
44 camera_name);
45 ACHECK(model) << "Can't find " << camera_name
46 << " in data/conf/sensor_meta.pb.txt";
47 auto pinhole = static_cast<base::PinholeCameraModel *>(model.get());
48 init_options.intrinsic = pinhole->get_intrinsic_params();
49 init_options.image_height = model->get_height();
50 init_options.image_width = model->get_width();
51 image_height_ = model->get_height();
52 image_width_ = model->get_width();
53
54 // Init detector
55 detector_.reset(BaseObstacleDetectorRegisterer::GetInstanceByName(
56 detector_plugin_param.name()));
57 detector_->Init(init_options);
58 return true;
59}
60
61bool CameraDetectionOccComponent::InitCameraFrame(
62 const CameraDetectionBEV &detection_param) {
63 DataProvider::InitOptions init_options;
64 init_options.image_height = image_height_;
65 init_options.image_width = image_width_;
66 init_options.do_undistortion = detection_param.enable_undistortion();
67 init_options.device_id = detection_param.gpu_id();
68 AINFO << "init_options.device_id: " << init_options.device_id
69 << " camera_name: " << init_options.sensor_name;
70
71 std::vector<std::string> bev_sensor_name{
72 detection_param.sensors().bev_camera_name().begin(),
73 detection_param.sensors().bev_camera_name().end()};
74
75 frame_ptr_ = std::make_shared<CameraFrame>();
76 frame_ptr_->data_provider.resize(bev_sensor_name.size());
77 frame_ptr_->k_lidar2img.clear();
78
79 for (int i = 0; i < bev_sensor_name.size(); ++i) {
80 init_options.sensor_name = bev_sensor_name[i];
81 // get extrinsic
82 // std::string camera_extrinsic_file_path =
83 // algorithm::SensorManager::Instance()->ExtrinsicPath(
84 // init_options.sensor_name);
85 std::string camera_extrinsic_file_path = FLAGS_obs_sensor_intrinsic_path +
86 "/" + init_options.sensor_name +
87 "_extrinsics.yaml";
88 Eigen::Affine3d camera2lidar_rt = Eigen::Affine3d::Identity();
89
90 if (detection_param.dataset_type() == "nuscenes") {
91 std::string lidar_extrinsic_file_path =
92 FLAGS_obs_sensor_intrinsic_path + "/LIDAR_TOP_novatel_extrinsics.yaml";
93 LoadCameraExtrinsicNus(
94 camera_extrinsic_file_path,
95 lidar_extrinsic_file_path,
96 &camera2lidar_rt);
97 } else {
98 LoadCameraExtrinsic(camera_extrinsic_file_path, &camera2lidar_rt);
99 }
100 Eigen::Matrix4d lidar2camera_rt =
101 camera2lidar_rt.inverse().matrix();
102
103 // get intrinsic
104 algorithm::SensorManager *sensor_manager =
105 algorithm::SensorManager::Instance();
106 if (!sensor_manager->IsSensorExist(init_options.sensor_name)) {
107 AERROR << "Sensor '" << init_options.sensor_name << "' not exists!";
108 return false;
109 }
110
111 Eigen::Matrix3f camera_intrinsic =
112 std::dynamic_pointer_cast<base::BrownCameraDistortionModel>(
113 sensor_manager->GetDistortCameraModel(init_options.sensor_name))
114 ->get_intrinsic_params();
115 Eigen::Matrix4f viewpad = Eigen::Matrix4f::Identity();
116 viewpad(0, 0) = camera_intrinsic(0, 0);
117 viewpad(0, 2) = camera_intrinsic(0, 2);
118 viewpad(1, 1) = camera_intrinsic(1, 1);
119 viewpad(1, 2) = camera_intrinsic(1, 2);
120
121 // lidar2img_rt = (viewpad @ lidar2cam_rt.T)
122 Eigen::Matrix4d lidar2img_rt =
123 viewpad.cast<double>() * lidar2camera_rt;
124
125 for (size_t i = 0; i < lidar2img_rt.rows(); i++) {
126 for (size_t j = 0; j < lidar2img_rt.cols(); j++) {
127 frame_ptr_->k_lidar2img.push_back(lidar2img_rt(i, j));
128 }
129 }
130
131 frame_ptr_->data_provider[i] = std::make_shared<DataProvider>();
132 frame_ptr_->data_provider[i]->Init(init_options);
133 }
134 return true;
135}
136
137bool CameraDetectionOccComponent::LoadCameraExtrinsic(
138 const std::string &file_path, Eigen::Affine3d *camera2lidar_rt) {
139 CHECK_NOTNULL(camera2lidar_rt);
140
141 YAML::Node config = YAML::LoadFile(file_path);
142 if (config["transform"]) {
143 if (config["transform"]["translation"]) {
144 camera2lidar_rt->translation()(0) =
145 config["transform"]["translation"]["x"].as<double>();
146 camera2lidar_rt->translation()(1) =
147 config["transform"]["translation"]["y"].as<double>();
148 camera2lidar_rt->translation()(2) =
149 config["transform"]["translation"]["z"].as<double>();
150 if (config["transform"]["rotation"]) {
151 double qx = config["transform"]["rotation"]["x"].as<double>();
152 double qy = config["transform"]["rotation"]["y"].as<double>();
153 double qz = config["transform"]["rotation"]["z"].as<double>();
154 double qw = config["transform"]["rotation"]["w"].as<double>();
155 camera2lidar_rt->linear() =
156 Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix();
157 return true;
158 }
159 }
160 }
161 return false;
162}
163
164bool CameraDetectionOccComponent::LoadCameraExtrinsicNus(
165 const std::string &camera_extrinsic_file_path,
166 const std::string &lidar_extrinsic_file_path,
167 Eigen::Affine3d *camera2lidar_rt) {
168 CHECK_NOTNULL(camera2lidar_rt);
169 Eigen::Affine3d local2lidar_rt = Eigen::Affine3d::Identity();
170 YAML::Node config = YAML::LoadFile(lidar_extrinsic_file_path);
171 if (config["transform"]) {
172 if (config["transform"]["translation"]) {
173 local2lidar_rt.translation()(0) =
174 config["transform"]["translation"]["x"].as<double>();
175 local2lidar_rt.translation()(1) =
176 config["transform"]["translation"]["y"].as<double>();
177 local2lidar_rt.translation()(2) =
178 config["transform"]["translation"]["z"].as<double>();
179 if (config["transform"]["rotation"]) {
180 double qx = config["transform"]["rotation"]["x"].as<double>();
181 double qy = config["transform"]["rotation"]["y"].as<double>();
182 double qz = config["transform"]["rotation"]["z"].as<double>();
183 double qw = config["transform"]["rotation"]["w"].as<double>();
184 local2lidar_rt.linear() =
185 Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix();
186 }
187 }
188 }
189 // AERROR << config["transform"]["translation"]["x"].as<double>() << " "
190 // << config["transform"]["translation"]["y"].as<double>() << " "
191 // << config["transform"]["translation"]["z"].as<double>() << " "
192 // << config["transform"]["rotation"]["x"].as<double>() << " "
193 // << config["transform"]["rotation"]["y"].as<double>() << " "
194 // << config["transform"]["rotation"]["z"].as<double>() << " "
195 // << config["transform"]["rotation"]["w"].as<double>();
196
197 Eigen::Affine3d local2cam_rt = Eigen::Affine3d::Identity();
198 config = YAML::LoadFile(camera_extrinsic_file_path);
199 if (config["transform"]) {
200 if (config["transform"]["translation"]) {
201 local2cam_rt.translation()(0) =
202 config["transform"]["translation"]["x"].as<double>();
203 local2cam_rt.translation()(1) =
204 config["transform"]["translation"]["y"].as<double>();
205 local2cam_rt.translation()(2) =
206 config["transform"]["translation"]["z"].as<double>();
207 if (config["transform"]["rotation"]) {
208 double qx = config["transform"]["rotation"]["x"].as<double>();
209 double qy = config["transform"]["rotation"]["y"].as<double>();
210 double qz = config["transform"]["rotation"]["z"].as<double>();
211 double qw = config["transform"]["rotation"]["w"].as<double>();
212 local2cam_rt.linear() =
213 Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix();
214 }
215 }
216 }
217
218 // AERROR << config["transform"]["translation"]["x"].as<double>() << " "
219 // << config["transform"]["translation"]["y"].as<double>() << " "
220 // << config["transform"]["translation"]["z"].as<double>() << " "
221 // << config["transform"]["rotation"]["x"].as<double>() << " "
222 // << config["transform"]["rotation"]["y"].as<double>() << " "
223 // << config["transform"]["rotation"]["z"].as<double>() << " "
224 // << config["transform"]["rotation"]["w"].as<double>();
225
226 *camera2lidar_rt = (local2cam_rt.inverse() * local2lidar_rt).inverse();
227 return false;
228}
229
230bool CameraDetectionOccComponent::InitListeners(
231 const CameraDetectionBEV &detection_param) {
232 for (const auto &channel :
233 detection_param.channel().input_camera_channel_name()) {
234 std::shared_ptr<cyber::Reader<drivers::Image>> reader;
235 if (channel ==
236 detection_param.channel().input_camera_channel_name()
237 [detection_param.channel().input_camera_channel_name().size() -
238 1]) {
239 reader = node_->CreateReader<drivers::Image>(
240 channel, [&](const std::shared_ptr<drivers::Image> &msg) {
241 OnReceiveImage(msg);
242 });
243 } else {
244 reader = node_->CreateReader<drivers::Image>(channel);
245 }
246 readers_.emplace_back(reader);
247 }
248 return true;
249}
250
251bool CameraDetectionOccComponent::InitTransformWrapper(
252 const CameraDetectionBEV &detection_param) {
253 trans_wrapper_.reset(new onboard::TransformWrapper());
254 // tf_camera_frame_id
255 trans_wrapper_->Init(detection_param.frame_id());
256 return true;
257}
258
259bool CameraDetectionOccComponent::Init() {
260 CameraDetectionBEV detection_param;
261 if (!GetProtoConfig(&detection_param)) {
262 AERROR << "Load camera detection 3d component config failed!";
263 return false;
264 }
265
266 // Init tracker
267 auto tracker_plugin_param = detection_param.tracker_plugin_param();
268 BaseTracker *tracker =
269 BaseTrackerRegisterer::GetInstanceByName(tracker_plugin_param.name());
270 CHECK_NOTNULL(tracker);
271 tracker_.reset(tracker);
272
273 TrackerInitOptions tracker_init_options;
274 tracker_init_options.config_path = tracker_plugin_param.config_path();
275 tracker_init_options.config_file = tracker_plugin_param.config_file();
276 ACHECK(tracker_->Init(tracker_init_options)) << "camera tracker init error";
277
278 // segmentor init
279 // auto segmentor_plugin_param = detection_param.segmentor_plugin_param();
280 // BaseSegmentor *segmentor =
281 // BaseSegmentorRegisterer::GetInstanceByName(segmentor_plugin_param.name());
282 // CHECK_NOTNULL(segmentor);
283 // segmentor_.reset(segmentor);
284
285 // SegmentorInitOptions segmentr_init_options;
286 // segmentr_init_options.config_path = segmentor_plugin_param.config_path();
287 // segmentr_init_options.config_file = segmentor_plugin_param.config_file();
288 // ACHECK(segmentor_->Init(segmentr_init_options))
289 // << "lidar segmentor init error";
290
291 // todo(zero): need init image_height_\image_width_
292 InitDetector(detection_param);
293 InitCameraFrame(detection_param);
294 InitListeners(detection_param);
295 InitTransformWrapper(detection_param);
296
297 writer_ = node_->CreateWriter<PerceptionObstacles>(
298 detection_param.channel().output_obstacles_channel_name());
299 return true;
300}
301
302void CameraDetectionOccComponent::CameraToWorldCoor(
303 const Eigen::Affine3d &camera2world, std::vector<base::ObjectPtr> *objs) {
304 for (auto &obj : *objs) {
305 Eigen::Vector3d local_center =
306 obj->camera_supplement.local_center.cast<double>();
307 obj->center = camera2world * local_center;
308
309 obj->direction = camera2world.linear().cast<float>() * obj->direction;
310 obj->theta = std::atan2(obj->direction(1), obj->direction(0));
311
312 for (size_t i = 0; i < obj->polygon.size(); ++i) {
313 auto &pt = obj->polygon.at(i);
314 Eigen::Vector3d local_polygon_point =
315 Eigen::Vector3f(pt.x, pt.y, pt.z).cast<double>();
316 Eigen::Vector3d world_polygon_point = camera2world * local_polygon_point;
317 pt.x = world_polygon_point[0];
318 pt.y = world_polygon_point[1];
319 pt.z = world_polygon_point[2];
320 }
321 }
322}
323
324int CameraDetectionOccComponent::ConvertObjectToPb(
325 const base::ObjectPtr &object_ptr, PerceptionObstacle *pb_msg) {
326 if (!object_ptr || !pb_msg) {
327 return cyber::FAIL;
328 }
329 pb_msg->set_id(object_ptr->track_id);
330 pb_msg->set_theta(object_ptr->theta);
331 pb_msg->set_confidence(object_ptr->confidence);
332
333 apollo::common::Point3D *obj_center = pb_msg->mutable_position();
334 obj_center->set_x(object_ptr->center(0));
335 obj_center->set_y(object_ptr->center(1));
336 obj_center->set_z(object_ptr->center(2));
337
338 apollo::common::Point3D *obj_velocity = pb_msg->mutable_velocity();
339 obj_velocity->set_x(object_ptr->velocity(0));
340 obj_velocity->set_y(object_ptr->velocity(1));
341 obj_velocity->set_z(object_ptr->velocity(2));
342
343 apollo::common::Point3D *obj_acceleration = pb_msg->mutable_acceleration();
344 obj_acceleration->set_x(object_ptr->acceleration(0));
345 obj_acceleration->set_y(object_ptr->acceleration(1));
346 obj_acceleration->set_z(object_ptr->acceleration(2));
347
348 pb_msg->set_length(object_ptr->size(0));
349 pb_msg->set_width(object_ptr->size(1));
350 pb_msg->set_height(object_ptr->size(2));
351
352 // for camera results, set object's center as anchor point
353 pb_msg->set_type(static_cast<PerceptionObstacle::Type>(object_ptr->type));
354 pb_msg->set_sub_type(
355 static_cast<PerceptionObstacle::SubType>(object_ptr->sub_type));
356 pb_msg->set_timestamp(object_ptr->latest_tracked_time); // in seconds.
357
358 pb_msg->set_height_above_ground(object_ptr->size(2));
359
360 for (size_t i = 0; i < object_ptr->polygon.size(); ++i) {
361 auto &pt = object_ptr->polygon.at(i);
362 apollo::common::Point3D *p = pb_msg->add_polygon_point();
363 p->set_x(pt.x);
364 p->set_y(pt.y);
365 p->set_z(pt.z);
366 }
367 return cyber::SUCC;
368}
369
370int CameraDetectionOccComponent::MakeProtobufMsg(
371 double msg_timestamp, int seq_num,
372 const std::vector<base::ObjectPtr> &objects,
373 PerceptionObstacles *obstacles) {
374 double publish_time = apollo::cyber::Clock::NowInSeconds();
375 auto header = obstacles->mutable_header();
376 header->set_timestamp_sec(publish_time);
377 header->set_module_name("perception_camera");
378 header->set_sequence_num(seq_num);
379 // in nanosecond
380 // PnC would use lidar timestamp to predict
381 header->set_lidar_timestamp(static_cast<uint64_t>(msg_timestamp * 1e9));
382 header->set_camera_timestamp(static_cast<uint64_t>(msg_timestamp * 1e9));
383
384 // write out obstacles in world coordinates
385 int count = 0;
386 for (const auto &obj : objects) {
387 obj->track_id = count++;
388 PerceptionObstacle *obstacle = obstacles->add_perception_obstacle();
389 if (ConvertObjectToPb(obj, obstacle) != cyber::SUCC) {
390 AERROR << "ConvertObjectToPb failed, Object:" << obj->ToString();
391 return cyber::FAIL;
392 }
393 }
394
395 return cyber::SUCC;
396}
397
398bool CameraDetectionOccComponent::OnReceiveImage(
399 const std::shared_ptr<drivers::Image> &msg) {
400 const double msg_timestamp = msg->measurement_time() + timestamp_offset_;
401
402 for (size_t i = 0; i < readers_.size(); ++i) {
403 readers_[i]->Observe();
404 const auto &camera_msg = readers_[i]->GetLatestObserved();
405 if (camera_msg == nullptr) {
406 AERROR << "Failed to get camera message, camera_name: "
407 << msg->frame_id();
408 return false;
409 }
410 frame_ptr_->data_provider[i]->FillImageData(
411 image_height_, image_width_,
412 reinterpret_cast<const uint8_t *>(camera_msg->data().data()),
413 camera_msg->encoding());
414 }
415
416 frame_ptr_->timestamp = msg_timestamp;
417 ++frame_ptr_->frame_id;
418
419 // Detect
420 PERF_BLOCK("foreground objects detection")
421 detector_->Detect(frame_ptr_.get());
422 PERF_BLOCK_END
423
424 // // segmention
425 // PERF_BLOCK("occ cluster")
426 // if (!segmentor_->Detect(frame_ptr_.get())) {
427 // AERROR << "occ cluster error.";
428 // return false;
429 // }
430 // PERF_BLOCK_END
431
432 // // object builder
433 // PERF_BLOCK("occ object builder")
434 // ObjectBuilderOptions builder_options;
435 // if (!builder_.Build(builder_options, frame_ptr_.get())) {
436 // AERROR << "Lidar segmentation, object builder error.";
437 // return false;
438 // }
439 // PERF_BLOCK_END
440
441 // Get sensor to world pose from TF
442 Eigen::Affine3d lidar2world_trans;
443 if (!trans_wrapper_->GetSensor2worldTrans(msg_timestamp,
444 &lidar2world_trans)) {
445 const std::string err_str =
446 absl::StrCat("failed to get camera to world pose, ts: ", msg_timestamp,
447 " camera_name: ", msg->frame_id());
448 AERROR << err_str;
449 return false;
450 }
451
452 CameraToWorldCoor(lidar2world_trans, &frame_ptr_->detected_objects);
453
454 // Tracker
455 // Tracker: converter
456 // std::shared_ptr<base::Frame> base_frame_ptr_(new base::Frame);
457 // if (!ConvertToBaseFrame(frame_ptr_.get(), base_frame_ptr_.get())) {
458 // AERROR << "Failed to convert frame";
459 // return false;
460 // }
461 // // Tracker: object track
462 // base_frame_ptr_->sensor2world_pose = lidar2world_trans;
463 // base::FramePtr tracker_frame_ptr(new base::Frame);
464 // if (!tracker_->Track(*base_frame_ptr_, tracker_frame_ptr)) {
465 // AERROR << "track error";
466 // return false;
467 // }
468
469 std::shared_ptr<PerceptionObstacles> out_message(new (std::nothrow)
470 PerceptionObstacles);
471 if (MakeProtobufMsg(msg_timestamp, seq_num_, frame_ptr_->detected_objects,
472 out_message.get()) != cyber::SUCC) {
473 AERROR << "MakeProtobufMsg failed ts: " << msg_timestamp;
474 return false;
475 }
476 seq_num_++;
477 // Send msg
478 writer_->Write(out_message);
479
480 return true;
481}
482
483bool CameraDetectionOccComponent::ConvertToBaseFrame(const CameraFrame *frame,
484 base::Frame *base_frame) {
485 base_frame->timestamp = frame->timestamp;
486 for (size_t i = 0; i < frame->detected_objects.size(); ++i) {
487 base::ObjectPtr object = frame->detected_objects[i];
488 base_frame->objects.emplace_back(object);
489 }
490 return true;
491}
492
493} // namespace camera
494} // namespace perception
495} // namespace apollo
camera_detection_occupancy_component.h
apollo::cyber::Clock::NowInSeconds
static double NowInSeconds()
gets the current time in second.
Definition clock.cc:56
apollo::cyber::ComponentBase::GetProtoConfig
bool GetProtoConfig(T *config) const
Definition component_base.h:63
apollo::cyber::ComponentBase::node_
std::shared_ptr< Node > node_
Definition component_base.h:119
ACHECK
#define ACHECK(cond)
Definition log.h:80
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
apollo::perception::base::BaseCameraModelPtr
std::shared_ptr< BaseCameraModel > BaseCameraModelPtr
Definition camera.h:75
apollo::perception::base::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition object.h:127
apollo
class register implement
Definition arena_queue.h:37
perception_gflags.h
PERF_BLOCK_END
#define PERF_BLOCK_END
Definition profiler.h:53
PERF_BLOCK
#define PERF_BLOCK(...)
Definition profiler.h:52
sensor_manager.h