Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(pointcloud preprocessor): fix ring outlier filter and concatenation to use PointXYZIRC #6870

Closed
wants to merge 4 commits into from
Closed

fix(pointcloud preprocessor): fix ring outlier filter and concatenation to use PointXYZIRC #6870

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0328e34
add PointXYZIRC
a-maumau Apr 23, 2024
33341bb
fix RingOutlierFilter and ConcatenateDataSynchronizer to use PointXYZIRC
a-maumau Apr 23, 2024
501a124
style(pre-commit): autofix
pre-commit-ci[bot] Apr 23, 2024
83e6d64
fix intensity of PointXYZIRC from float to uint8
a-maumau May 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions common/autoware_point_types/include/autoware_point_types/types.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,29 @@ enum ReturnType : uint8_t {
DUAL_ONLY,
};

struct PointXYZIRC
{
float x{0.0F};
float y{0.0F};
float z{0.0F};
float intensity{0.0F};
union { // for memory alignment
uint16_t _data;
struct
{
uint8_t padding{0U};
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

for all reviewers, is this padding OK to be the field name?

uint8_t return_type{0U};
};
};
uint16_t channel{0U};
friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept
{
return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
float_eq<float>(p1.z, p2.z) && float_eq<float>(p1.intensity, p2.intensity) &&
p1.return_type == p2.return_type && p1.channel == p2.channel;
}
};

struct PointXYZIRADRT
{
float x{0.0F};
Expand All @@ -78,10 +101,16 @@ struct PointXYZIRADRT
enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp };

LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp);

using PointXYZIRCGenerator = std::tuple<
point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
field_return_type_generator, field_channel_generator>;

using PointXYZIRADRTGenerator = std::tuple<
point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
Expand All @@ -90,6 +119,12 @@ using PointXYZIRADRTGenerator = std::tuple<

} // namespace autoware_point_types

POINT_CLOUD_REGISTER_POINT_STRUCT(
autoware_point_types::PointXYZIRC,
(float, x,
x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint8_t, padding, padding)(
std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel))

POINT_CLOUD_REGISTER_POINT_STRUCT(
autoware_point_types::PointXYZIRADRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
Expand Down
1 change: 1 addition & 0 deletions ...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,7 @@
namespace pointcloud_preprocessor
{
using autoware_point_types::PointXYZI;
using autoware_point_types::PointXYZIRC;
using point_cloud_msg_wrapper::PointCloud2Modifier;

/** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data
Expand Down
9 changes: 6 additions & 3 deletions ...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,8 @@

namespace pointcloud_preprocessor
{
using autoware_point_types::PointXYZI;
using autoware_point_types::PointXYZIRADRT;
using autoware_point_types::PointXYZIRC;
using point_cloud_msg_wrapper::PointCloud2Modifier;

class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
Expand Down Expand Up @@ -64,8 +65,10 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
{
if (walk_size > num_points_threshold_) return true;

auto first_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.first]);
auto last_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.second]);
auto first_point =
reinterpret_cast<const PointXYZIRADRT *>(&input->data[data_idx_both_ends.first]);
auto last_point =
reinterpret_cast<const PointXYZIRADRT *>(&input->data[data_idx_both_ends.second]);

const auto x = first_point->x - last_point->x;
const auto y = first_point->y - last_point->y;
Expand Down
16 changes: 10 additions & 6 deletions sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc.

Check notice on line 1 in sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 6.08 to 6.17, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -420,7 +420,15 @@
not_subscribed_topic_names_.insert(e.first);
}
}

// after concatenation, channels have no consistent meaning
size_t channel_offset = offsetof(PointXYZIRC, channel);
for (size_t data_idx = 0; data_idx < concat_cloud_ptr->data.size();
data_idx += concat_cloud_ptr->point_step) {
concat_cloud_ptr->data[data_idx + channel_offset] = 0;
}
concat_cloud_ptr->header.stamp = oldest_stamp;

Check warning on line 431 in sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

PointCloudConcatenateDataSynchronizerComponent::combineClouds increases in cyclomatic complexity from 14 to 15, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
return transformed_clouds;
}

Expand Down Expand Up @@ -545,14 +553,10 @@
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name)
{
std::lock_guard<std::mutex> lock(mutex_);
sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
auto input = std::make_shared<sensor_msgs::msg::PointCloud2>(*input_ptr);
if (input->data.empty()) {
RCLCPP_WARN_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
} else {
// convert to XYZI pointcloud if pointcloud is not empty
convertToXYZICloud(input, xyzi_input_ptr);
}

const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr);
Expand All @@ -561,7 +565,7 @@
[](const auto & e) { return e.second != nullptr; });

if (is_already_subscribed_this) {
cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr;
cloud_stdmap_tmp_[topic_name] = input;

if (!is_already_subscribed_tmp) {
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
Expand All @@ -574,7 +578,7 @@
timer_->reset();
}
} else {
cloud_stdmap_[topic_name] = xyzi_input_ptr;
cloud_stdmap_[topic_name] = input;

const bool is_subscribed_all = std::all_of(
std::begin(cloud_stdmap_), std::end(cloud_stdmap_),
Expand Down
71 changes: 54 additions & 17 deletions sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,173 +69,208 @@
}
stop_watch_ptr_->toc("processing_time", true);

output.point_step = sizeof(PointXYZI);
output.point_step = sizeof(PointXYZIRC);
output.data.resize(output.point_step * input->width);
size_t output_size = 0;

// Set up the noise points cloud, if noise points are to be published.
PointCloud2 outlier_points;
size_t outlier_points_size = 0;
if (publish_outlier_pointcloud_) {
outlier_points.point_step = sizeof(PointXYZI);
outlier_points.point_step = sizeof(PointXYZIRC);
outlier_points.data.resize(outlier_points.point_step * input->width);
}

const auto ring_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
const auto azimuth_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Azimuth)).offset;
const auto distance_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Distance)).offset;
const auto intensity_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Intensity)).offset;
const auto return_type_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::ReturnType)).offset;

std::vector<std::vector<size_t>> ring2indices;
ring2indices.reserve(max_rings_num_);

for (uint16_t i = 0; i < max_rings_num_; i++) {
ring2indices.push_back(std::vector<size_t>());
ring2indices.back().reserve(max_points_num_per_ring_);
}

for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) {
const uint16_t ring = *reinterpret_cast<const uint16_t *>(&input->data[data_idx + ring_offset]);
ring2indices[ring].push_back(data_idx);
}

// walk range: [walk_first_idx, walk_last_idx]
int walk_first_idx = 0;
int walk_last_idx = -1;

for (const auto & indices : ring2indices) {
if (indices.size() < 2) continue;

walk_first_idx = 0;
walk_last_idx = -1;

for (size_t idx = 0U; idx < indices.size() - 1; ++idx) {
const size_t & current_data_idx = indices[idx];
const size_t & next_data_idx = indices[idx + 1];
walk_last_idx = idx;

// if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08)

const float & current_azimuth =
*reinterpret_cast<const float *>(&input->data[current_data_idx + azimuth_offset]);
const float & next_azimuth =
*reinterpret_cast<const float *>(&input->data[next_data_idx + azimuth_offset]);
float azimuth_diff = next_azimuth - current_azimuth;
azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff;

const float & current_distance =
*reinterpret_cast<const float *>(&input->data[current_data_idx + distance_offset]);
const float & next_distance =
*reinterpret_cast<const float *>(&input->data[next_data_idx + distance_offset]);

if (
std::max(current_distance, next_distance) <
std::min(current_distance, next_distance) * distance_ratio_ &&
azimuth_diff < 100.f) {
continue; // Determined to be included in the same walk
}

if (isCluster(
input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]),
walk_last_idx - walk_first_idx + 1)) {
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
auto output_ptr = reinterpret_cast<PointXYZI *>(&output.data[output_size]);
auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
auto output_ptr = reinterpret_cast<PointXYZIRC *>(&output.data[output_size]);
auto input_ptr = reinterpret_cast<const PointXYZIRADRT *>(&input->data[indices[i]]);

if (transform_info.need_transform) {
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
p = transform_info.eigen_transform * p;
output_ptr->x = p[0];
output_ptr->y = p[1];
output_ptr->z = p[2];
} else {
*output_ptr = *input_ptr;
output_ptr->x = input_ptr->x;
output_ptr->y = input_ptr->y;
output_ptr->z = input_ptr->z;
}
const float & intensity =
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
output_ptr->intensity = intensity;
const uint8_t & return_type =
*reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
output_ptr->return_type = return_type;
const uint16_t & channel =
*reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
output_ptr->channel = channel;

output_size += output.point_step;
}
} else if (publish_outlier_pointcloud_) {
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
auto outlier_ptr =
reinterpret_cast<PointXYZI *>(&outlier_points.data[outlier_points_size]);
reinterpret_cast<PointXYZIRC *>(&outlier_points.data[outlier_points_size]);
auto input_ptr =
reinterpret_cast<const PointXYZI *>(&input->data[indices[walk_first_idx]]);
reinterpret_cast<const PointXYZIRADRT *>(&input->data[indices[walk_first_idx]]);
if (transform_info.need_transform) {
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
p = transform_info.eigen_transform * p;
outlier_ptr->x = p[0];
outlier_ptr->y = p[1];
outlier_ptr->z = p[2];
} else {
*outlier_ptr = *input_ptr;
outlier_ptr->x = input_ptr->x;
outlier_ptr->y = input_ptr->y;
outlier_ptr->z = input_ptr->z;
}
const float & intensity = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + intensity_offset]);
outlier_ptr->intensity = intensity;
const uint8_t & return_type = *reinterpret_cast<const uint8_t *>(
&input->data[indices[walk_first_idx] + return_type_offset]);
outlier_ptr->return_type = return_type;
const uint16_t & channel = *reinterpret_cast<const uint16_t *>(
&input->data[indices[walk_first_idx] + ring_offset]);
outlier_ptr->channel = channel;

outlier_points_size += outlier_points.point_step;
}
}

walk_first_idx = idx + 1;
}

if (walk_first_idx > walk_last_idx) continue;

if (isCluster(
input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]),
walk_last_idx - walk_first_idx + 1)) {
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
auto output_ptr = reinterpret_cast<PointXYZI *>(&output.data[output_size]);
auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
auto output_ptr = reinterpret_cast<PointXYZIRC *>(&output.data[output_size]);
auto input_ptr = reinterpret_cast<const PointXYZIRADRT *>(&input->data[indices[i]]);

if (transform_info.need_transform) {
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
p = transform_info.eigen_transform * p;
output_ptr->x = p[0];
output_ptr->y = p[1];
output_ptr->z = p[2];
} else {
*output_ptr = *input_ptr;
output_ptr->x = input_ptr->x;
output_ptr->y = input_ptr->y;
output_ptr->z = input_ptr->z;
}
const float & intensity =
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
output_ptr->intensity = intensity;
const uint8_t & return_type =
*reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
output_ptr->return_type = return_type;
const uint16_t & channel =
*reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
output_ptr->channel = channel;

output_size += output.point_step;
}
} else if (publish_outlier_pointcloud_) {
for (int i = walk_first_idx; i < walk_last_idx; i++) {
auto outlier_ptr = reinterpret_cast<PointXYZI *>(&outlier_points.data[outlier_points_size]);
auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
auto outlier_ptr =
reinterpret_cast<PointXYZIRC *>(&outlier_points.data[outlier_points_size]);
auto input_ptr = reinterpret_cast<const PointXYZIRADRT *>(&input->data[indices[i]]);
if (transform_info.need_transform) {
Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1);
p = transform_info.eigen_transform * p;
outlier_ptr->x = p[0];
outlier_ptr->y = p[1];
outlier_ptr->z = p[2];
} else {
*outlier_ptr = *input_ptr;
outlier_ptr->x = input_ptr->x;
outlier_ptr->y = input_ptr->y;
outlier_ptr->z = input_ptr->z;
}
const float & intensity =
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
outlier_ptr->intensity = intensity;
const uint8_t & return_type =
*reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
outlier_ptr->return_type = return_type;
const uint16_t & channel =
*reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
outlier_ptr->channel = channel;
outlier_points_size += outlier_points.point_step;
}
}
}

setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4);
setUpPointCloudFormat(input, output, output_size, /*num_fields=*/7);

if (publish_outlier_pointcloud_) {
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/7);

Check warning on line 273 in sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

RingOutlierFilterComponent::faster_filter already has high cyclomatic complexity, and now it increases in Lines of Code from 166 to 201. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
outlier_pointcloud_publisher_->publish(outlier_points);
}

Expand Down Expand Up @@ -316,7 +351,9 @@
pcd_modifier.setPointCloud2Fields(
num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32, "padding", 1,
sensor_msgs::msg::PointField::UINT8, "return_type", 1, sensor_msgs::msg::PointField::UINT8,
"channel", 1, sensor_msgs::msg::PointField::UINT16);
}

} // namespace pointcloud_preprocessor
Expand Down
Loading