Skip to content

Commit

Permalink
fix(compare_map_segmentation): add intensity field (autowarefoundatio…
Browse files Browse the repository at this point in the history 
…n#6807)

* fix(comare_map_segmentation): add intensity field

Signed-off-by: badai-nguyen <[email protected]>

* fix: add intensity other compare map filter

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: badai-nguyen <[email protected]>
  • Loading branch information
@badai-nguyen
badai-nguyen committed Jun 18, 2024
1 parent 281b7fc commit d8c6ae4
Show file tree
Hide file tree
Showing 4 changed files with 91 additions and 36 deletions.
33 changes: 23 additions & 10 deletions perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,20 +137,33 @@ void DistanceBasedCompareMapFilterComponent::filter(
std::scoped_lock lock(mutex_);
stop_watch_ptr_->toc("processing_time", true);

pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
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_) {
Expand Down
32 changes: 23 additions & 9 deletions ...ption/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,19 +106,33 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter(
{
std::scoped_lock lock(mutex_);
stop_watch_ptr_->toc("processing_time", true);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
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_) {
Expand Down
30 changes: 22 additions & 8 deletions perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,19 +67,33 @@ void VoxelBasedCompareMapFilterComponent::filter(
{
std::scoped_lock lock(mutex_);
stop_watch_ptr_->toc("processing_time", true);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
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_) {
Expand Down
32 changes: 23 additions & 9 deletions perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,20 +135,34 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter(
{
std::scoped_lock lock(mutex_);
stop_watch_ptr_->toc("processing_time", true);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
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_) {
Expand Down

0 comments on commit d8c6ae4

Please sign in to comment.