From 986fe474272413fca0621b245d1c7aa6a1e75316 Mon Sep 17 00:00:00 2001 From: SeasoulChris <274224843@qq.com> Date: Thu, 21 Jul 2022 13:01:15 +0800 Subject: [PATCH] add centerpoint detector --- .../perception/common/perception_gflags.cc | 29 +- modules/perception/common/perception_gflags.h | 10 + modules/perception/lidar/app/BUILD | 1 + .../lidar/app/lidar_obstacle_detection.cc | 3 +- .../lib/detector/center_point_detection/BUILD | 42 ++ .../center_point_detection.cc | 473 ++++++++++++++++++ .../center_point_detection.h | 125 +++++ .../detector/center_point_detection/common.h | 72 +++ .../detector/center_point_detection/params.h | 51 ++ third_party/centerpoint_infer_op/BUILD | 2 +- third_party/paddleinference/BUILD | 2 +- 11 files changed, 805 insertions(+), 5 deletions(-) create mode 100644 modules/perception/lidar/lib/detector/center_point_detection/BUILD create mode 100644 modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.cc create mode 100644 modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.h create mode 100644 modules/perception/lidar/lib/detector/center_point_detection/common.h create mode 100644 modules/perception/lidar/lib/detector/center_point_detection/params.h diff --git a/modules/perception/common/perception_gflags.cc b/modules/perception/common/perception_gflags.cc index 44d38fd91f4..a87b37cb739 100644 --- a/modules/perception/common/perception_gflags.cc +++ b/modules/perception/common/perception_gflags.cc @@ -115,6 +115,32 @@ DEFINE_string(mask_bbox_head_torch_file, "models/detection/mask_pillars/pts_bbox_head.zip", "The path of pillars bbox head torch file."); +// lidar_center_point +DEFINE_string(center_point_model_file, + "/apollo/modules/perception/production/data/perception/lidar/" + "models/detection/center_point/centerpoint.pdmodel", + "The path of center point model file."); +DEFINE_string(center_point_params_file, + "/apollo/modules/perception/production/data/perception/lidar/" + "models/detection/center_point/centerpoint.pdiparams", + "The path of center point params file."); +DEFINE_bool(use_trt, true, "True if preprocess in CPU mode."); +DEFINE_int32(trt_precision, 1, + "Precision type of tensorrt, 0: kFloat32, 1: kHalf"); +DEFINE_int32( + trt_use_static, 0, + "Whether to load the tensorrt graph optimization from a disk path"); +DEFINE_string( + trt_static_dir, + "/apollo/modules/perception/lidar/lib/detector/center_point_detection/", + "Path of a tensorrt graph optimization directory"); +DEFINE_int32(collect_shape_info, 1, + "Whether to collect dynamic shape before using tensorrt"); +DEFINE_string(dynamic_shape_file, + "/apollo/modules/perception/production/data/perception/lidar/" + "models/detection/center_point/collect_shape_info.pbtxt", + "Path of a dynamic shape file for tensorrt"); + // emergency detection onnx DEFINE_string(onnx_obstacle_detector_model, "/apollo/modules/perception/camera" @@ -141,7 +167,6 @@ DEFINE_string(torch_detector_model, "The torch model file for emergency detection"); // lidar sensor name -DEFINE_string(lidar_sensor_name, "velodyne128", - "lidar sensor name"); +DEFINE_string(lidar_sensor_name, "velodyne128", "lidar sensor name"); } // namespace perception } // namespace apollo diff --git a/modules/perception/common/perception_gflags.h b/modules/perception/common/perception_gflags.h index 09acf9ddb41..2836c457718 100644 --- a/modules/perception/common/perception_gflags.h +++ b/modules/perception/common/perception_gflags.h @@ -65,6 +65,16 @@ DECLARE_string(mask_backbone_torch_file); DECLARE_string(mask_fpn_torch_file); DECLARE_string(mask_bbox_head_torch_file); +// lidar_center_point +DECLARE_string(center_point_model_file); +DECLARE_string(center_point_params_file); +DECLARE_bool(use_trt); +DECLARE_int32(trt_precision); +DECLARE_int32(trt_use_static); +DECLARE_string(trt_static_dir); +DECLARE_int32(collect_shape_info); +DECLARE_string(dynamic_shape_file); + // emergency detection onnx DECLARE_string(onnx_obstacle_detector_model); DECLARE_string(onnx_test_input_path); diff --git a/modules/perception/lidar/app/BUILD b/modules/perception/lidar/app/BUILD index 3319a9c6736..05c655c4e16 100644 --- a/modules/perception/lidar/app/BUILD +++ b/modules/perception/lidar/app/BUILD @@ -29,6 +29,7 @@ cc_library( "//modules/perception/lidar/lib/interface:base_lidar_obstacle_detection", "//modules/perception/lidar/lib/pointcloud_preprocessor:pointcloud_preprocessor", "//modules/perception/lidar/lib/detector/point_pillars_detection:point_pillars_detection", + "//modules/perception/lidar/lib/detector/center_point_detection:center_point_detection", "//modules/perception/lidar/lib/detector/cnn_segmentation:cnn_segmentation", "//modules/perception/lidar/lib/detector/mask_pillars_detection:mask_pillars_detection", "//modules/perception/lidar/lib/detector/ncut_segmentation:ncut_segmentation", diff --git a/modules/perception/lidar/app/lidar_obstacle_detection.cc b/modules/perception/lidar/app/lidar_obstacle_detection.cc index 41e2ec91244..25418d70a72 100644 --- a/modules/perception/lidar/app/lidar_obstacle_detection.cc +++ b/modules/perception/lidar/app/lidar_obstacle_detection.cc @@ -47,7 +47,8 @@ bool LidarObstacleDetection::Init( use_map_manager_ = config.use_map_manager(); use_object_filter_bank_ = config.use_object_filter_bank(); use_object_builder_ = ("PointPillarsDetection" != config.detector() || - "MaskPillarsDetection" != config.detector()); + "MaskPillarsDetection" != config.detector() || + "CenterPointDetection" != config.detector()); use_map_manager_ = use_map_manager_ && options.enable_hdmap_input; diff --git a/modules/perception/lidar/lib/detector/center_point_detection/BUILD b/modules/perception/lidar/lib/detector/center_point_detection/BUILD new file mode 100644 index 00000000000..feb728edc1b --- /dev/null +++ b/modules/perception/lidar/lib/detector/center_point_detection/BUILD @@ -0,0 +1,42 @@ +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") +load("@local_config_cuda//cuda:build_defs.bzl", "cuda_library") +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "center_point_detection", + srcs = [ + "center_point_detection.cc", + ], + hdrs = [ + "center_point_detection.h", + ], + deps = [ + ":params", + "//cyber/common", + "//modules/perception/base", + "//modules/perception/common:perception_gflags", + "//modules/perception/lidar/common", + "//modules/perception/lidar/lib/interface:base_lidar_detector", + "//modules/perception/lib/thread", + "@centerpoint_infer_op//:centerpoint_infer_op", + "@eigen", + "@local_config_pcl//:pcl", + "@local_config_tensorrt//:tensorrt", + "@paddleinference//:paddleinference_lib", + ], + alwayslink = True, +) + +cc_library( + name = "common", + hdrs = ["common.h"], +) + +cc_library( + name = "params", + hdrs = ["params.h"], +) + +cpplint() diff --git a/modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.cc b/modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.cc new file mode 100644 index 00000000000..fc1d4633151 --- /dev/null +++ b/modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.cc @@ -0,0 +1,473 @@ +/****************************************************************************** + * Copyright 2022 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ +#include "modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.h" + +#include +#include +#include +#include + +#include + +#include "cyber/common/file.h" +#include "cyber/common/log.h" +#include "modules/perception/base/object_pool_types.h" +#include "modules/perception/base/point_cloud_util.h" +#include "modules/perception/common/perception_gflags.h" +#include "modules/perception/lidar/common/lidar_timer.h" +#include "modules/perception/lidar/common/pcl_util.h" +#include "modules/perception/lidar/lib/detector/center_point_detection/params.h" + +namespace apollo { +namespace perception { +namespace lidar { + +using base::Object; +using base::PointD; +using base::PointF; + +// point cloud range +CenterPointDetection::CenterPointDetection() + : x_min_range_(Params::kMinXRange), + x_max_range_(Params::kMaxXRange), + y_min_range_(Params::kMinYRange), + y_max_range_(Params::kMaxYRange), + z_min_range_(Params::kMinZRange), + z_max_range_(Params::kMaxZRange) { + if (FLAGS_enable_ground_removal) { + z_min_range_ = + std::max(z_min_range_, static_cast(FLAGS_ground_removal_height)); + } +} + +bool CenterPointDetection::Init(const LidarDetectorInitOptions &options) { + /* + num_point_feature + */ + paddle::AnalysisConfig config; + config.EnableUseGpu(1000, FLAGS_gpu_id); + config.SetModel(FLAGS_center_point_model_file, + FLAGS_center_point_params_file); + if (FLAGS_use_trt) { + paddle::AnalysisConfig::Precision precision; + if (FLAGS_trt_precision == 0) { + precision = paddle_infer::PrecisionType::kFloat32; + } else if (FLAGS_trt_precision == 1) { + precision = paddle_infer::PrecisionType::kHalf; + } else { + AERROR << "Tensorrt type can only support 0 or 1, but recieved is" + << FLAGS_trt_precision << "\n"; + return false; + } + config.EnableTensorRtEngine(1 << 30, 1, 3, precision, FLAGS_trt_use_static, + false); + // todo: solve EnableTunedTensorRtDynamicShape + config.CollectShapeRangeInfo(FLAGS_dynamic_shape_file); + // config.EnableTunedTensorRtDynamicShape(FLAGS_dynamic_shape_file, true); + + if (FLAGS_trt_use_static) { + config.SetOptimCacheDir(FLAGS_trt_static_dir); + } + } + config.SwitchIrOptim(true); + + predictor_ = paddle_infer::CreatePredictor(config); + return true; +} + +bool CenterPointDetection::Detect(const LidarDetectorOptions &options, + LidarFrame *frame) { + // check input + if (frame == nullptr) { + AERROR << "Input null frame ptr."; + return false; + } + if (frame->cloud == nullptr) { + AERROR << "Input null frame cloud."; + return false; + } + if (frame->cloud->size() == 0) { + AERROR << "Input none points."; + return false; + } + + // record input cloud and lidar frame + original_cloud_ = frame->cloud; + original_world_cloud_ = frame->world_cloud; + lidar_frame_ref_ = frame; + + // check output + frame->segmented_objects.clear(); + + if (cudaSetDevice(FLAGS_gpu_id) != cudaSuccess) { + AERROR << "Failed to set device to gpu " << FLAGS_gpu_id; + return false; + } + + Timer timer; + + int num_points; + cur_cloud_ptr_ = std::shared_ptr( + new base::PointFCloud(*original_cloud_)); + + // down sample the point cloud through filtering beams + if (FLAGS_enable_downsample_beams) { + base::PointFCloudPtr downsample_beams_cloud_ptr(new base::PointFCloud()); + if (DownSamplePointCloudBeams(original_cloud_, downsample_beams_cloud_ptr, + FLAGS_downsample_beams_factor)) { + cur_cloud_ptr_ = downsample_beams_cloud_ptr; + } else { + AWARN << "Down-sample beams factor must be >= 1. Cancel down-sampling." + " Current factor: " + << FLAGS_downsample_beams_factor; + } + } + + // down sample the point cloud through filtering voxel grid + if (FLAGS_enable_downsample_pointcloud) { + pcl::PointCloud::Ptr pcl_cloud_ptr( + new pcl::PointCloud()); + pcl::PointCloud::Ptr filtered_cloud_ptr( + new pcl::PointCloud()); + TransformToPCLXYZI(*cur_cloud_ptr_, pcl_cloud_ptr); + DownSampleCloudByVoxelGrid( + pcl_cloud_ptr, filtered_cloud_ptr, FLAGS_downsample_voxel_size_x, + FLAGS_downsample_voxel_size_y, FLAGS_downsample_voxel_size_z); + + // transform pcl point cloud to apollo point cloud + base::PointFCloudPtr downsample_voxel_cloud_ptr(new base::PointFCloud()); + TransformFromPCLXYZI(filtered_cloud_ptr, downsample_voxel_cloud_ptr); + cur_cloud_ptr_ = downsample_voxel_cloud_ptr; + } + downsample_time_ = timer.toc(true); + + num_points = cur_cloud_ptr_->size(); + AINFO << "num points before fusing: " << num_points; + + // fuse clouds of preceding frames with current cloud + cur_cloud_ptr_->mutable_points_timestamp()->assign(cur_cloud_ptr_->size(), + 0.0); + if (FLAGS_enable_fuse_frames && FLAGS_num_fuse_frames > 1) { + // before fusing + while (!prev_world_clouds_.empty() && + frame->timestamp - prev_world_clouds_.front()->get_timestamp() > + FLAGS_fuse_time_interval) { + prev_world_clouds_.pop_front(); + } + // transform current cloud to world coordinate and save to a new ptr + base::PointDCloudPtr cur_world_cloud_ptr = + std::make_shared(); + for (size_t i = 0; i < cur_cloud_ptr_->size(); ++i) { + auto &pt = cur_cloud_ptr_->at(i); + Eigen::Vector3d trans_point(pt.x, pt.y, pt.z); + trans_point = lidar_frame_ref_->lidar2world_pose * trans_point; + PointD world_point; + world_point.x = trans_point(0); + world_point.y = trans_point(1); + world_point.z = trans_point(2); + world_point.intensity = pt.intensity; + cur_world_cloud_ptr->push_back(world_point); + } + cur_world_cloud_ptr->set_timestamp(frame->timestamp); + + // fusing clouds + for (auto &prev_world_cloud_ptr : prev_world_clouds_) { + num_points += prev_world_cloud_ptr->size(); + } + FuseCloud(cur_cloud_ptr_, prev_world_clouds_); + + // after fusing + while (static_cast(prev_world_clouds_.size()) >= + FLAGS_num_fuse_frames - 1) { + prev_world_clouds_.pop_front(); + } + prev_world_clouds_.emplace_back(cur_world_cloud_ptr); + } + fuse_time_ = timer.toc(true); + + // shuffle points and cut off + if (FLAGS_enable_shuffle_points) { + num_points = std::min(num_points, FLAGS_max_num_points); + std::vector point_indices = GenerateIndices(0, num_points, true); + base::PointFCloudPtr shuffle_cloud_ptr( + new base::PointFCloud(*cur_cloud_ptr_, point_indices)); + cur_cloud_ptr_ = shuffle_cloud_ptr; + } + shuffle_time_ = timer.toc(true); + + // points_array[x, y, z, i,timestampe, ......] + std::vector points_data(num_points * FLAGS_num_point_feature); + CloudToArray(cur_cloud_ptr_, points_data.data(), FLAGS_normalizing_factor); + cloud_to_array_time_ = timer.toc(true); + + // paddle inference + std::vector out_detections; + std::vector out_labels; + std::vector out_scores; + std::vector out_detections_final; + std::vector out_labels_final; + + DoInference(points_data, num_points, &out_detections, &out_labels, + &out_scores); + + FilterScore(&out_detections, &out_labels, &out_scores, FLAGS_score_threshold, + &out_detections_final, &out_labels_final); + + GetObjects(&frame->segmented_objects, frame->lidar2world_pose, + &out_detections_final, &out_labels_final); + inference_time_ = timer.toc(true); + + AINFO << "CenterPoint: " + << "\n" + << "down sample: " << downsample_time_ << "\t" + << "fuse: " << fuse_time_ << "\t" + << "shuffle: " << shuffle_time_ << "\t" + << "cloud_to_array: " << cloud_to_array_time_ << "\t" + << "inference: " << inference_time_ << "\t"; + return true; +} + +// normalizing_factor: Normalize intensity range to [0, 1] by this factor +void CenterPointDetection::CloudToArray(const base::PointFCloudPtr &pc_ptr, + float *out_points_array, + const float normalizing_factor) { + for (size_t i = 0; i < pc_ptr->size(); ++i) { + const auto &point = pc_ptr->at(i); + float x = point.x; + float y = point.y; + float z = point.z; + float intensity = point.intensity; + if (z < z_min_range_ || z > z_max_range_ || y < y_min_range_ || + y > y_max_range_ || x < x_min_range_ || x > x_max_range_) { + continue; + } + out_points_array[i * FLAGS_num_point_feature + 0] = x; + out_points_array[i * FLAGS_num_point_feature + 1] = y; + out_points_array[i * FLAGS_num_point_feature + 2] = z; + out_points_array[i * FLAGS_num_point_feature + 3] = + intensity / normalizing_factor; + // delta of timestamp between prev and cur frames + out_points_array[i * FLAGS_num_point_feature + 4] = + static_cast(pc_ptr->points_timestamp(i)); + } +} + +void CenterPointDetection::FuseCloud( + const base::PointFCloudPtr &out_cloud_ptr, + const std::deque &fuse_clouds) { + for (auto iter = fuse_clouds.rbegin(); iter != fuse_clouds.rend(); ++iter) { + double delta_t = lidar_frame_ref_->timestamp - (*iter)->get_timestamp(); + // transform prev world point cloud to current sensor's coordinates + for (size_t i = 0; i < (*iter)->size(); ++i) { + auto &point = (*iter)->at(i); + Eigen::Vector3d trans_point(point.x, point.y, point.z); + trans_point = lidar_frame_ref_->lidar2world_pose.inverse() * trans_point; + base::PointF pt; + pt.x = static_cast(trans_point(0)); + pt.y = static_cast(trans_point(1)); + pt.z = static_cast(trans_point(2)); + pt.intensity = static_cast(point.intensity); + // delta of time between current and prev frame + out_cloud_ptr->push_back(pt, delta_t); + } + } +} + +void CenterPointDetection::DoInference(const std::vector &points_data, + const int in_num_points, + std::vector *out_detections, + std::vector *out_labels, + std::vector *out_scores) { + // todo: check gpu_id + std::vector points_shape; + points_shape.push_back(in_num_points); + points_shape.push_back(FLAGS_num_point_feature); + + Run(predictor_.get(), points_shape, points_data, out_detections, out_labels, + out_scores); +} + +std::vector CenterPointDetection::GenerateIndices(int start_index, + int size, bool shuffle) { + // create a range number array + std::vector indices(size); + std::iota(indices.begin(), indices.end(), start_index); + + // shuffle the index array + if (shuffle) { + unsigned seed = 0; + std::shuffle(indices.begin(), indices.end(), + std::default_random_engine(seed)); + } + return indices; +} + +void CenterPointDetection::Run(paddle_infer::Predictor *predictor, + const std::vector &points_shape, + const std::vector &points_data, + std::vector *box3d_lidar, + std::vector *label_preds, + std::vector *scores) { + auto input_names = predictor->GetInputNames(); + for (const auto &tensor_name : input_names) { + auto in_tensor = predictor->GetInputHandle(tensor_name); + if (tensor_name == "data") { + in_tensor->Reshape(points_shape); + in_tensor->CopyFromCpu(points_data.data()); + } + } + ACHECK(predictor->Run()); + + auto output_names = predictor->GetOutputNames(); + for (size_t i = 0; i != output_names.size(); i++) { + auto output = predictor->GetOutputHandle(output_names[i]); + std::vector output_shape = output->shape(); + int out_num = std::accumulate(output_shape.begin(), output_shape.end(), 1, + std::multiplies()); + if (i == 0) { + box3d_lidar->resize(out_num); + output->CopyToCpu(box3d_lidar->data()); + } else if (i == 1) { + label_preds->resize(out_num); + output->CopyToCpu(label_preds->data()); + } else if (i == 2) { + scores->resize(out_num); + output->CopyToCpu(scores->data()); + } + } +} + +void CenterPointDetection::GetObjects( + std::vector> *objects, const Eigen::Affine3d &pose, + std::vector *detections, std::vector *labels) { + int num_objects = detections->size() / num_output_box_feature_; + + objects->clear(); + base::ObjectPool::Instance().BatchGet(num_objects, objects); + + for (int i = 0; i < num_objects; ++i) { + auto &object = objects->at(i); + object->id = i; + + // no velocity + float x = detections->at(i * FLAGS_num_output_box_feature + 0); + float y = detections->at(i * FLAGS_num_output_box_feature + 1); + float z = detections->at(i * FLAGS_num_output_box_feature + 2); + float dx = detections->at(i * FLAGS_num_output_box_feature + 3); + float dy = detections->at(i * FLAGS_num_output_box_feature + 4); + float dz = detections->at(i * FLAGS_num_output_box_feature + 5); + float yaw = detections->at(i * FLAGS_num_output_box_feature + 6); + // yaw += M_PI / 2; + yaw = std::atan2(sinf(yaw), cosf(yaw)); + yaw = -yaw; + + // directions + object->theta = yaw; + object->direction[0] = cosf(yaw); + object->direction[1] = sinf(yaw); + object->direction[2] = 0; + object->lidar_supplement.is_orientation_ready = true; + + // compute vertexes of bounding box and transform to world coordinate + object->lidar_supplement.num_points_in_roi = 8; + object->lidar_supplement.on_use = true; + object->lidar_supplement.is_background = false; + float roll = 0, pitch = 0; + Eigen::Quaternionf quater = + Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) * + Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); + Eigen::Translation3f translation(x, y, z); + Eigen::Affine3f affine3f = translation * quater.toRotationMatrix(); + for (float vx : std::vector{dx / 2, -dx / 2}) { + for (float vy : std::vector{dy / 2, -dy / 2}) { + for (float vz : std::vector{0, dz}) { + Eigen::Vector3f v3f(vx, vy, vz); + v3f = affine3f * v3f; + PointF point; + point.x = v3f.x(); + point.y = v3f.y(); + point.z = v3f.z(); + object->lidar_supplement.cloud.push_back(point); + + Eigen::Vector3d trans_point(point.x, point.y, point.z); + trans_point = pose * trans_point; + PointD world_point; + world_point.x = trans_point(0); + world_point.y = trans_point(1); + world_point.z = trans_point(2); + object->lidar_supplement.cloud_world.push_back(world_point); + } + } + } + + // classification + object->lidar_supplement.raw_probs.push_back(std::vector( + static_cast(base::ObjectType::MAX_OBJECT_TYPE), 0.f)); + object->lidar_supplement.raw_classification_methods.push_back(Name()); + object->sub_type = GetObjectSubType(labels->at(i)); + object->type = base::kSubType2TypeMap.at(object->sub_type); + object->lidar_supplement.raw_probs.back()[static_cast(object->type)] = + 1.0f; + // copy to type + object->type_probs.assign(object->lidar_supplement.raw_probs.back().begin(), + object->lidar_supplement.raw_probs.back().end()); + } +} + +base::ObjectSubType CenterPointDetection::GetObjectSubType(const int label) { + switch (label) { + case 0: + return base::ObjectSubType::CAR; + case 1: + return base::ObjectSubType::TRUCK; + case 3: + return base::ObjectSubType::BUS; + case 6: + return base::ObjectSubType::MOTORCYCLIST; + case 7: + return base::ObjectSubType::CYCLIST; + case 8: + return base::ObjectSubType::PEDESTRIAN; + case 9: + return base::ObjectSubType::TRAFFICCONE; + default: + return base::ObjectSubType::UNKNOWN; + } +} + +void CenterPointDetection::FilterScore( + const std::vector *box3d_lidar, + const std::vector *label_preds, const std::vector *scores, + const float score_threshold, std::vector *box3d_lidar_final, + std::vector *label_preds_final) { + for (size_t i = 0; i < scores->size(); i++) { + if (scores->at(i) > score_threshold) { + box3d_lidar_final->insert( + box3d_lidar_final->end(), + box3d_lidar->begin() + num_output_box_feature_ * i, + box3d_lidar->begin() + num_output_box_feature_ * (i + 1)); + label_preds_final->insert(label_preds_final->end(), + *(label_preds->begin() + i)); + } + } +} + +PERCEPTION_REGISTER_LIDARDETECTOR(CenterPointDetection); + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.h b/modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.h new file mode 100644 index 00000000000..e46f099de12 --- /dev/null +++ b/modules/perception/lidar/lib/detector/center_point_detection/center_point_detection.h @@ -0,0 +1,125 @@ +/****************************************************************************** + * Copyright 2022 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include + +#include "paddle/include/paddle_inference_api.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" + +#include "modules/perception/base/object.h" +#include "modules/perception/base/point_cloud.h" +#include "modules/perception/lidar/common/lidar_frame.h" +#include "modules/perception/lidar/lib/interface/base_lidar_detector.h" + +namespace apollo { +namespace perception { +namespace lidar { + +class CenterPointDetection : public BaseLidarDetector { + public: + CenterPointDetection(); + virtual ~CenterPointDetection() = default; + + bool Init(const LidarDetectorInitOptions &options = + LidarDetectorInitOptions()) override; + + bool Detect(const LidarDetectorOptions &options, LidarFrame *frame) override; + + std::string Name() const override { return "CenterPointDetection"; } + + private: + void CloudToArray(const base::PointFCloudPtr &pc_ptr, float *out_points_array, + float normalizing_factor); + + void FuseCloud(const base::PointFCloudPtr &out_cloud_ptr, + const std::deque &fuse_clouds); + + bool Preprocess(const float *in_points_array, const int in_num_points, + std::vector *voxels_shape, + std::vector *voxels_data, + std::vector *num_points_shape, + std::vector *num_points_data, + std::vector *coords_shape, + std::vector *coords_data); + + std::vector GenerateIndices(int start_index, int size, bool shuffle); + + void DoInference(const std::vector &points_data, + const int in_num_points, std::vector *out_detections, + std::vector *out_labels, + std::vector *out_scores); + + void Run(paddle_infer::Predictor *predictor, + const std::vector &points_shape, + const std::vector &points_data, + std::vector *box3d_lidar, std::vector *label_preds, + std::vector *scores); + + void GetObjects(std::vector> *objects, + const Eigen::Affine3d &pose, std::vector *detections, + std::vector *labels); + + void FilterScore(const std::vector *box3d_lidar, + const std::vector *label_preds, + const std::vector *scores, + const float score_threshold, + std::vector *box3d_lidar_final, + std::vector *label_preds_final); + + base::ObjectSubType GetObjectSubType(int label); + + // reference pointer of lidar frame + LidarFrame *lidar_frame_ref_ = nullptr; + std::shared_ptr> original_cloud_; + std::shared_ptr> + original_world_cloud_; + + std::deque prev_world_clouds_; + + base::PointFCloudPtr cur_cloud_ptr_; + + // point cloud range + float x_min_range_; + float x_max_range_; + float y_min_range_; + float y_max_range_; + float z_min_range_; + float z_max_range_; + + // time statistics + double downsample_time_ = 0.0; + double fuse_time_ = 0.0; + + double shuffle_time_ = 0.0; + double cloud_to_array_time_ = 0.0; + double inference_time_ = 0.0; + double collect_time_ = 0.0; + + // bounding_box + const int num_output_box_feature_ = 7; + + std::shared_ptr predictor_; +}; // class CenterPointDetection + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/modules/perception/lidar/lib/detector/center_point_detection/common.h b/modules/perception/lidar/lib/detector/center_point_detection/common.h new file mode 100644 index 00000000000..e3dad3c860d --- /dev/null +++ b/modules/perception/lidar/lib/detector/center_point_detection/common.h @@ -0,0 +1,72 @@ +/****************************************************************************** + * Copyright 2022 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file common.h + * @brief MACRO for CUDA codes + * @author Kosuke Murakami + * @date 2019/02/26 + */ + +#pragma once + +// headers in STL +#include + +// headers in CUDA +#include + +namespace apollo { +namespace perception { +namespace lidar { + +// using MACRO to allocate memory inside CUDA kernel +#define NUM_3D_BOX_CORNERS_MACRO 8 +#define NUM_2D_BOX_CORNERS_MACRO 4 +#define NUM_THREADS_MACRO 64 // need to be changed when num_threads_ is changed + +#define DIVUP(m, n) ((m) / (n) + ((m) % (n) > 0)) + +#define GPU_CHECK(ans) \ + { GPUAssert((ans), __FILE__, __LINE__); } +inline void GPUAssert(cudaError_t code, const char* file, int line, + bool abort = true) { + if (code != cudaSuccess) { + fprintf(stderr, "GPUassert: %s %s %d\n", cudaGetErrorString(code), file, + line); + if (abort) exit(code); + } +} + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/modules/perception/lidar/lib/detector/center_point_detection/params.h b/modules/perception/lidar/lib/detector/center_point_detection/params.h new file mode 100644 index 00000000000..ecfa3b8be8b --- /dev/null +++ b/modules/perception/lidar/lib/detector/center_point_detection/params.h @@ -0,0 +1,51 @@ +/****************************************************************************** + * Copyright 2022 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include +#include +namespace apollo { +namespace perception { +namespace lidar { + +class Params { + public: + static constexpr float kVoxelXSize = 0.2f; + static constexpr float kVoxelYSize = 0.2f; + static constexpr float kVoxelZSize = 8.0f; + static constexpr float kMinXRange = -51.2f; + static constexpr float kMinYRange = -51.2f; + static constexpr float kMinZRange = -5.0f; + static constexpr float kMaxXRange = 51.2f; + static constexpr float kMaxYRange = 51.2f; + static constexpr float kMaxZRange = 3.0f; + static constexpr int kNumClass = 3; + static constexpr int kMaxNumVoxels = 60000; + static constexpr int kMaxNumPointsPerVoxel = 20; + static constexpr int kNumPointFeature = 5; // x, y, z, i, delta of time + static constexpr int kNumOutputBoxFeature = 7; + static constexpr int kBatchSize = 1; + + private: + Params() = default; + ~Params() = default; +}; // class Params + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/third_party/centerpoint_infer_op/BUILD b/third_party/centerpoint_infer_op/BUILD index f3490d58ed0..67cb1e21da1 100644 --- a/third_party/centerpoint_infer_op/BUILD +++ b/third_party/centerpoint_infer_op/BUILD @@ -1,3 +1,3 @@ package( default_visibility = ["//visibility:public"], -) \ No newline at end of file +) diff --git a/third_party/paddleinference/BUILD b/third_party/paddleinference/BUILD index f3490d58ed0..67cb1e21da1 100644 --- a/third_party/paddleinference/BUILD +++ b/third_party/paddleinference/BUILD @@ -1,3 +1,3 @@ package( default_visibility = ["//visibility:public"], -) \ No newline at end of file +)