From d8c6ae49caa61f3335d8ab13187c2ac114db8b2c Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 16 Apr 2024 09:52:10 +0900 Subject: [PATCH] fix(compare_map_segmentation): add intensity field (#6807) * fix(comare_map_segmentation): add intensity field Signed-off-by: badai-nguyen * fix: add intensity other compare map filter Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...tance_based_compare_map_filter_nodelet.cpp | 33 +++++++++++++------ ...approximate_compare_map_filter_nodelet.cpp | 32 +++++++++++++----- ...voxel_based_compare_map_filter_nodelet.cpp | 30 ++++++++++++----- ...tance_based_compare_map_filter_nodelet.cpp | 32 +++++++++++++----- 4 files changed, 91 insertions(+), 36 deletions(-) diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index 519afa4145bcf..0f870d37c0de1 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -137,20 +137,33 @@ void DistanceBasedCompareMapFilterComponent::filter( std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - if (distance_based_map_loader_->is_close_to_map(pcl_input->points.at(i), distance_threshold_)) { + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); + if (distance_based_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(pcl_input->points.at(i)); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 34a0f17bdb389..7ad479a6e74bd 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -106,19 +106,33 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - if (voxel_based_approximate_map_loader_->is_close_to_map( - pcl_input->points.at(i), distance_threshold_)) { + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); + if (voxel_based_approximate_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(pcl_input->points.at(i)); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 82c3576dd951c..595336145dabd 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -67,19 +67,33 @@ void VoxelBasedCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - const pcl::PointXYZ point = pcl_input->points.at(i); + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); if (voxel_grid_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(point); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 32e5367fbcc38..f169e2069aee7 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -135,20 +135,34 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - if (voxel_distance_based_map_loader_->is_close_to_map( - pcl_input->points.at(i), distance_threshold_)) { + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); + if (voxel_distance_based_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(pcl_input->points.at(i)); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) {