diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 2d983dc47545c..6e3883346b4a4 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -224,12 +224,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 8c0ba744491e7..906ad71d21732 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -124,7 +124,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -132,7 +132,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "not found ${FILE_NAME}") message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -145,8 +145,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) endfunction() # default model - download(pts_voxel_encoder_pointpainting.onnx e674271096f6fbbe0f808eef87f5a5ab) - download(pts_backbone_neck_head_pointpainting.onnx d4527a4da08959c2bf9c997112a1de35) + download(pts_voxel_encoder_pointpainting.onnx 25c70f76a45a64944ccd19f604c99410) + download(pts_backbone_neck_head_pointpainting.onnx 2c7108245240fbd7bf0104dd68225868) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 56346b5a3eb41..e1be5426cba4b 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,15 +1,16 @@ /**: ros__parameters: class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] - point_feature_size: 9 # x, y, z, time-lag and car, pedestrian, bicycle + paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle max_voxel_size: 40000 - point_cloud_range: [-102.4, -102.4, -4.0, 102.4, 102.4, 6.0] - voxel_size: [0.32, 0.32, 10.0] + point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + voxel_size: [0.32, 0.32, 8.0] downsample_factor: 1 - encoder_in_feature_size: 14 + encoder_in_feature_size: 12 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # post-process params - circle_nms_dist_threshold: 0.5 + circle_nms_dist_threshold: 0.3 iou_nms_target_class_names: ["CAR"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 diff --git a/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml new file mode 100755 index 0000000000000..bd49dc65749a9 --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + # UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + passthrough_lower_bound_probability_thresholds: [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.50] + trust_distances: [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + min_iou_threshold: 0.5 + use_roi_probability: false + roi_probability_threshold: 0.5 + + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg + [1, 0, 0, 0, 0, 0, 0, 0, # UNKNOWN <-detected_objects + 0, 1, 1, 1, 1, 0, 0, 0, # CAR + 0, 1, 1, 1, 1, 0, 0, 0, # TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, # BUS + 0, 1, 1, 1, 1, 0, 0, 0, # TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, # MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, # BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] # PEDESTRIAN diff --git a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md index accdc350bf256..4e054fc911dc6 100644 --- a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md @@ -41,14 +41,16 @@ The DetectedObject has three possible shape choices/implementations, where the p ### Core Parameters -| Name | Type | Description | -| ----------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | -| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | -| `passthrough_lower_bound_probability_threshold` | double | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | -| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | -| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | +| Name | Type | Description | +| ------------------------------------------------ | -------------- | -------------------------------------------------------------------------------------------------------------------------- | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | +| `passthrough_lower_bound_probability_thresholds` | vector[double] | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | +| `trust_distances` | vector[double] | If the distance of a detected object from the origin of frame_id is greater than the threshold, it is published in output. | +| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | +| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | +| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | +| `can_assign_matrix` | vector[int] | association matrix between rois and detected_objects to check that two rois on images can be match | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 8168ad2c9f265..e11a62c060480 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -18,6 +18,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" + #include #include #include @@ -40,15 +42,14 @@ class RoiDetectedObjectFusionNode : public FusionNode generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection); + std::map generateDetectedObjectRoIs( + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); void fuseObjectsOnImage( - const std::vector & objects, + const DetectedObjects & input_object_msg, const std::vector & image_rois, - const std::map & object_roi_map); + const std::map & object_roi_map); void publish(const DetectedObjects & output_msg) override; @@ -57,13 +58,16 @@ class RoiDetectedObjectFusionNode : public FusionNode passthrough_lower_bound_probability_thresholds{}; + std::vector trust_distances{}; double min_iou_threshold{}; bool use_roi_probability{}; double roi_probability_threshold{}; + Eigen::MatrixXi can_assign_matrix; } fusion_params_; - std::vector passthrough_object_flags_, fused_object_flags_, ignored_object_flags_; + std::map> passthrough_object_flags_map_, fused_object_flags_map_, + ignored_object_flags_map_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 0e18e9ee42a47..e6275a2ee83c7 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -41,7 +41,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index dd71ca462f666..b6165fc7c87d2 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -18,6 +18,7 @@ + @@ -39,13 +40,9 @@ + - - - - - diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 083a40044489b..32d7a5633b811 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -17,6 +17,7 @@ image_transport lidar_centerpoint message_filters + object_recognition_utils pcl_conversions pcl_ros rclcpp diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 01ec679a82efc..5253ac192c786 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -29,6 +30,18 @@ #include +namespace +{ + +Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + namespace image_projection_based_fusion { @@ -102,8 +115,12 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); class_names_ = this->declare_parameter>("class_names"); + const auto paint_class_names = + this->declare_parameter>("paint_class_names"); std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; - if (std::find(class_names_.begin(), class_names_.end(), "TRUCK") != class_names_.end()) { + if ( + std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") != + paint_class_names.end()) { isClassTable_["CAR"] = std::bind(&isCar, std::placeholders::_1); } else { isClassTable_["CAR"] = std::bind(&isVehicle, std::placeholders::_1); @@ -113,9 +130,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_["BICYCLE"] = std::bind(&isBicycle, std::placeholders::_1); isClassTable_["PEDESTRIAN"] = std::bind(&isPedestrian, std::placeholders::_1); for (const auto & cls : classes_) { - auto it = find(class_names_.begin(), class_names_.end(), cls); - if (it != class_names_.end()) { - int index = it - class_names_.begin(); + auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); + if (it != paint_class_names.end()) { + int index = it - paint_class_names.begin(); class_index_[cls] = index + 1; } else { isClassTable_.erase(cls); @@ -253,39 +270,38 @@ void PointPaintingFusionNode::fuseOnSingleImage( std::vector debug_image_points; // get transform from cluster frame id to camera optical frame id - geometry_msgs::msg::TransformStamped transform_stamped; + // geometry_msgs::msg::TransformStamped transform_stamped; + Eigen::Affine3f lidar2cam_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ painted_pointcloud_msg.header.frame_id, camera_info.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ painted_pointcloud_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } - transform_stamped = transform_stamped_optional.value(); + lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } // transform - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - - std::chrono::system_clock::time_point start, end; - start = std::chrono::system_clock::now(); + // sensor_msgs::msg::PointCloud2 transformed_pointcloud; + // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); const auto x_offset = - transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::X)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::X)) .offset; const auto y_offset = - transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Y)) .offset; const auto z_offset = - transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Z)) .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; - const auto p_step = transformed_pointcloud.point_step; + const auto p_step = painted_pointcloud_msg.point_step; // projection matrix Eigen::Matrix3f camera_projection; // use only x,y,z camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6); + Eigen::Vector3f point_lidar, point_camera; /** dc : don't care x | f x1 x2 dc ||xc| @@ -295,18 +311,24 @@ dc | dc dc dc dc ||zc| **/ auto objects = input_roi_msg.feature_objects; - int iterations = transformed_pointcloud.data.size() / transformed_pointcloud.point_step; + int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step; // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); #pragma omp parallel for for (int i = 0; i < iterations; i++) { int stride = p_step * i; - unsigned char * data = &transformed_pointcloud.data[0]; + unsigned char * data = &painted_pointcloud_msg.data[0]; unsigned char * output = &painted_pointcloud_msg.data[0]; float p_x = *reinterpret_cast(&data[stride + x_offset]); float p_y = *reinterpret_cast(&data[stride + y_offset]); float p_z = *reinterpret_cast(&data[stride + z_offset]); + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { continue; } diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index d9dce27fb0ecc..9789f52de5f74 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -37,8 +37,8 @@ namespace { const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_voxel_size_ in config const std::size_t WARPS_PER_BLOCK = 4; -const std::size_t ENCODER_IN_FEATURE_SIZE = 14; // same as encoder_in_feature_size_ in config.hpp -const int POINT_FEATURE_SIZE = 9; +const std::size_t ENCODER_IN_FEATURE_SIZE = 12; // same as encoder_in_feature_size_ in config.hpp +const int POINT_FEATURE_SIZE = 7; // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 37637f99f69c9..333ec7d7ed206 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -14,6 +14,8 @@ #include "image_projection_based_fusion/roi_detected_object_fusion/node.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" + #include #include @@ -25,28 +27,47 @@ namespace image_projection_based_fusion RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_detected_object_fusion", options) { - fusion_params_.passthrough_lower_bound_probability_threshold = - declare_parameter("passthrough_lower_bound_probability_threshold"); + fusion_params_.passthrough_lower_bound_probability_thresholds = + declare_parameter>("passthrough_lower_bound_probability_thresholds"); + fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); + fusion_params_.trust_distances = declare_parameter>("trust_distances"); fusion_params_.use_roi_probability = declare_parameter("use_roi_probability"); fusion_params_.roi_probability_threshold = declare_parameter("roi_probability_threshold"); - fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); + { + const auto can_assign_vector_tmp = declare_parameter>("can_assign_matrix"); + std::vector can_assign_vector(can_assign_vector_tmp.begin(), can_assign_vector_tmp.end()); + const int label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), label_num, label_num); + fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); + } } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { - passthrough_object_flags_.clear(); - passthrough_object_flags_.resize(output_msg.objects.size()); - fused_object_flags_.clear(); - fused_object_flags_.resize(output_msg.objects.size()); - ignored_object_flags_.clear(); - ignored_object_flags_.resize(output_msg.objects.size()); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; + passthrough_object_flags.resize(output_msg.objects.size()); + fused_object_flags.resize(output_msg.objects.size()); + ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - if ( - output_msg.objects.at(obj_i).existence_probability > - fusion_params_.passthrough_lower_bound_probability_threshold) { - passthrough_object_flags_.at(obj_i) = true; + const auto & object = output_msg.objects.at(obj_i); + const auto label = object_recognition_utils::getHighestProbLabel(object.classification); + const auto pos = object_recognition_utils::getPose(object).position; + const auto object_sqr_dist = pos.x * pos.x + pos.y * pos.y; + const auto prob_threshold = + fusion_params_.passthrough_lower_bound_probability_thresholds.at(label); + const auto trust_sqr_dist = + fusion_params_.trust_distances.at(label) * fusion_params_.trust_distances.at(label); + if (object.existence_probability > prob_threshold || object_sqr_dist > trust_sqr_dist) { + passthrough_object_flags.at(obj_i) = true; } } + + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + passthrough_object_flags_map_.insert(std::make_pair(timestamp_nsec, passthrough_object_flags)); + fused_object_flags_map_.insert(std::make_pair(timestamp_nsec, fused_object_flags)); + ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags)); } void RoiDetectedObjectFusionNode::fuseOnSingleImage( @@ -58,8 +79,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } @@ -73,9 +94,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( camera_info.p.at(11); const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg.objects, static_cast(camera_info.width), + input_object_msg, static_cast(camera_info.width), static_cast(camera_info.height), object2camera_affine, camera_projection); - fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map); + fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); @@ -86,26 +107,35 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } } -std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection) +std::map +RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) { - std::map object_roi_map; - for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { + std::map object_roi_map; + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (passthrough_object_flags_map_.size() == 0) { + return object_roi_map; + } + if (passthrough_object_flags_map_.count(timestamp_nsec) == 0) { + return object_roi_map; + } + const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; - { - const auto & object = input_objects.at(obj_i); + const auto & object = input_object_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { - continue; - } + if (passthrough_object_flags.at(obj_i)) { + continue; + } - // filter point out of scope - if (debugger_ && out_of_scope(object)) { - continue; - } + // filter point out of scope + if (debugger_ && out_of_scope(object)) { + continue; + } + { std::vector vertices; objectToVertices(object.kinematics.pose_with_covariance.pose, object.shape, vertices); transformPoints(vertices, object2camera_affine, vertices_camera_coord); @@ -142,7 +172,7 @@ std::map RoiDetectedObjectFusionNode::generateDet } } } - if (point_on_image_cnt == 0) { + if (point_on_image_cnt < 3) { continue; } @@ -151,16 +181,18 @@ std::map RoiDetectedObjectFusionNode::generateDet max_x = std::min(max_x, image_width - 1); max_y = std::min(max_y, image_height - 1); - // build roi - RegionOfInterest roi; - roi.x_offset = static_cast(min_x); - roi.y_offset = static_cast(min_y); - roi.width = static_cast(max_x) - static_cast(min_x); - roi.height = static_cast(max_y) - static_cast(min_y); - object_roi_map.insert(std::make_pair(obj_i, roi)); + DetectedObjectWithFeature object_roi; + object_roi.feature.roi.x_offset = static_cast(min_x); + object_roi.feature.roi.y_offset = static_cast(min_y); + object_roi.feature.roi.width = + static_cast(max_x) - static_cast(min_x); + object_roi.feature.roi.height = + static_cast(max_y) - static_cast(min_y); + object_roi.object = object; + object_roi_map.insert(std::make_pair(obj_i, object_roi)); if (debugger_) { - debugger_->obstacle_rois_.push_back(roi); + debugger_->obstacle_rois_.push_back(object_roi.feature.roi); } } @@ -168,13 +200,26 @@ std::map RoiDetectedObjectFusionNode::generateDet } void RoiDetectedObjectFusionNode::fuseObjectsOnImage( - const std::vector & objects __attribute__((unused)), + const DetectedObjects & input_object_msg, const std::vector & image_rois, - const std::map & object_roi_map) + const std::map & object_roi_map) { + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { + return; + } + if ( + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + for (const auto & object_pair : object_roi_map) { const auto & obj_i = object_pair.first; - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { continue; } @@ -182,7 +227,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( float max_iou = 0.0f; for (const auto & image_roi : image_rois) { const auto & object_roi = object_pair.second; - const double iou = calcIoU(object_roi, image_roi.feature.roi); + const auto object_roi_label = + object_recognition_utils::getHighestProbLabel(object_roi.object.classification); + const auto image_roi_label = + object_recognition_utils::getHighestProbLabel(image_roi.object.classification); + if (!fusion_params_.can_assign_matrix(object_roi_label, image_roi_label)) { + continue; + } + + const double iou = calcIoU(object_roi.feature.roi, image_roi.feature.roi); if (iou > max_iou) { max_iou = iou; roi_prob = image_roi.object.existence_probability; @@ -192,15 +245,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( if (max_iou > fusion_params_.min_iou_threshold) { if (fusion_params_.use_roi_probability) { if (roi_prob > fusion_params_.roi_probability_threshold) { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } else { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } } @@ -234,19 +287,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) return; } + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + if ( + passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || + ignored_object_flags_map_.size() == 0) { + return; + } + if ( + passthrough_object_flags_map_.count(timestamp_nsec) == 0 || + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; output_objects_msg.header = output_msg.header; debug_fused_objects_msg.header = output_msg.header; debug_ignored_objects_msg.header = output_msg.header; for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); } - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); - } else if (ignored_object_flags_.at(obj_i)) { + } + if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } @@ -255,6 +326,10 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + + passthrough_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); + ignored_object_flags_map_.erase(timestamp_nsec); } } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 1eaf04562b2b8..670c5596001fb 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -24,7 +24,7 @@ std::optional getTransformStamped( try { geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tf_buffer.lookupTransform( - target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.01)); return transform_stamped; } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(rclcpp::get_logger("image_projection_based_fusion"), ex.what());