Apollo 11.0
自动驾驶开放平台
cpdet.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
17#include "modules/perception/lidar_cpdet_detection/detector/cpdet/cpdet.h"
18
19#include <algorithm>
20#include <functional>
21#include <numeric>
22#include <random>
23#include <utility>
24
25#include <unistd.h>
26
27#include "cyber/common/file.h"
28#include "cyber/common/log.h"
29#include "modules/perception/common/base/object_pool_types.h"
30#include "modules/perception/common/base/point_cloud_util.h"
31#include "modules/perception/common/base/blob.h"
32#include "modules/perception/common/inference/model_util.h"
33#include "modules/perception/common/lidar/common/cloud_mask.h"
34#include "modules/perception/common/lidar/common/lidar_timer.h"
35#include "modules/perception/common/lidar/common/pcl_util.h"
36#include "modules/perception/common/lidar/common/config_util.h"
37#include "modules/perception/common/util.h"
38
39#include "cyber/profiler/profiler.h"
40
41namespace apollo {
42namespace perception {
43namespace lidar {
44
45const float MIN_X_RANGE = -51.2;
46const float MAX_X_RANGE = 51.2;
47const float MIN_Y_RANGE = -51.2;
48const float MAX_Y_RANGE = 51.2;
49const float MIN_Z_RANGE = -3.5;
50const float MAX_Z_RANGE = 3.5;
51
52CPDetection::CPDetection()
53 : x_min_range_(MIN_X_RANGE), x_max_range_(MAX_X_RANGE),
54 y_min_range_(MIN_Y_RANGE), y_max_range_(MAX_Y_RANGE) {}
55
56bool CPDetection::Init(const CPDetectorInitOptions &options) {
57 std::string conf_file =
58 GetConfigFile(options.config_path, options.config_file);
59 ACHECK(apollo::cyber::common::GetProtoFromFile(conf_file, &cpdet_config_));
60
61 LoadParams(cpdet_config_);
62
63 if (!InitModel(cpdet_config_.model_param())) {
64 AERROR << "Init CPDetection Model Failed";
65 return false;
66 }
67
68 // create cuda stream
69 cudaStreamCreate(&stream_);
70 pfn_inference_->SetStream(stream_);
71 backbone_inference_->SetStream(stream_);
72
73 // set private feature blob
74 point2grid_blob_.reset(new base::Blob<int>(std::vector<int>{max_voxel_num_}));
75 grid2pointnum_blob_.reset(new base::Blob<int>(std::vector<int>{map_size_}));
76
77 if (use_gpu_generate_feature_) {
78 voxels_blob_->mutable_gpu_data();
79 canvas_feature_blob_->mutable_gpu_data();
80 point2grid_blob_->mutable_gpu_data();
81 grid2pointnum_blob_->mutable_gpu_data();
82 } else {
83 voxels_blob_->mutable_cpu_data();
84 canvas_feature_blob_->mutable_cpu_data();
85 point2grid_blob_->mutable_cpu_data();
86 grid2pointnum_blob_->mutable_cpu_data();
87 }
88
89 int channel_index = pillar_feature_dim_;
90 feature_offset_["canvas_feature"] = canvas_feature_blob_->offset(0, 0);
91 if (use_cnnseg_features_) {
92 feature_offset_["max_height"] = canvas_feature_blob_->offset(0, channel_index++);
93 feature_offset_["mean_height"] = canvas_feature_blob_->offset(0, channel_index++);
94 feature_offset_["top_intensity"] = canvas_feature_blob_->offset(0, channel_index++);
95 feature_offset_["mean_intensity"] = canvas_feature_blob_->offset(0, channel_index++);
96 feature_offset_["count"] = canvas_feature_blob_->offset(0, channel_index++);
97 feature_offset_["nonempty"] = canvas_feature_blob_->offset(0, channel_index++);
98 feature_offset_["height_bin"] = canvas_feature_blob_->offset(0, channel_index);
99 }
100
101 max_candidate_num_ = num_tasks_ * nms_pre_max_size_;
102 all_res_box_blob_.reset(new base::Blob<float>(std::vector<int>{max_candidate_num_, kBoxBlockSize}));
103 all_res_conf_blob_.reset(new base::Blob<float>(std::vector<int>{max_candidate_num_}));
104 all_res_cls_blob_.reset(new base::Blob<int>(std::vector<int>{max_candidate_num_}));
105
106 res_box_num_blob_.reset(new base::Blob<int>(std::vector<int>{1}));
107 res_box_blob_.reset(new base::Blob<float>(std::vector<int>{nms_pre_max_size_, kBoxBlockSize}));
108 res_conf_blob_.reset(new base::Blob<float>(std::vector<int>{nms_pre_max_size_}));
109 res_cls_blob_.reset(new base::Blob<int>(std::vector<int>{nms_pre_max_size_}));
110
111 score_class_map_blob_.reset(new base::Blob<float>(std::vector<int>{num_classes_}));
112
113 res_sorted_indices_blob_.reset(new base::Blob<int>(std::vector<int>{nms_pre_max_size_}));
114 box_for_nms_blob_.reset(new base::Blob<float>(std::vector<int>{max_candidate_num_, 4}));
115 box_corner_blob_.reset(new base::Blob<float>(std::vector<int>{max_candidate_num_, 8}));
116 remain_conf_blob_.reset(new base::Blob<float>(std::vector<int>{nms_pre_max_size_}));
117 rotate_overlapped_blob_.reset(new base::Blob<bool>(std::vector<int>{nms_pre_max_size_, nms_pre_max_size_}));
118 kept_indices_blob_.reset(new base::Blob<int>(std::vector<int>{num_tasks_ * nms_post_max_size_}));
119
120 box_corners_blob_.reset(new base::Blob<float>(std::vector<int>{max_candidate_num_, 8}));
121 box_rects_blob_.reset(new base::Blob<float>(std::vector<int>{max_candidate_num_, 4}));
122 valid_point_num_blob_.reset(new base::Blob<int>(std::vector<int>{1}));
123 valid_point_indices_blob_.reset(new base::Blob<int>(std::vector<int>{max_valid_point_size_}));
124 valid_point2boxid_blob_.reset(new base::Blob<int>(std::vector<int>{max_valid_point_size_}));
125
126 res_box_num_blob_->mutable_gpu_data();
127 res_box_blob_->mutable_gpu_data();
128 res_conf_blob_->mutable_gpu_data();
129 res_cls_blob_->mutable_gpu_data();
130 all_res_box_blob_->mutable_gpu_data();
131 all_res_conf_blob_->mutable_gpu_data();
132 all_res_cls_blob_->mutable_gpu_data();
133 score_class_map_blob_->mutable_gpu_data();
134 remain_conf_blob_->mutable_gpu_data();
135 rotate_overlapped_blob_->mutable_gpu_data();
136 kept_indices_blob_->mutable_gpu_data();
137 box_corners_blob_->mutable_gpu_data();
138 box_rects_blob_->mutable_gpu_data();
139 valid_point_num_blob_->mutable_gpu_data();
140 valid_point_indices_blob_->mutable_gpu_data();
141 valid_point2boxid_blob_->mutable_gpu_data();
142
143 res_box_num_blob_->mutable_cpu_data();
144 all_res_box_blob_->mutable_cpu_data();
145 all_res_conf_blob_->mutable_cpu_data();
146 all_res_cls_blob_->mutable_cpu_data();
147 rotate_overlapped_blob_->mutable_cpu_data();
148 box_corners_blob_->mutable_cpu_data();
149 box_rects_blob_->mutable_cpu_data();
150 valid_point_num_blob_->mutable_cpu_data();
151 valid_point_indices_blob_->mutable_cpu_data();
152 valid_point2boxid_blob_->mutable_cpu_data();
153
154 if (cpdet_config_.has_plugins()) {
155 const auto &plugin = cpdet_config_.plugins();
156 const auto &name = plugin.name();
157 down_sample_ = apollo::cyber::plugin_manager::PluginManager::Instance()
158 ->CreateInstance<BaseDownSample>(ConfigUtil::GetFullClassName(name));
159 if (!down_sample_) {
160 AINFO << "Failed to find down_sample plugin: " << name << ", skipped";
161 return false;
162 }
163 DownSampleInitOptions option;
164 option.config_path = plugin.config_path();
165 option.config_file = plugin.config_file();
166 if (!down_sample_->Init(option)) {
167 AINFO << "Failed to init down_sample plugin: " << name << ", skipped";
168 return false;
169 }
170 }
171
172 return true;
173}
174
175void CPDetection::LoadParams(const cpdet::CPDetConfig &cpdet_config) {
176 x_min_range_ = cpdet_config.pre_process().min_x_range();
177 x_max_range_ = cpdet_config.pre_process().max_x_range();
178 y_min_range_ = cpdet_config.pre_process().min_y_range();
179 y_max_range_ = cpdet_config.pre_process().max_y_range();
180 z_min_range_ = cpdet_config.pre_process().min_z_range();
181 z_max_range_ = cpdet_config.pre_process().max_z_range();
182 voxel_x_size_ = cpdet_config.pre_process().voxel_x_size();
183 voxel_y_size_ = cpdet_config.pre_process().voxel_y_size();
184 voxel_z_size_ = cpdet_config.pre_process().voxel_z_size();
185 x_offset_ = (voxel_x_size_) / 2 + x_min_range_;
186 y_offset_ = (voxel_y_size_) / 2 + y_min_range_;
187 z_offset_ = (voxel_z_size_) / 2 + z_min_range_;
188 float x_range = x_max_range_ - x_min_range_;
189 float y_range = y_max_range_ - y_min_range_;
190 float z_range = z_max_range_ - z_min_range_;
191 grid_x_size_ = static_cast<int>(x_range / voxel_x_size_);
192 grid_y_size_ = static_cast<int>(y_range / voxel_y_size_);
193 grid_z_size_ = static_cast<int>(z_range / voxel_z_size_);
194 map_size_ = grid_x_size_ * grid_y_size_;
195 point_dim_ = cpdet_config.pre_process().point_dim();
196 enable_rotate_45degree_ = cpdet_config.pre_process().enable_rotate_45degree();
197 use_input_norm_ = cpdet_config.pre_process().use_input_norm();
198
199 max_point_number_ = 160000;
200
201 use_cnnseg_features_ = cpdet_config.pre_process().use_cnnseg_features();
202 cnnseg_feature_dim_ = 0;
203 if (use_cnnseg_features_) {
204 cnnseg_feature_dim_ = cpdet_config.pre_process().cnnseg_feature_dim();
205 height_bin_min_height_ = cpdet_config.pre_process().height_bin_min_height();
206 height_bin_max_height_ = cpdet_config.pre_process().height_bin_max_height();
207 height_bin_voxel_size_ = cpdet_config.pre_process().height_bin_voxel_size();
208 height_bin_dim_ = static_cast<int>((height_bin_max_height_
209 - height_bin_min_height_) / height_bin_voxel_size_);
210 }
211
212 use_gpu_generate_feature_ = cpdet_config.pre_process().use_gpu_generate_feature();
213 max_voxel_num_ = cpdet_config.pre_process().max_voxel_num();
214 max_points_in_voxel_ = cpdet_config.pre_process().max_points_in_voxel();
215 voxel_feature_dim_ = cpdet_config.pre_process().voxel_feature_dim();
216 pillar_feature_dim_ = cpdet_config.pre_process().pillar_feature_dim();
217 num_classes_ = cpdet_config.pre_process().num_classes();
218
219 downsample_size_ = cpdet_config.post_process().downsample_size();
220 num_tasks_ = cpdet_config.post_process().task_size();
221 head_x_size_ = static_cast<int>(grid_x_size_ / downsample_size_);
222 head_y_size_ = static_cast<int>(grid_y_size_ / downsample_size_);
223 head_map_size_ = head_x_size_ * head_y_size_;
224
225 nms_pre_max_size_ = cpdet_config.post_process().nms_pre_max_size();
226 nms_post_max_size_ = cpdet_config.post_process().nms_post_max_size();
227 score_thresh_ = cpdet_config.post_process().score_thresh();
228 nms_overlap_thresh_ = cpdet_config.post_process().nms_overlap_thresh();
229
230 for (size_t i = 0; i < cpdet_config.post_process().det_head_size(); ++i) {
231 head_map_[cpdet_config.post_process().det_head(i).head_name()] =
232 cpdet_config.post_process().det_head(i).head_shape();
233 }
234 for (size_t i = 0; i < num_tasks_; i++) {
235 num_classes_in_task_.push_back(cpdet_config.post_process().task(i).sub_task_size());
236 for (size_t j = 0; j < cpdet_config.post_process().task(i).sub_task_size(); j++) {
237 auto type_thres_pair = std::make_pair(cpdet_config.post_process().task(i).sub_task(j).class_name(),
238 cpdet_config.post_process().task(i).sub_task(j).score_thresh());
239 score_thresh_per_class_[cpdet_config.post_process().task(i).sub_task(j).class_index()] = type_thres_pair;
240 }
241 }
242 for (size_t i = 0; i < num_classes_; i++) {
243 score_thresh_map_.push_back(score_thresh_per_class_.at(i).second);
244 }
245 max_valid_point_size_ = std::max(static_cast<int>(cpdet_config.pre_process().max_voxel_num() * 2),
246 max_valid_point_size_);
247
248 top_enlarge_value_ = cpdet_config.post_process().top_enlarge_height();
249 bottom_enlarge_value_ = cpdet_config.post_process().bottom_enlarge_height();
250 width_enlarge_value_ = cpdet_config.post_process().width_enlarge_value();
251 length_enlarge_value_ = cpdet_config.post_process().length_enlarge_value();
252 use_cpu_get_objects_ = cpdet_config.post_process().use_cpu_get_objects();
253 use_cpu_assign_points_ = cpdet_config.post_process().use_cpu_assign_points();
254
255 remove_semantic_ground_ = cpdet_config.filter_param().remove_semantic_ground();
256 remove_raw_ground_ = cpdet_config.filter_param().remove_raw_ground();
257 point_unique_ = cpdet_config.filter_param().point_unique();
258 filter_by_semantic_type_ = cpdet_config.filter_param().filter_by_semantic_type();
259 inter_class_nms_ = cpdet_config.filter_param().inter_class_nms();
260 class_nms_iou_thres_ = cpdet_config.filter_param().class_nms_iou_thres();
261 nms_strategy_ = cpdet_config.filter_param().nms_strategy();
262 min_pts_num_fg_ = cpdet_config.filter_param().min_points_threshold();
263
264 for (int i = 0; i < cpdet_config.filter_param().fore_semantic_filter_size(); i++) {
265 int semantic_type = cpdet_config.filter_param().fore_semantic_filter(i).semantic_type();
266 auto pair = std::make_pair(cpdet_config.filter_param().fore_semantic_filter(i).roi_out_ratio(),
267 cpdet_config.filter_param().fore_semantic_filter(i).filter_range());
268 fore_semantic_filter_map_[semantic_type] = pair;
269 }
270
271 enable_downsample_ = cpdet_config.pre_process().enable_downsample_pointcloud();
272}
273
274bool CPDetection::InitModel(const cpdet::CPDetModelParam &model_param) {
275 // fpn name:blob maps
276 std::map<std::string, std::vector<int>> pfn_blob_maps;
277 // pfn model
278 auto pfn_model_info = model_param.pfn_model_info();
279 auto preprocess = model_param.preprocess();
280 std::string model_path = GetModelPath(pfn_model_info.name());
281
282 std::string pfn_proto_file = GetModelFile(model_path,
283 pfn_model_info.proto_file().file());
284 std::vector<std::string> pfn_input_names =
285 inference::GetBlobNames(pfn_model_info.inputs());
286 std::vector<std::string> pfn_output_names =
287 inference::GetBlobNames(pfn_model_info.outputs());
288 pfn_inference_ = apollo::cyber::plugin_manager::PluginManager::Instance()->CreateInstance<inference::Inference>(
289 "apollo::perception::inference::" + pfn_model_info.infer_plugin());
290 pfn_inference_->set_model_info(pfn_proto_file,
291 pfn_input_names, pfn_output_names);
292 pfn_inference_->set_gpu_id(preprocess.gpu_id());
293 pfn_inference_->set_max_batch_size(max_point_number_ * 2);
294
295 inference::AddShape(&pfn_blob_maps, pfn_model_info.inputs());
296 inference::AddShape(&pfn_blob_maps, pfn_model_info.outputs());
297
298 pfn_inference_->Init(pfn_blob_maps);
299 // init pfn input blob
300 if (pfn_input_names.size() == 1) {
301 voxels_blob_ = pfn_inference_->get_blob(pfn_input_names.at(0));
302 CHECK_NOTNULL(voxels_blob_.get());
303 } else {
304 AERROR << "Init cpdet_pfn_inference input blob error.";
305 return false;
306 }
307 // init pfn output blobs
308 if (pfn_output_names.size() == 1) {
309 pfn_pillar_feature_blob_ = pfn_inference_->get_blob(pfn_output_names.at(0));
310 CHECK_NOTNULL(pfn_pillar_feature_blob_.get());
311 } else {
312 AERROR << "Init cpdet_pfn_inference output blob error.";
313 return false;
314 }
315 AINFO << "Init cpdet pfn model success.";
316
317 // backbone name : blob maps
318 std::map<std::string, std::vector<int>> backbone_blob_maps;
319 // backbone info
320 auto backbone_model_info = model_param.backbone_model_info();
321 // backbone model file
322 std::string backbone_proto_file = GetModelFile(
323 model_path, backbone_model_info.proto_file().file());
324 std::vector<std::string> backbone_input_names =
325 inference::GetBlobNames(backbone_model_info.inputs());
326 std::vector<std::string> backbone_output_names =
327 inference::GetBlobNames(backbone_model_info.outputs());
328 backbone_inference_ = apollo::cyber::plugin_manager::PluginManager::Instance()->CreateInstance<inference::Inference>(
329 "apollo::perception::inference::" + backbone_model_info.infer_plugin());
330 backbone_inference_->set_model_info(backbone_proto_file,
331 backbone_input_names, backbone_output_names);
332 backbone_inference_->set_gpu_id(preprocess.gpu_id());
333 backbone_inference_->set_max_batch_size(max_point_number_ * 2);
334
335 inference::AddShape(&backbone_blob_maps, backbone_model_info.inputs());
336 inference::AddShape(&backbone_blob_maps, backbone_model_info.outputs());
337
338 backbone_inference_->Init(backbone_blob_maps);
339 // init backbone input blobs
340 if (backbone_input_names.size() == 1) {
341 canvas_feature_blob_ = backbone_inference_->get_blob(backbone_input_names.at(0));
342 CHECK_NOTNULL(canvas_feature_blob_.get());
343 } else {
344 AERROR << "Init cpdet_backbone_inference input blob error.";
345 return false;
346 }
347 if (backbone_output_names.size() == 3) {
348 output_box_blob_ = backbone_inference_->get_blob(backbone_output_names.at(0));
349 output_cls_blob_ = backbone_inference_->get_blob(backbone_output_names.at(1));
350 output_dir_blob_ = backbone_inference_->get_blob(backbone_output_names.at(2));
351 CHECK_NOTNULL(output_box_blob_.get());
352 CHECK_NOTNULL(output_cls_blob_.get());
353 CHECK_NOTNULL(output_dir_blob_.get());
354 } else {
355 AERROR << "Init cpdet_backbone_inference output blobs error.";
356 return false;
357 }
358 AINFO << "Init cpdet backbone model Success.";
359 return true;
360}
361
362void CPDetection::PointCloudPreprocess() {
363 if (!enable_downsample_) {
364 return;
365 }
366 DownSampleOptions down_sample_options;
367 ACHECK(down_sample_ != nullptr);
368 down_sample_->Process(down_sample_options, cur_cloud_ptr_);
369}
370
371// for mmdetection-apollo model
372base::ObjectSubType CPDetection::GetObjectSubType(const int label) {
373 switch (label) {
374 case 0:
375 return base::ObjectSubType::SMALLMOT;
376 case 1:
377 return base::ObjectSubType::PEDESTRIAN;
378 case 2:
379 return base::ObjectSubType::NONMOT;
380 case 3:
381 return base::ObjectSubType::TRAFFICCONE;
382 default:
383 return base::ObjectSubType::UNKNOWN;
384 }
385}
386
387void CPDetection::GenerateObjects(LidarFrame* frame) {
388 // background object
389 std::vector<base::ObjectPtr> fore_objects;
390 size_t size = res_outputs_.size();
391 for (size_t i = 0; i < size; ++i) {
392 auto cluster = res_outputs_[i];
393 if (cluster.point_ids.size() <= min_pts_num_fg_) {
394 continue;
395 }
396 base::Object object;
397
398 object.lidar_supplement.on_use = true;
399 object.lidar_supplement.is_in_roi = true;
400 object.lidar_supplement.is_background = false;
401 object.lidar_supplement.is_clustered = false;
402
403 for (auto idx : cluster.point_ids) {
404 object.lidar_supplement.point_ids.push_back(static_cast<int>(idx));
405 }
406 object.lidar_supplement.num_points_in_roi = cluster.point_ids.size();
407 object.lidar_supplement.cloud.CopyPointCloud(*original_cloud_, cluster.point_ids);
408 object.lidar_supplement.cloud_world.CopyPointCloud(*original_world_cloud_, cluster.point_ids);
409
410 // model original output
411 object.lidar_supplement.detections.resize(7);
412 object.lidar_supplement.detections[0] = cluster.x;
413 object.lidar_supplement.detections[1] = cluster.y;
414 object.lidar_supplement.detections[2] = cluster.z;
415 object.lidar_supplement.detections[3] = cluster.l;
416 object.lidar_supplement.detections[4] = cluster.w;
417 object.lidar_supplement.detections[5] = cluster.h;
418 object.lidar_supplement.detections[6] = cluster.yaw;
419
420 // center and size
421 object.center(0) = cluster.x;
422 object.center(1) = cluster.y;
423 object.center(2) = cluster.z;
424 object.size(0) = cluster.l;
425 object.size(1) = cluster.w;
426 object.size(2) = cluster.h;
427
428 // direction
429 object.theta = cluster.yaw;
430 object.direction[0] = cosf(cluster.yaw);
431 object.direction[1] = sinf(cluster.yaw);
432 object.direction[2] = 0;
433 object.lidar_supplement.is_orientation_ready = true;
434
435 // confidence
436 object.confidence = cluster.confidence;
437 object.id = i;
438
439 // classification
440 object.lidar_supplement.raw_probs.push_back(std::vector<float>(
441 static_cast<int>(base::ObjectType::MAX_OBJECT_TYPE), 0.f));
442 object.lidar_supplement.raw_classification_methods.push_back(Name());
443 object.sub_type = GetObjectSubType(cluster.model_type_index);
444 object.type = base::kSubType2TypeMap.at(object.sub_type);
445 // AERROR << "[Type:] " << static_cast<int>(object.type);
446 if (object.sub_type == base::ObjectSubType::TRAFFICCONE) {
447 object.type = base::ObjectType::UNKNOWN;
448 }
449 object.lidar_supplement.raw_probs.back()[static_cast<int>(object.type)] = 1.0f;
450 // copy to type
451 object.type_probs.assign(object.lidar_supplement.raw_probs.back().begin(),
452 object.lidar_supplement.raw_probs.back().end());
453 // copy to background objects
454 std::shared_ptr<base::Object> obj(new base::Object);
455 *obj = object;
456 fore_objects.push_back(std::move(obj));
457 }
458 // object builder for fore objects
459 frame->segmented_objects.insert(frame->segmented_objects.end(), fore_objects.begin(), fore_objects.end());
460}
461
462void CPDetection::SetPointsInROI(std::vector<base::ObjectPtr> *objects) {
463 CloudMask roi_mask_;
464 roi_mask_.Set(original_cloud_->size(), 0);
465 roi_mask_.AddIndices(lidar_frame_ref_->roi_indices, 1);
466 for (uint32_t i = 0; i < objects->size(); ++i) {
467 auto &object = objects->at(i);
468 object->lidar_supplement.num_points_in_roi =
469 roi_mask_.ValidIndicesCount(object->lidar_supplement.point_ids);
470 }
471}
472
473void CPDetection::RemoveForePoints(std::vector<base::ObjectPtr> *objects) {
474 CloudMask mask;
475 mask.Set(original_cloud_->size(), 0);
476 mask.AddIndicesOfIndices(lidar_frame_ref_->roi_indices,
477 lidar_frame_ref_->non_ground_indices, 1);
478 for (uint32_t i = 0; i < objects->size(); ++i) {
479 auto &object = objects->at(i);
480 base::PointIndices ind;
481 ind.indices = object->lidar_supplement.point_ids;
482 mask.RemoveIndices(ind);
483 }
484 mask.GetValidIndices(&lidar_frame_ref_->secondary_indices);
485}
486
487void CPDetection::MapPointsToGridCpu(int* point2grid_data) {
488 auto Pc2Pixel = [&](float pc_axis, float voxel_size, float start_range) {
489 float fpixel = (pc_axis - start_range) / voxel_size;
490 return fpixel < 0 ? -1 : static_cast<int>(fpixel);
491 };
492
493 int pos_x = -1;
494 int pos_y = -1;
495 for (int i = 0; i < model_cloud_size_; ++i) {
496 auto pt = original_cloud_->at(i);
497 float px = pt.x;
498 float py = pt.y;
499 float pz = pt.z;
500 if (enable_rotate_45degree_) {
501 px = 0.707107 * pt.x - 0.707107 * pt.y;
502 py = 0.707107 * pt.x + 0.707107 * pt.y;
503 }
504 pos_x = Pc2Pixel(px, voxel_x_size_, x_min_range_);
505 pos_y = Pc2Pixel(py, voxel_y_size_, y_min_range_);
506 if (pos_y < 0 || pos_y >= grid_y_size_ || pos_x < 0 || pos_x >= grid_x_size_) {
507 point2grid_data[i] = -1;
508 continue;
509 }
510 point2grid_data[i] = pos_y * grid_x_size_ + pos_x;
511 }
512}
513
514void CPDetection::GeneratePfnFeatureCPU() {
515 // DO NOT remove this line!!!
516 // Otherwise, the gpu_data will not be updated for the later frames.
517 // It marks the head at cpu for blob.
518 voxels_blob_->mutable_cpu_data();
519
520 float* voxels_cluster_data = canvas_feature_blob_->mutable_cpu_data();
521 int* grid2pointnum_data = grid2pointnum_blob_->mutable_cpu_data();
522 memset(voxels_cluster_data, 0, map_size_ * 3 * sizeof(float));
523 memset(grid2pointnum_data, 0, map_size_ * sizeof(int));
524
525 point2grid_blob_->Reshape(std::vector<int>{model_cloud_size_});
526 voxels_blob_->Reshape(std::vector<int>{model_cloud_size_, 1, voxel_feature_dim_, 1});
527 int* point2grid_data = point2grid_blob_->mutable_cpu_data();
528 float* voxels_data = voxels_blob_->mutable_cpu_data();
529 memset(point2grid_data, 0, model_cloud_size_ * sizeof(int));
530 memset(voxels_data, 0.f, (model_cloud_size_) * voxel_feature_dim_ * sizeof(float));
531
532 MapPointsToGridCpu(point2grid_data);
533
534 if (use_cnnseg_features_) {
535 float* out_data = canvas_feature_blob_->mutable_cpu_data();
536 max_height_data_ = out_data + feature_offset_["max_height"];
537 mean_height_data_ = out_data + feature_offset_["mean_height"];
538 top_intensity_data_ = out_data + feature_offset_["top_intensity"];
539 mean_intensity_data_ = out_data + feature_offset_["mean_intensity"];
540 count_data_ = out_data + feature_offset_["count"];
541 nonempty_data_ = out_data + feature_offset_["nonempty"];
542 height_bin_data_ = out_data + feature_offset_["height_bin"];
543
544 memset(mean_height_data_, 0, map_size_ * sizeof(float));
545 memset(max_height_data_, -5.0, map_size_ * sizeof(float));
546 memset(top_intensity_data_, 0, map_size_ * sizeof(float));
547 memset(mean_intensity_data_, 0, map_size_ * sizeof(float));
548 memset(count_data_, 0, map_size_ * sizeof(float));
549 memset(nonempty_data_, 0, map_size_ * sizeof(float));
550 memset(height_bin_data_, 0, map_size_ * height_bin_dim_ * sizeof(float));
551 }
552
553 // compute features
554 for (int i = 0; i < model_cloud_size_; ++i) {
555 int idx = point2grid_data[i];
556 if (idx == -1) {
557 continue;
558 }
559 auto pt = original_cloud_->at(i);
560 float px = pt.x;
561 float py = pt.y;
562 float pz = pt.z;
563 if (enable_rotate_45degree_) {
564 px = 0.707107 * pt.x - 0.707107 * pt.y;
565 py = 0.707107 * pt.x + 0.707107 * pt.y;
566 }
567 // calculate voxel cluster
568 voxels_cluster_data[map_size_ * 0 + idx] += px;
569 voxels_cluster_data[map_size_ * 1 + idx] += py;
570 voxels_cluster_data[map_size_ * 2 + idx] += pz;
571 grid2pointnum_data[idx] += 1;
572 }
573
574 for (int i = 0; i < model_cloud_size_; ++i) {
575 int grid_idx = point2grid_data[i];
576 if (grid_idx == -1) {
577 continue;
578 }
579 auto pt = original_cloud_->at(i);
580 float px = pt.x;
581 float py = pt.y;
582 float pz = pt.z;
583 if (enable_rotate_45degree_) {
584 px = 0.707107 * pt.x - 0.707107 * pt.y;
585 py = 0.707107 * pt.x + 0.707107 * pt.y;
586 }
587 int coord_y = grid_idx / grid_x_size_;
588 int coord_x = grid_idx % grid_x_size_;
589 int voxel_data_idx = i * voxel_feature_dim_;
590 float point_num = std::max(1.f, static_cast<float>(grid2pointnum_data[grid_idx]));
591
592 voxels_data[voxel_data_idx + 4] = px - voxels_cluster_data[map_size_ * 0 + grid_idx] / point_num;
593 voxels_data[voxel_data_idx + 5] = py - voxels_cluster_data[map_size_ * 1 + grid_idx] / point_num;
594 voxels_data[voxel_data_idx + 6] = pz - voxels_cluster_data[map_size_ * 2 + grid_idx] / point_num;
595 voxels_data[voxel_data_idx + 7] = px - (static_cast<float>(coord_x) * voxel_x_size_ + x_offset_);
596 voxels_data[voxel_data_idx + 8] = py - (static_cast<float>(coord_y) * voxel_y_size_ + y_offset_);
597 if (use_input_norm_) {
598 voxels_data[voxel_data_idx + 0] = px / x_max_range_;
599 voxels_data[voxel_data_idx + 1] = py / y_max_range_;
600 voxels_data[voxel_data_idx + 2] = pz / z_max_range_;
601 voxels_data[voxel_data_idx + 3] = pt.intensity / 255.0;
602 } else {
603 voxels_data[voxel_data_idx + 0] = pt.x;
604 voxels_data[voxel_data_idx + 1] = pt.y;
605 voxels_data[voxel_data_idx + 2] = pt.z;
606 voxels_data[voxel_data_idx + 3] = pt.intensity;
607 }
608 if (use_cnnseg_features_) {
609 if (max_height_data_[grid_idx] < pz) {
610 max_height_data_[grid_idx] = pz;
611 top_intensity_data_[grid_idx] = pt.intensity / 255.0;
612 }
613 mean_height_data_[grid_idx] += static_cast<float>(pz);
614 mean_intensity_data_[grid_idx] += static_cast<float>(
615 pt.intensity / 255.0);
616 count_data_[grid_idx] += 1.f;
617 int height_bin_index = static_cast<int>(
618 (pz - height_bin_min_height_) / height_bin_voxel_size_);
619 height_bin_index = height_bin_index < 0 ? 0 : height_bin_index;
620 height_bin_index = height_bin_index >= height_bin_dim_ ?
621 (height_bin_dim_ - 1) : height_bin_index;
622 height_bin_data_[height_bin_index * map_size_ + grid_idx] = 1.f;
623 }
624 }
625
626 if (use_cnnseg_features_) {
627 for (int i = 0; i < map_size_; ++i) {
628 if (count_data_[i] <= std::numeric_limits<float>::epsilon()) {
629 max_height_data_[i] = 0.f;
630 } else {
631 mean_height_data_[i] /= count_data_[i];
632 mean_intensity_data_[i] /= count_data_[i];
633 nonempty_data_[i] = 1.f;
634 }
635 count_data_[i] = static_cast<int>(log(static_cast<float>(1 + count_data_[i])));
636 }
637 }
638 // output
639 pfn_pillar_feature_blob_->Reshape({model_cloud_size_, pillar_feature_dim_});
640}
641
642void CPDetection::GenerateBackboneFeatureCPU(const base::Blob<float>* pillar_feature_blob) {
643 // DO NOT remove this line!!!
644 // Otherwise, the gpu_data will not be updated for the later frames.
645 // It marks the head at cpu for blob.
646 canvas_feature_blob_->mutable_cpu_data();
647
648 const float* pillar_feat_data = pillar_feature_blob->cpu_data();
649 const int* point2grid_data = point2grid_blob_->cpu_data();
650 const int* grid2pointnum_data = grid2pointnum_blob_->cpu_data();
651
652 // input
653 int shape = pillar_feature_dim_ + cnnseg_feature_dim_;
654 canvas_feature_blob_->Reshape({1, shape, grid_x_size_, grid_y_size_});
655 float* canvas_feat_data = canvas_feature_blob_->mutable_cpu_data() + feature_offset_["canvas_feature"];
656 memset(canvas_feat_data, 0.f, map_size_ * pillar_feature_dim_ * sizeof(float));
657
658 // max-pooling
659 for (int i = 0; i < model_cloud_size_; ++i) {
660 int grid_idx = point2grid_data[i];
661 if (grid_idx == -1) {
662 continue;
663 }
664 int y = grid_idx / grid_x_size_;
665 int x = grid_idx % grid_x_size_;
666 for (int c_idx = 0; c_idx < pillar_feature_dim_; c_idx++) {
667 int canvas_idx = c_idx * map_size_ + y * grid_x_size_ + x;
668 int pillar_idx = i * pillar_feature_dim_ + c_idx;
669 canvas_feat_data[canvas_idx] = std::max(
670 canvas_feat_data[canvas_idx], pillar_feat_data[pillar_idx]);
671 }
672 }
673}
674
675float CPDetection::CalculateUnionArea(const float *corners1, const float *corners2) {
676 auto trangle_area_cpu = [&](float* a, float* b, float* c) {
677 return ((a[0] - c[0]) * (b[1] - c[1]) - (a[1] - c[1]) * (b[0] - c[0])) / 2.f;
678 };
679
680 auto sort_vertex_in_convex_polygon_cpu = [&](float* int_pts, int num_of_inter) {
681 if (num_of_inter == 0) {
682 return;
683 }
684 float center_x = 0.f;
685 float center_y = 0.f;
686 for (int i = 0; i < num_of_inter; i++) {
687 center_x += int_pts[2 * i];
688 center_y += int_pts[2 * i + 1];
689 }
690 center_x /= num_of_inter;
691 center_y /= num_of_inter;
692 float v0;
693 float v1;
694 float vs[16];
695 for (int i = 0; i < num_of_inter; i++) {
696 v0 = int_pts[2 * i] - center_x;
697 v1 = int_pts[2 * i + 1] - center_y;
698 float d = sqrt(v0 * v0 + v1 * v1);
699 v0 = v0 / d;
700 v1 = v1 / d;
701 if (v1 < 0) {
702 v0 = -2 - v0;
703 }
704 vs[i] = v0;
705 }
706 int j = 0;
707 float temp = 0.f;
708 for (int i = 0; i < num_of_inter; i++) {
709 if (vs[i - 1] > vs[i]) {
710 temp = vs[i];
711 float tx = int_pts[2 * i];
712 float ty = int_pts[2 * i + 1];
713 j = i;
714 while (j > 0 && vs[j - 1] > temp) {
715 vs[j] = vs[j - 1];
716 int_pts[j * 2] = int_pts[j * 2 - 2];
717 int_pts[j * 2 + 1] = int_pts[j * 2 - 1];
718 j -= 1;
719 }
720 vs[j] = temp;
721 int_pts[j * 2] = tx;
722 int_pts[j * 2 + 1] = ty;
723 }
724 }
725 };
726
727 auto line_segment_intersection_cpu = [&](const float* pts1, const float* pts2, int i, int j, float* temp_pts) {
728 float A0 = pts1[2 * i];
729 float A1 = pts1[2 * i + 1];
730
731 float B0 = pts1[2 * ((i + 1) % 4)];
732 float B1 = pts1[2 * ((i + 1) % 4) + 1];
733
734 float C0 = pts2[2 * j];
735 float C1 = pts2[2 * j + 1];
736
737 float D0 = pts2[2 * ((j + 1) % 4)];
738 float D1 = pts2[2 * ((j + 1) % 4) + 1];
739 float BA0 = B0 - A0;
740 float BA1 = B1 - A1;
741 float DA0 = D0 - A0;
742 float CA0 = C0 - A0;
743 float DA1 = D1 - A1;
744 float CA1 = C1 - A1;
745 bool acd = DA1 * CA0 > CA1 * DA0;
746 bool bcd = (D1 - B1) * (C0 - B0) > (C1 - B1) * (D0 - B0);
747 if (acd != bcd) {
748 bool abc = CA1 * BA0 > BA1 * CA0;
749 bool abd = DA1 * BA0 > BA1 * DA0;
750 if (abc != abd) {
751 float DC0 = D0 - C0;
752 float DC1 = D1 - C1;
753 float ABBA = A0 * B1 - B0 * A1;
754 float CDDC = C0 * D1 - D0 * C1;
755 float DH = BA1 * DC0 - BA0 * DC1;
756 float Dx = ABBA * DC0 - BA0 * CDDC;
757 float Dy = ABBA * DC1 - BA1 * CDDC;
758 temp_pts[0] = Dx / DH;
759 temp_pts[1] = Dy / DH;
760 return true;
761 }
762 }
763 return false;
764 };
765
766 auto point_in_quadrilateral_cpu = [&](float pt_x, float pt_y, const float* corners) {
767 float ab0 = corners[2] - corners[0];
768 float ab1 = corners[3] - corners[1];
769 float ad0 = corners[6] - corners[0];
770 float ad1 = corners[7] - corners[1];
771 float ap0 = pt_x - corners[0];
772 float ap1 = pt_y - corners[1];
773 float abab = ab0 * ab0 + ab1 * ab1;
774 float abap = ab0 * ap0 + ab1 * ap1;
775 float adad = ad0 * ad0 + ad1 * ad1;
776 float adap = ad0 * ap0 + ad1 * ap1;
777 return abab >= abap && abap >= 0 && adad >= adap && adap >= 0;
778 };
779
780 float intersection_corners[16];
781 // quadrilateral_intersection
782 int num_of_inter = 0;
783 for (int i = 0; i < 4; i++) {
784 if (point_in_quadrilateral_cpu(corners1[2 * i], corners1[2 * i + 1], corners2)) {
785 intersection_corners[num_of_inter * 2] = corners1[2 * i];
786 intersection_corners[num_of_inter * 2 + 1] = corners1[2 * i + 1];
787 num_of_inter += 1;
788 }
789 if (point_in_quadrilateral_cpu(corners2[2 * i], corners2[2 * i + 1], corners1)) {
790 intersection_corners[num_of_inter * 2] = corners2[2 * i];
791 intersection_corners[num_of_inter * 2 + 1] = corners2[2 * i + 1];
792 num_of_inter += 1;
793 }
794 }
795 for (int i = 0; i < 4; i++) {
796 for (int j = 0; j < 4; j++) {
797 if (line_segment_intersection_cpu(corners1, corners2, i, j, intersection_corners + num_of_inter * 2)) {
798 ++num_of_inter;
799 }
800 }
801 }
802 sort_vertex_in_convex_polygon_cpu(intersection_corners, num_of_inter);
803 // area
804 float area_val = 0.f;
805 for (int i = 0; i < num_of_inter - 2; i++) {
806 area_val += abs(trangle_area_cpu(intersection_corners,
807 intersection_corners + 2 * (i + 1),
808 intersection_corners + 2 * (i + 2)));
809 }
810 return area_val;
811}
812
813std::vector<ResultsOutput> CPDetection::ApplyNMSCPU(std::vector<ResultsOutput>& boxes) {
814 auto get_res_corners = [&](ResultsOutput& o) {
815 std::vector<float> box_corner;
816 box_corner.resize(8);
817 float x = o.x;
818 float y = o.y;
819 float l = o.l;
820 float w = o.w;
821 float r = o.yaw;
822 // if (length_enlarge_value_ > 0) {
823 // l = l + length_enlarge_value_;
824 // }
825 // if (width_enlarge_value_ > 0) {
826 // w = w + width_enlarge_value_;
827 // }
828 float cos_r = cos(r);
829 float sin_r = sin(r);
830 float hl = l * 0.5;
831 float hw = w * 0.5;
832 float x1 = (-hl) * cos_r - (-hw) * sin_r + x;
833 float y1 = (-hl) * sin_r + (-hw) * cos_r + y;
834 float x2 = ( hl) * cos_r - (-hw) * sin_r + x;
835 float y2 = ( hl) * sin_r + (-hw) * cos_r + y;
836 float x3 = ( hl) * cos_r - ( hw) * sin_r + x;
837 float y3 = ( hl) * sin_r + ( hw) * cos_r + y;
838 float x4 = (-hl) * cos_r - ( hw) * sin_r + x;
839 float y4 = (-hl) * sin_r + ( hw) * cos_r + y;
840 box_corner[0] = x1;
841 box_corner[1] = y1;
842 box_corner[2] = x2;
843 box_corner[3] = y2;
844 box_corner[4] = x3;
845 box_corner[5] = y3;
846 box_corner[6] = x4;
847 box_corner[7] = y4;
848 return box_corner;
849 };
850
851/*
852 auto trangle_area_cpu = [&](float* a, float* b, float* c) {
853 return ((a[0] - c[0]) * (b[1] - c[1]) - (a[1] - c[1]) * (b[0] - c[0])) / 2.f;
854 };
855
856 auto sort_vertex_in_convex_polygon_cpu = [&](float* int_pts, int num_of_inter) {
857 if (num_of_inter == 0) {
858 return;
859 }
860 float center_x = 0.f;
861 float center_y = 0.f;
862 for (int i = 0; i < num_of_inter; i++) {
863 center_x += int_pts[2 * i];
864 center_y += int_pts[2 * i + 1];
865 }
866 center_x /= num_of_inter;
867 center_y /= num_of_inter;
868 float v0;
869 float v1;
870 float vs[16];
871 for (int i = 0; i < num_of_inter; i++) {
872 v0 = int_pts[2 * i] - center_x;
873 v1 = int_pts[2 * i + 1] - center_y;
874 float d = sqrt(v0 * v0 + v1 * v1);
875 v0 = v0 / d;
876 v1 = v1 / d;
877 if (v1 < 0) {
878 v0 = -2 - v0;
879 }
880 vs[i] = v0;
881 }
882 int j = 0;
883 float temp = 0.f;
884 for (int i = 0; i < num_of_inter; i++) {
885 if (vs[i - 1] > vs[i]) {
886 temp = vs[i];
887 float tx = int_pts[2 * i];
888 float ty = int_pts[2 * i + 1];
889 j = i;
890 while (j > 0 && vs[j - 1] > temp) {
891 vs[j] = vs[j - 1];
892 int_pts[j * 2] = int_pts[j * 2 - 2];
893 int_pts[j * 2 + 1] = int_pts[j * 2 - 1];
894 j -= 1;
895 }
896 vs[j] = temp;
897 int_pts[j * 2] = tx;
898 int_pts[j * 2 + 1] = ty;
899 }
900 }
901 };
902
903 auto line_segment_intersection_cpu = [&](const float* pts1, const float* pts2, int i, int j, float* temp_pts) {
904 float A0 = pts1[2 * i];
905 float A1 = pts1[2 * i + 1];
906
907 float B0 = pts1[2 * ((i + 1) % 4)];
908 float B1 = pts1[2 * ((i + 1) % 4) + 1];
909
910 float C0 = pts2[2 * j];
911 float C1 = pts2[2 * j + 1];
912
913 float D0 = pts2[2 * ((j + 1) % 4)];
914 float D1 = pts2[2 * ((j + 1) % 4) + 1];
915 float BA0 = B0 - A0;
916 float BA1 = B1 - A1;
917 float DA0 = D0 - A0;
918 float CA0 = C0 - A0;
919 float DA1 = D1 - A1;
920 float CA1 = C1 - A1;
921 bool acd = DA1 * CA0 > CA1 * DA0;
922 bool bcd = (D1 - B1) * (C0 - B0) > (C1 - B1) * (D0 - B0);
923 if (acd != bcd) {
924 bool abc = CA1 * BA0 > BA1 * CA0;
925 bool abd = DA1 * BA0 > BA1 * DA0;
926 if (abc != abd) {
927 float DC0 = D0 - C0;
928 float DC1 = D1 - C1;
929 float ABBA = A0 * B1 - B0 * A1;
930 float CDDC = C0 * D1 - D0 * C1;
931 float DH = BA1 * DC0 - BA0 * DC1;
932 float Dx = ABBA * DC0 - BA0 * CDDC;
933 float Dy = ABBA * DC1 - BA1 * CDDC;
934 temp_pts[0] = Dx / DH;
935 temp_pts[1] = Dy / DH;
936 return true;
937 }
938 }
939 return false;
940 };
941
942 auto point_in_quadrilateral_cpu = [&](float pt_x, float pt_y, const float* corners) {
943 float ab0 = corners[2] - corners[0];
944 float ab1 = corners[3] - corners[1];
945 float ad0 = corners[6] - corners[0];
946 float ad1 = corners[7] - corners[1];
947 float ap0 = pt_x - corners[0];
948 float ap1 = pt_y - corners[1];
949 float abab = ab0 * ab0 + ab1 * ab1;
950 float abap = ab0 * ap0 + ab1 * ap1;
951 float adad = ad0 * ad0 + ad1 * ad1;
952 float adap = ad0 * ap0 + ad1 * ap1;
953 return abab >= abap && abap >= 0 && adad >= adap && adap >= 0;
954 };
955
956 auto rotate_inter = [&](const float *corners1, const float *corners2) {
957 float intersection_corners[16];
958 // quadrilateral_intersection
959 int num_of_inter = 0;
960 for (int i = 0; i < 4; i++) {
961 if (point_in_quadrilateral_cpu(corners1[2 * i], corners1[2 * i + 1], corners2)) {
962 intersection_corners[num_of_inter * 2] = corners1[2 * i];
963 intersection_corners[num_of_inter * 2 + 1] = corners1[2 * i + 1];
964 num_of_inter += 1;
965 }
966 if (point_in_quadrilateral_cpu(corners2[2 * i], corners2[2 * i + 1], corners1)) {
967 intersection_corners[num_of_inter * 2] = corners2[2 * i];
968 intersection_corners[num_of_inter * 2 + 1] = corners2[2 * i + 1];
969 num_of_inter += 1;
970 }
971 }
972 for (int i = 0; i < 4; i++) {
973 for (int j = 0; j < 4; j++) {
974 if (line_segment_intersection_cpu(corners1, corners2, i, j, intersection_corners + num_of_inter * 2)) {
975 ++num_of_inter;
976 }
977 }
978 }
979 sort_vertex_in_convex_polygon_cpu(intersection_corners, num_of_inter);
980 // area
981 float area_val = 0.f;
982 for (int i = 0; i < num_of_inter - 2; i++) {
983 area_val += abs(trangle_area_cpu(intersection_corners,
984 intersection_corners + 2 * (i + 1),
985 intersection_corners + 2 * (i + 2)));
986 }
987 return area_val;
988 };
989*/
990
991 auto BBoxIoU = [&](ResultsOutput& o1, ResultsOutput& o2) {
992 std::vector<float> box_corner1 = get_res_corners(o1);
993 std::vector<float> box_corner2 = get_res_corners(o2);
994 float area_inter = CalculateUnionArea(box_corner1.data(), box_corner2.data());
995 float overlap = area_inter / std::max(o1.l * o1.w + o2.l * o2.w - area_inter, 0.001f);
996 return overlap;
997 // return 0.0;
998 };
999
1000 auto NmsCompare = [&](const ResultsOutput& a, const ResultsOutput& b) {
1001 if (a.confidence == b.confidence) {
1002 return a.model_type_index < b.model_type_index;
1003 }
1004 return a.confidence > b.confidence;
1005 };
1006
1007 std::vector<ResultsOutput> res;
1008 std::sort(boxes.begin(), boxes.end(), NmsCompare);
1009 while (boxes.size() > 0) {
1010 res.push_back(boxes[0]);
1011 int index = 1;
1012 while (index < boxes.size()) {
1013 float iou = BBoxIoU(boxes[0], boxes[index]);
1014 if (iou > nms_overlap_thresh_) {
1015 boxes.erase(boxes.begin() + index);
1016 } else {
1017 index++;
1018 }
1019 }
1020 boxes.erase(boxes.begin());
1021 }
1022 return res;
1023}
1024
1025void CPDetection::GetObjectsCPU() {
1026 // DO NOT remove this line!!!
1027 // Otherwise, the gpu_data will not be updated for the later frames.
1028 // It marks the head at cpu for blob.
1029 int box_range = head_map_["reg"] + head_map_["hei"] + head_map_["dim"];
1030 std::vector<int> cls_range {0};
1031 Timer timer;
1032
1033 float* all_res_box_data = all_res_box_blob_->mutable_cpu_data();
1034 float* all_res_conf_data = all_res_conf_blob_->mutable_cpu_data();
1035 int* all_res_cls_data = all_res_cls_blob_->mutable_cpu_data();
1036 memset(all_res_box_data, 0.f, sizeof(float) * max_candidate_num_ * kBoxBlockSize);
1037 memset(all_res_conf_data, 0.f, sizeof(float) * max_candidate_num_);
1038 memset(all_res_cls_data, -1, sizeof(int) * max_candidate_num_);
1039
1040 res_outputs_.clear();
1041 std::vector<ResultsOutput> total_results;
1042 total_results.clear();
1043 for (int i = 0; i < num_tasks_; ++i) {
1044 std::vector<ResultsOutput> task_results;
1045 task_results.clear();
1046
1047 const float* reg = output_box_blob_->cpu_data() + output_box_blob_->offset(0, box_range * i);
1048 const float* hei = output_box_blob_->cpu_data() + output_box_blob_->offset(0, box_range * i + head_map_["reg"]);
1049 const float* dim = output_box_blob_->cpu_data() + output_box_blob_->offset(0, box_range * i + head_map_["reg"] + head_map_["hei"]);
1050 const float* cls = output_cls_blob_->cpu_data() + output_cls_blob_->offset(0, cls_range[i]);
1051 const float* rot = output_dir_blob_->cpu_data() + output_dir_blob_->offset(0, head_map_["rot"] * i);
1052 cls_range.push_back(cls_range[i] + num_classes_in_task_[i]);
1053 // float* res_box_data = res_box_blob_->mutable_cpu_data();
1054 // float* res_conf_data = res_conf_blob_->mutable_cpu_data();
1055 // int* res_cls_data = res_cls_blob_->mutable_cpu_data();
1056 // memset(res_box_data, 0.f, sizeof(float) * nms_pre_max_size_ * kBoxBlockSize);
1057 // memset(res_conf_data, 0.f, sizeof(float) * nms_pre_max_size_);
1058 // memset(res_cls_data, -1, sizeof(int) * nms_pre_max_size_);
1059
1060 int box_num = 0;
1061 for (int idx = 0; idx < head_map_size_; idx++) {
1062 float max_score = cls[idx];
1063 int label = cls_range[i];
1064 for (int index = 1; index < num_classes_in_task_[i]; ++index) {
1065 float cur_score = cls[idx + index * head_map_size_];
1066 if (cur_score > max_score) {
1067 max_score = cur_score;
1068 label = index + cls_range[i];
1069 }
1070 }
1071 int coor_x = idx % head_x_size_;
1072 int coor_y = idx / head_x_size_;
1073 float conf = 1.0 / (1.0 + exp(-max_score));
1074 if (conf > score_thresh_map_[label]) {
1075 box_num++;
1076 if (box_num >= nms_pre_max_size_) {
1077 continue;
1078 }
1079 float x = (reg[idx + 0 * head_map_size_] + coor_x) * downsample_size_ * voxel_x_size_ + x_min_range_;
1080 float y = (reg[idx + 1 * head_map_size_] + coor_y) * downsample_size_ * voxel_y_size_ + y_min_range_;
1081 float z = hei[idx];
1082 float l = expf(dim[idx + 0 * head_map_size_]);
1083 float w = expf(dim[idx + 1 * head_map_size_]);
1084 float h = expf(dim[idx + 2 * head_map_size_]);
1085 float theta = atan2f(rot[idx], rot[idx + head_map_size_]);
1086 int type = label;
1087 // res_box_data[box_num * kBoxBlockSize + 0] = x;
1088 // res_box_data[box_num * kBoxBlockSize + 1] = y;
1089 // res_box_data[box_num * kBoxBlockSize + 2] = z;
1090 // res_box_data[box_num * kBoxBlockSize + 3] = l;
1091 // res_box_data[box_num * kBoxBlockSize + 4] = w;
1092 // res_box_data[box_num * kBoxBlockSize + 5] = h;
1093 // res_box_data[box_num * kBoxBlockSize + 6] = theta;
1094 // res_conf_data[box_num * kBoxBlockSize + 0] = conf;
1095 // res_cls_data[box_num * kBoxBlockSize + 0] = type;
1096 ResultsOutput output;
1097 output.x = x;
1098 output.y = y;
1099 output.z = z;
1100 output.l = l;
1101 output.w = w;
1102 output.h = h;
1103 output.yaw = theta;
1104 output.model_type_index = type;
1105 output.confidence = conf;
1106 output.point_ids.clear();
1107 task_results.push_back(output);
1108 }
1109 }
1110 // after
1111 // should apply task-NMS
1112 std::vector<ResultsOutput> nms_results = ApplyNMSCPU(task_results);
1113 for (int j = 0; j < nms_results.size(); j++) {
1114 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 0] = res_box_data[idx * kBoxBlockSize + 0];
1115 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 1] = res_box_data[idx * kBoxBlockSize + 1];
1116 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 2] = res_box_data[idx * kBoxBlockSize + 2];
1117 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 3] = res_box_data[idx * kBoxBlockSize + 3];
1118 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 4] = res_box_data[idx * kBoxBlockSize + 4];
1119 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 5] = res_box_data[idx * kBoxBlockSize + 5];
1120 // all_res_box_data[(all_res_num + idx) * kBoxBlockSize + 6] = res_box_data[idx * kBoxBlockSize + 6];
1121 // all_res_conf_data[all_res_num + idx] = res_conf_data[idx];
1122 // all_res_cls_data[all_res_num + idx] = res_cls_data[idx];
1123 ResultsOutput output;
1124 output.x = nms_results[j].x;
1125 output.y = nms_results[j].y;
1126 output.z = nms_results[j].z;
1127 output.l = nms_results[j].l;
1128 output.w = nms_results[j].w;
1129 output.h = nms_results[j].h;
1130 output.yaw = nms_results[j].yaw;
1131 output.model_type_index = nms_results[j].model_type_index;
1132 output.confidence = nms_results[j].confidence;
1133 output.point_ids.clear();
1134 total_results.push_back(output);
1135 }
1136 }
1137
1138 // get total outputs
1139 const int total_box = total_results.size();
1140 res_outputs_.resize(total_box);
1141 for (int i = 0; i < total_box; i++) {
1142 // auto cand_bbox = all_res_box_blob_->cpu_data() + all_res_box_blob_->offset(i);
1143 // auto cand_conf = all_res_conf_blob_->cpu_data() + i;
1144 // auto cand_cls = all_res_cls_blob_->cpu_data() + i;
1145 // float x = cand_bbox[0];
1146 // float y = cand_bbox[1];
1147 // float r = cand_bbox[6];
1148 float x = total_results[i].x;
1149 float y = total_results[i].y;
1150 float r = total_results[i].yaw;
1151 if (enable_rotate_45degree_) {
1152 x = 0.707107 * total_results[i].x + 0.707107 * total_results[i].y;
1153 y = -0.707107 * total_results[i].x + 0.707107 * total_results[i].y;
1154 r -= M_PI / 4;
1155 }
1156 r += (r <= -M_PI ? M_PI : 0.0);
1157 r -= (r >= M_PI ? M_PI : 0.0);
1158 res_outputs_.at(i).clear();
1159 res_outputs_.at(i).x = x;
1160 res_outputs_.at(i).y = y;
1161 res_outputs_.at(i).z = total_results[i].z - total_results[i].h / 2;
1162 res_outputs_.at(i).l = total_results[i].l;
1163 res_outputs_.at(i).w = total_results[i].w;
1164 res_outputs_.at(i).h = total_results[i].h;
1165 res_outputs_.at(i).yaw = r;
1166 res_outputs_.at(i).confidence = total_results[i].confidence;
1167 res_outputs_.at(i).model_type_index = total_results[i].model_type_index;
1168 }
1169 total_results.clear();
1170 double get_time = timer.toc(true);
1171 ADEBUG << "[GetObjectsCPU] time: " << get_time;
1172}
1173
1174void CPDetection::AssignPointsCPU() {
1175 const int num_objects = res_outputs_.size();
1176 const float quantize = voxel_x_size_;
1177 Timer timer;
1178 std::vector<float> box_corner(num_objects * 8);
1179 std::vector<float> box_rectangular(num_objects * 4);
1180 for (int i = 0; i < num_objects; ++i) {
1181 float xx = res_outputs_[i].x;
1182 float yy = res_outputs_[i].y;
1183 float l = res_outputs_[i].l;
1184 float w = res_outputs_[i].w;
1185 float a = res_outputs_[i].yaw;
1186 // should revert to original format
1187 float x = xx;
1188 float y = yy;
1189 if (enable_rotate_45degree_) {
1190 x = 0.707107 * xx - 0.707107 * yy;
1191 y = 0.707107 * xx + 0.707107 * yy;
1192 a += M_PI / 4;
1193 }
1194 if (quantize > 0) {
1195 w = ceil(w / quantize) * quantize;
1196 l = ceil(l / quantize) * quantize;
1197 }
1198 if (width_enlarge_value_ > 0) {
1199 w = w + width_enlarge_value_;
1200 }
1201 if (length_enlarge_value_ > 0) {
1202 l = l + length_enlarge_value_;
1203 }
1204 float cos_a = cos(a);
1205 float sin_a = sin(a);
1206 float hl = l * 0.5;
1207 float hw = w * 0.5;
1208
1209 float x1 = (-hl) * cos_a - (-hw) * sin_a + x;
1210 float y1 = (-hl) * sin_a + (-hw) * cos_a + y;
1211 float x2 = ( hl) * cos_a - (-hw) * sin_a + x;
1212 float y2 = ( hl) * sin_a + (-hw) * cos_a + y;
1213 float x3 = ( hl) * cos_a - ( hw) * sin_a + x;
1214 float y3 = ( hl) * sin_a + ( hw) * cos_a + y;
1215 float x4 = (-hl) * cos_a - ( hw) * sin_a + x;
1216 float y4 = (-hl) * sin_a + ( hw) * cos_a + y;
1217 box_corner[i * 8 + 0] = x1;
1218 box_corner[i * 8 + 1] = y1;
1219 box_corner[i * 8 + 2] = x2;
1220 box_corner[i * 8 + 3] = y2;
1221 box_corner[i * 8 + 4] = x3;
1222 box_corner[i * 8 + 5] = y3;
1223 box_corner[i * 8 + 6] = x4;
1224 box_corner[i * 8 + 7] = y4;
1225 box_rectangular[i * 4 + 0] = std::min(std::min(std::min(x1, x2), x3), x4);
1226 box_rectangular[i * 4 + 1] = std::min(std::min(std::min(y1, y2), y3), y4);
1227 box_rectangular[i * 4 + 2] = std::max(std::max(std::max(x1, x2), x3), x4);
1228 box_rectangular[i * 4 + 3] = std::max(std::max(std::max(y1, y2), y3), y4);
1229 }
1230 double nms_time = timer.toc(true);
1231
1232 for (int point_idx = 0; point_idx < original_cloud_->size(); ++point_idx) {
1233 bool semantic_flag = (static_cast<PointSemanticLabel>(
1234 original_cloud_->points_semantic_label(point_idx) & 15) ==
1235 PointSemanticLabel::GROUND);
1236 bool raw_flag = (original_cloud_->points_label(point_idx) ==
1237 static_cast<uint8_t>(LidarPointLabel::GROUND));
1238 if ((remove_semantic_ground_ && semantic_flag) ||
1239 (remove_raw_ground_ && raw_flag)) {
1240 continue;
1241 }
1242 const auto &point = original_cloud_->at(point_idx);
1243 float px = point.x;
1244 float py = point.y;
1245 float pz = point.z;
1246 if (enable_rotate_45degree_) {
1247 px = 0.707107 * point.x - 0.707107 * point.y;
1248 py = 0.707107 * point.x + 0.707107 * point.y;
1249 }
1250 for (int i = 0; i < num_objects; i++) {
1251 if (px < box_rectangular[i * 4 + 0] || px > box_rectangular[i * 4 + 2]) {
1252 continue;
1253 }
1254 if (py < box_rectangular[i * 4 + 1] || py > box_rectangular[i * 4 + 3]) {
1255 continue;
1256 }
1257 float z = res_outputs_[i].z;
1258 float h = res_outputs_[i].h;
1259 z = z + h / 2;
1260 if (pz < (z - h / 2 - bottom_enlarge_value_) || pz > (z + h / 2 + top_enlarge_value_)) {
1261 continue;
1262 }
1263 float x1 = box_corner[i * 8 + 0];
1264 float x2 = box_corner[i * 8 + 2];
1265 float x3 = box_corner[i * 8 + 4];
1266 float x4 = box_corner[i * 8 + 6];
1267 float y1 = box_corner[i * 8 + 1];
1268 float y2 = box_corner[i * 8 + 3];
1269 float y3 = box_corner[i * 8 + 5];
1270 float y4 = box_corner[i * 8 + 7];
1271 double angl1 = (px - x1) * (x2 - x1) + (py - y1) * (y2 - y1);
1272 double angl2 = (px - x2) * (x3 - x2) + (py - y2) * (y3 - y2);
1273 double angl3 = (px - x3) * (x4 - x3) + (py - y3) * (y4 - y3);
1274 double angl4 = (px - x4) * (x1 - x4) + (py - y4) * (y1 - y4);
1275 if ((angl1 < 0 && angl2 < 0 && angl3 < 0 && angl4 < 0) ||
1276 (angl1 > 0 && angl2 > 0 && angl3 > 0 && angl4 > 0)) {
1277 res_outputs_.at(i).point_ids.push_back(point_idx);
1278 }
1279 }
1280 }
1281 double assign_point_time = timer.toc(true);
1282 ADEBUG << "[AssignPointsCPU] total_point size " << original_cloud_->size()
1283 << " nms_box_time: " << nms_time
1284 << " assign point time: " << assign_point_time;
1285}
1286
1287void CPDetection::FilterObjectsbyClassNMS(
1288 std::vector<std::shared_ptr<base::Object>> *objects) {
1289 auto ordinary_nms_strategy = [&](size_t i, size_t j,
1290 bool use_strategy = false) {
1291 // return FILTER_INDEX
1292 size_t type_i = static_cast<size_t>(objects->at(i)->type);
1293 size_t type_j = static_cast<size_t>(objects->at(j)->type);
1294 size_t filter_index =
1295 (objects->at(i)->confidence < objects->at(j)->confidence) ? i : j;
1296 // ordinary_nms_strategy: VEHICLE > BICYCLE > PEDESTRIAN > TRAFFICCONE
1297 if (nms_strategy_ || use_strategy) {
1298 return type_i > type_j ? j : i;
1299 }
1300 return filter_index;
1301 };
1302
1303 auto special_nms_strategy = [&](size_t keep_index,
1304 base::ObjectType keep_type, size_t filter_index,
1305 base::ObjectType filter_type, float conf_buffer) {
1306 if (nms_strategy_) {
1307 return;
1308 }
1309 objects->at(keep_index)->lidar_supplement.raw_probs.clear();
1310 objects->at(keep_index)->type_probs.clear();
1311
1312 objects->at(keep_index)->lidar_supplement.raw_probs.push_back(
1313 std::vector<float>(static_cast<int>(
1314 base::ObjectType::MAX_OBJECT_TYPE), 0.f));
1315 float sum_confidence = objects->at(keep_index)->confidence +
1316 objects->at(filter_index)->confidence;
1317 float filter_prob = 1.0 * (objects->at(filter_index)->confidence
1318 + conf_buffer) / (sum_confidence + conf_buffer);
1319 float keep_prob = 1.0 * objects->at(keep_index)->confidence /
1320 (sum_confidence + conf_buffer);
1321 objects->at(keep_index)->lidar_supplement.raw_probs.back()[
1322 static_cast<int>(filter_type)] = filter_prob;
1323 objects->at(keep_index)->lidar_supplement.raw_probs.back()[
1324 static_cast<int>(keep_type)] = keep_prob;
1325 objects->at(keep_index)->type_probs.assign(
1326 objects->at(keep_index)->lidar_supplement.raw_probs.back().begin(),
1327 objects->at(keep_index)->lidar_supplement.raw_probs.back().end());
1328 };
1329
1330 auto get_obj_corners = [&](const base::ObjectPtr obj) {
1331 std::vector<float> box_corner;
1332 box_corner.resize(8);
1333 float x = obj->center(0);
1334 float y = obj->center(1);
1335 float l = obj->size(0);
1336 float w = obj->size(1);
1337 float r = obj->theta;
1338 // if (length_enlarge_value_ > 0) {
1339 // l = l + length_enlarge_value_;
1340 // }
1341 // if (width_enlarge_value_ > 0) {
1342 // w = w + width_enlarge_value_;
1343 // }
1344 float cos_r = cos(r);
1345 float sin_r = sin(r);
1346 float hl = l * 0.5;
1347 float hw = w * 0.5;
1348 float x1 = (-hl) * cos_r - (-hw) * sin_r + x;
1349 float y1 = (-hl) * sin_r + (-hw) * cos_r + y;
1350 float x2 = ( hl) * cos_r - (-hw) * sin_r + x;
1351 float y2 = ( hl) * sin_r + (-hw) * cos_r + y;
1352 float x3 = ( hl) * cos_r - ( hw) * sin_r + x;
1353 float y3 = ( hl) * sin_r + ( hw) * cos_r + y;
1354 float x4 = (-hl) * cos_r - ( hw) * sin_r + x;
1355 float y4 = (-hl) * sin_r + ( hw) * cos_r + y;
1356 box_corner[0] = x1;
1357 box_corner[1] = y1;
1358 box_corner[2] = x2;
1359 box_corner[3] = y2;
1360 box_corner[4] = x3;
1361 box_corner[5] = y3;
1362 box_corner[6] = x4;
1363 box_corner[7] = y4;
1364 return box_corner;
1365 };
1366
1367 auto calculate_nms = [&](base::ObjectPtr obj1, base::ObjectPtr obj2) {
1368 std::vector<float> box_corner1 = get_obj_corners(obj1);
1369 std::vector<float> box_corner2 = get_obj_corners(obj2);
1370 float area_inter = CalculateUnionArea(box_corner1.data(), box_corner2.data());
1371 float overlap = area_inter / std::max(obj1->size(0) * obj1->size(1) + obj2->size(0) * obj2->size(1) - area_inter, 0.001f);
1372 return overlap;
1373 };
1374
1375 std::vector<bool> delete_array(objects->size(), false);
1376 std::vector<bool> nms_visited(objects->size(), false);
1377 std::vector<std::vector<size_t>> nms_pairs;
1378 nms_pairs.resize(objects->size());
1379 // different class nms
1380 for (size_t i = 0; i < objects->size(); i++) {
1381 auto &obj_i = objects->at(i);
1382 if (!obj_i || nms_visited[i]) {
1383 continue;
1384 }
1385 for (size_t j = i + 1; j < objects->size(); j++) {
1386 auto &obj_j = objects->at(j);
1387 if (!obj_j) {
1388 continue;
1389 }
1390 if (calculate_nms(obj_i, obj_j) <= class_nms_iou_thres_) {
1391 continue;
1392 }
1393 if (static_cast<int>(obj_i->type) == static_cast<int>(obj_j->type)) {
1394 continue;
1395 }
1396 nms_visited[j] = true;
1397 nms_pairs[i].push_back(j);
1398 }
1399 }
1400 // do NMS
1401 for (size_t i = 0; i < objects->size(); i++) {
1402 if (nms_pairs[i].size() == 0) {
1403 continue;
1404 }
1405 if (nms_pairs[i].size() == 1) {
1406 size_t filter_index = ordinary_nms_strategy(i, nms_pairs[i][0]);
1407 size_t keep_index = filter_index == i ? nms_pairs[i][0] : i;
1408
1409 delete_array[keep_index] = false;
1410 delete_array[filter_index] = true;
1411
1412 // special-nms-strategy
1413 auto filter_type = objects->at(filter_index)->type;
1414 auto reserve_type = objects->at(keep_index)->type;
1415 // Only trafficcone reserve but bicycle filter -> add bicycle conf
1416 if (filter_type == base::ObjectType::BICYCLE &&
1417 reserve_type == base::ObjectType::UNKNOWN) {
1418 special_nms_strategy(keep_index, base::ObjectType::UNKNOWN,
1419 filter_index, base::ObjectType::BICYCLE, 0.3);
1420 }
1421 // Only pedestrian reserve but bicycle filter -> add bicycle conf
1422 if (filter_type == base::ObjectType::BICYCLE &&
1423 reserve_type == base::ObjectType::PEDESTRIAN) {
1424 special_nms_strategy(keep_index, base::ObjectType::PEDESTRIAN,
1425 filter_index, base::ObjectType::BICYCLE, 0.2);
1426 }
1427 // Only trafficcone reserve but ped filter -> add ped conf
1428 // if (filter_type == base::ObjectType::PEDESTRIAN &&
1429 // reserve_type == base::ObjectType::UNKNOWN) {
1430 // special_nms_strategy(keep_index, base::ObjectType::UNKNOWN,
1431 // filter_index, base::ObjectType::PEDESTRIAN, 0.2);
1432 // }
1433 // Only ped reserve but trafficcone filter -> add trafficcone conf
1434 if (filter_type == base::ObjectType::UNKNOWN &&
1435 reserve_type == base::ObjectType::PEDESTRIAN) {
1436 special_nms_strategy(keep_index, base::ObjectType::PEDESTRIAN,
1437 filter_index, base::ObjectType::UNKNOWN, 0.2);
1438 }
1439 ADEBUG << "Only Two NMS: reserve " << keep_index
1440 << " filter " << filter_index;
1441 }
1442 if (nms_pairs[i].size() >= 2) {
1443 // car strategy
1444 bool have_car = false;
1445 if (objects->at(i)->type == base::ObjectType::VEHICLE) {
1446 have_car = true;
1447 }
1448 for (size_t k = 0; k < nms_pairs[i].size() && !have_car; k++) {
1449 auto type = objects->at(nms_pairs[i][k])->type;
1450 if (type == base::ObjectType::VEHICLE) {
1451 have_car = true;
1452 break;
1453 }
1454 }
1455 // no car -> BICYCLE priorTo PEDESTRIAN priorTo TRAFFICCONE
1456 // have car -> confidence
1457 if (have_car) {
1458 auto keep_conf = objects->at(i)->confidence;
1459 auto keep_index = i;
1460 for (size_t k = 0; k < nms_pairs[i].size(); k++) {
1461 if (objects->at(nms_pairs[i][k])->confidence > keep_conf) {
1462 delete_array[keep_index] = true;
1463 keep_conf = objects->at(nms_pairs[i][k])->confidence;
1464 keep_index = nms_pairs[i][k];
1465 } else {
1466 delete_array[nms_pairs[i][k]] = true;
1467 ADEBUG << "NMS filter_index: " << nms_pairs[i][k];
1468 }
1469 }
1470 } else {
1471 auto keep_type = static_cast<size_t>(objects->at(i)->type);
1472 auto keep_index = i;
1473 for (size_t k = 0; k < nms_pairs[i].size(); k++) {
1474 if (static_cast<size_t>(
1475 objects->at(nms_pairs[i][k])->type) > keep_type) {
1476 delete_array[keep_index] = true;
1477 keep_type = static_cast<size_t>(
1478 objects->at(nms_pairs[i][k])->type);
1479 keep_index = nms_pairs[i][k];
1480 } else {
1481 delete_array[nms_pairs[i][k]] = true;
1482 ADEBUG << "NMS filter_index: " << nms_pairs[i][k];
1483 }
1484 }
1485 }
1486 }
1487 }
1488 size_t valid_num = 0;
1489 for (size_t i = 0; i < objects->size(); i++) {
1490 auto &object = objects->at(i);
1491 if (!delete_array[i]) {
1492 objects->at(valid_num) = object;
1493 valid_num++;
1494 }
1495 }
1496 objects->resize(valid_num);
1497}
1498
1499void CPDetection::FilterObjectsbySemanticType(
1500 std::vector<std::shared_ptr<base::Object>> *objects) {
1501 std::vector<bool> filter_flag(objects->size(), false);
1502 for (size_t i = 0; i < objects->size(); i++) {
1503 auto object = objects->at(i);
1504 std::vector<int> type_count(static_cast<int>(PointSemanticLabel::MAX_LABEL), 0);
1505 for (size_t k = 0; k < object->lidar_supplement.cloud.size(); k++) {
1506 // uint8_t value = object->lidar_supplement.cloud.points_semantic_label(k);
1507 // int index = static_cast<int>(value);
1508 int index = static_cast<int>(GetSemanticLabel(object->lidar_supplement.cloud.points_semantic_label(k)));
1509 if (index >= 0 && index < static_cast<int>(PointSemanticLabel::MAX_LABEL)) {
1510 type_count[index]++;
1511 }
1512 }
1513 // get max index
1514 int max_value = -1;
1515 int max_index = 0;
1516 for (size_t j = 0; j < type_count.size(); j++) {
1517 if (type_count.at(j) > max_value) {
1518 max_value = type_count.at(j);
1519 max_index = j;
1520 }
1521 }
1522 if (fore_semantic_filter_map_.find(max_index) == fore_semantic_filter_map_.end()) {
1523 // Not-write == No-filter
1524 continue;
1525 }
1526 float ratio = static_cast<float>(object->lidar_supplement.num_points_in_roi * 1.0f /
1527 object->lidar_supplement.cloud.size());
1528 ratio = 1 - ratio;
1529 float range = sqrt(object->center(0) * object->center(0) + object->center(1) * object->center(1));
1530 if (ratio >= fore_semantic_filter_map_.at(max_index).first &&
1531 range <= fore_semantic_filter_map_.at(max_index).second) {
1532 ADEBUG << "id = " << object->id << ": total_size: " << object->lidar_supplement.cloud.size() << ", roi_size: "
1533 << object->lidar_supplement.num_points_in_roi << ", out_ratio: " << ratio
1534 << ", semantic_type: " << max_index;
1535 filter_flag.at(i) = true;
1536 }
1537 }
1538 // filter object which semantic type is not unknown
1539 int valid_size = 0;
1540 for (size_t i = 0; i < objects->size(); i++) {
1541 if (filter_flag.at(i)) {
1542 continue;
1543 }
1544 objects->at(valid_size) = objects->at(i);
1545 valid_size++;
1546 }
1547 AINFO << "Filter " << (objects->size() - valid_size) << " objects from " << objects->size() << " objects.";
1548 objects->resize(valid_size);
1549}
1550
1551bool CPDetection::Detect(const CPDetectorOptions &options,
1552 LidarFrame *frame) {
1553 if (frame == nullptr) {
1554 AERROR << "Input null frame ptr.";
1555 return false;
1556 }
1557 if (frame->cloud == nullptr) {
1558 AERROR << "Input null frame cloud.";
1559 return false;
1560 }
1561 if (frame->cloud->size() == 0) {
1562 AERROR << "Input none points.";
1563 return false;
1564 }
1565 frame->segmented_objects.clear();
1566 original_cloud_ = frame->cloud;
1567 original_world_cloud_ = frame->world_cloud;
1568 lidar_frame_ref_ = frame;
1569 cur_cloud_ptr_ = std::shared_ptr<base::PointFCloud>(
1570 new base::PointFCloud(*original_cloud_));
1571
1572 // BASE_GPU_CHECK(cudaStreamCreate(&stream_));
1573 if (cudaSetDevice(cpdet_config_.model_param().preprocess().gpu_id()) != cudaSuccess) {
1574 AERROR << "Failed to set device to "
1575 << cpdet_config_.model_param().preprocess().gpu_id();
1576 return false;
1577 }
1578
1579 Timer timer;
1580 PointCloudPreprocess();
1581 AINFO << "Downsample PointCloud from " << original_cloud_->size() << " to " << cur_cloud_ptr_->size();
1582 double pointcloud_pre_time = timer.toc(true);
1583
1584 model_cloud_size_ = static_cast<int>(cur_cloud_ptr_->size());
1585 total_cloud_size_ = static_cast<int>(original_cloud_->size());
1586 GeneratePfnFeature();
1587 double pfn_feature_generate_time = timer.toc(true);
1588
1589 // pfn_output
1590 pfn_inference_->Infer();
1591 // cudaStreamSynchronize(stream_);
1592 double pfn_inference_time = timer.toc(true);
1593
1594 // generate backbone feature
1595 GenerateBackboneFeature(pfn_pillar_feature_blob_.get());
1596 double backbone_feature_generate_time = timer.toc(true);
1597
1598 // backbone output
1599 backbone_inference_->Infer();
1600 // cudaStreamSynchronize(stream_);
1601 double backbone_inference_time = timer.toc(true);
1602
1603 GetObjectsAndAssignPoints();
1604 double get_objects_and_assign_time = timer.toc(true);
1605
1606 GenerateObjects(frame);
1607 std::stringstream sstr;
1608 sstr << "[CPDetection BeforeNMS] "
1609 << std::to_string(frame->timestamp)
1610 << " objs: " << frame->segmented_objects.size() << std::endl;
1611 for (auto obj : frame->segmented_objects) {
1612 sstr << "id = " << obj->id << ": " << obj->center(0) << ", "
1613 << obj->center(1) << ", " << obj->center(2) << ", "
1614 << obj->size(0) << ", " << obj->size(1) << ", "
1615 << obj->size(2) << ", " << obj->theta << ", "
1616 << static_cast<int>(obj->type) << ", "
1617 << obj->confidence << std::endl;
1618 }
1619 ADEBUG << sstr.str();
1620 // filter
1621 if (num_tasks_ > 1 && inter_class_nms_) {
1622 FilterObjectsbyClassNMS(&frame->segmented_objects);
1623 }
1624 std::stringstream ssstr;
1625 ssstr << "[CenterPointDetection AfterNMS] "
1626 << std::to_string(frame->timestamp)
1627 << " objs: " << frame->segmented_objects.size() << std::endl;
1628 for (auto obj : frame->segmented_objects) {
1629 ssstr << "id = " << obj->id << ": " << obj->center(0) << ", "
1630 << obj->center(1) << ", " << obj->center(2) << ", "
1631 << obj->size(0) << ", " << obj->size(1) << ", "
1632 << obj->size(2) << ", " << obj->theta << ", "
1633 << static_cast<int>(obj->type) << std::endl;
1634 }
1635 ADEBUG << ssstr.str();
1636
1637 // filter semantic if it's not unknown
1638 SetPointsInROI(&frame->segmented_objects);
1639 if (filter_by_semantic_type_) {
1640 FilterObjectsbySemanticType(&frame->segmented_objects);
1641 }
1642 RemoveForePoints(&frame->segmented_objects);
1643
1644 res_outputs_.clear();
1645 double post_process_time = timer.toc(true);
1646
1647 double total_time = pointcloud_pre_time + pfn_feature_generate_time +
1648 pfn_inference_time + backbone_feature_generate_time +
1649 backbone_inference_time + get_objects_and_assign_time + post_process_time;
1650 AINFO << "[CpDetTime] " << std::to_string(frame->timestamp)
1651 << ", pointcloud preprocess: " << pointcloud_pre_time
1652 << ", pfn feature generate: " << pfn_feature_generate_time
1653 << ", pfn inference: " << pfn_inference_time
1654 << ", backbone feature generate: " << backbone_feature_generate_time
1655 << ", backbone inference: " << backbone_inference_time
1656 << ", getObjects and assignPoints: " << get_objects_and_assign_time
1657 << ", postProcess: " << post_process_time
1658 << ", Total: " << total_time;
1659 return true;
1660}
1661
1662PERCEPTION_REGISTER_LIDARDETECTOR(CPDetection);
1663
1664} // namespace lidar
1665} // namespace perception
1666} // namespace apollo
PERCEPTION_REGISTER_LIDARDETECTOR
#define PERCEPTION_REGISTER_LIDARDETECTOR(name)
Definition base_cpdetector.h:85
apollo::cyber::plugin_manager::PluginManager::Instance
static PluginManager * Instance()
get singleton instance of PluginManager
Definition plugin_manager.cc:203
apollo::cyber::plugin_manager::PluginManager::CreateInstance
std::shared_ptr< Base > CreateInstance(const std::string &derived_class)
create plugin instance of derived class based on Base
Definition plugin_manager.h:150
apollo::perception::base::Blob
A wrapper around SyncedMemory holders serving as the basic computational unit for images,...
Definition blob.h:88
apollo::perception::lidar::CPDetection::Init
bool Init(const CPDetectorInitOptions &options=CPDetectorInitOptions()) override
Init the Base Lidar Detector object
Definition cpdet.cc:56
apollo::perception::lidar::CPDetection::Detect
bool Detect(const CPDetectorOptions &options, LidarFrame *frame) override
Detect foreground objects
Definition cpdet.cc:1551
apollo::perception::lidar::CPDetection::Name
std::string Name() const
Name of Base Lidar Detector class
Definition cpdet.h:83
apollo::perception::lidar::CPDetection::GenerateBackboneFeature
void GenerateBackboneFeature(const base::Blob< float > *pillar_feature_blob)
Definition cpdet.h:93
apollo::perception::lidar::ConfigUtil::GetFullClassName
static std::string GetFullClassName(const std::string &class_name)
Given the class name of perception module, combine the namespace "apollo::perception::lidar::" with i...
Definition config_util.cc:23
apollo::perception::lidar::Timer::toc
double toc(bool reset=false)
Definition lidar_timer.h:28
cloud_mask.h
lidar_timer.h
ACHECK
#define ACHECK(cond)
Definition log.h:80
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
model_util.h
apollo::common::math::cos
float cos(Angle16 a)
Definition angle.cc:42
apollo::common::math::sin
float sin(Angle16 a)
Definition angle.cc:25
apollo::cyber::common::GetProtoFromFile
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
apollo::perception::base::kSubType2TypeMap
const std::map< ObjectSubType, ObjectType > kSubType2TypeMap
ObjectSubType mapping
Definition object_types.h:230
apollo::perception::base::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition object.h:127
apollo::perception::inference::GetBlobNames
std::vector< std::string > GetBlobNames(const google::protobuf::RepeatedPtrField< common::ModelBlob > &model_blobs)
Definition model_util.cc:23
apollo::perception::inference::AddShape
void AddShape(std::map< std::string, std::vector< int > > *shape_map, const google::protobuf::RepeatedPtrField< common::ModelBlob > &model_blobs)
Definition model_util.cc:32
apollo::perception::lidar::MIN_X_RANGE
const float MIN_X_RANGE
Definition cpdet.cc:45
apollo::perception::lidar::MAX_Z_RANGE
const float MAX_Z_RANGE
Definition cpdet.cc:50
apollo::perception::lidar::GetSemanticLabel
PointSemanticLabel GetSemanticLabel(uint8_t value)
Definition lidar_point_label.h:56
apollo::perception::lidar::Pc2Pixel
int Pc2Pixel(float in_pc, float in_range, float out_size)
Definition util.h:47
apollo::perception::lidar::MAX_X_RANGE
const float MAX_X_RANGE
Definition cpdet.cc:46
apollo::perception::lidar::MIN_Y_RANGE
const float MIN_Y_RANGE
Definition cpdet.cc:47
apollo::perception::lidar::MIN_Z_RANGE
const float MIN_Z_RANGE
Definition cpdet.cc:49
apollo::perception::lidar::MAX_Y_RANGE
const float MAX_Y_RANGE
Definition cpdet.cc:48
apollo::perception::lidar::abs
T abs(const T &x)
Get absolute value of x
Definition misc.h:63
apollo::perception::GetModelFile
std::string GetModelFile(const std::string &model_name, const std::string &file_name)
Get the model file path by model path and file name
Definition util.cc:55
apollo::perception::GetConfigFile
std::string GetConfigFile(const std::string &config_path, const std::string &config_file)
Definition util.cc:80
apollo::perception::GetModelPath
std::string GetModelPath(const std::string &model_name)
Get the model path by model name, search from APOLLO_MODEL_PATH
Definition util.cc:44
apollo
class register implement
Definition arena_queue.h:37
object_pool_types.h
pcl_util.h
config_util.h
point_cloud_util.h
apollo::perception::lidar::LidarFrame::world_cloud
std::shared_ptr< base::AttributePointCloud< base::PointD > > world_cloud
Definition lidar_frame.h:37
apollo::perception::lidar::LidarFrame::segmented_objects
std::vector< std::shared_ptr< base::Object > > segmented_objects
Definition lidar_frame.h:53
apollo::perception::lidar::LidarFrame::cloud
std::shared_ptr< base::AttributePointCloud< base::PointF > > cloud
Definition lidar_frame.h:35
apollo::perception::lidar::cpdet::CPDetConfig::post_process
optional CPDetPostProcessParam post_process
Definition model_param.proto:104
apollo::perception::lidar::cpdet::CPDetFilter::fore_semantic_filter
repeated ForeSemanticFilter fore_semantic_filter
Definition model_param.proto:95
apollo::perception::lidar::cpdet::CPDetModelParam::preprocess
optional common::PointCloudPreProcess preprocess
Definition model_param.proto:75