From 9a24f2ca035d634c97d0cfa161b1f8a3f36228da Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 6 Apr 2023 09:57:41 +0900 Subject: [PATCH] perf(ring_outlier_filter): an attempt of cache friendly impl Signed-off-by: Vincent Richard style(pre-commit): autofix fix(ring_outlier_filter): cleanup code with ranges Signed-off-by: Vincent Richard [breaking] fix autoware point type padding for faster memory access can memcpy between input data buffer and PointXYZI* make assumption on memory layout for faster data fetch misc optimization (reordering, constexpr, etc) Signed-off-by: Vincent Richard style(pre-commit): autofix comment limitations Signed-off-by: Vincent Richard feat(ring_outlier_filter): add accurate isCluster impl Signed-off-by: Vincent Richard style(pre-commit): autofix fix #3218 Signed-off-by: Vincent Richard cleaning Signed-off-by: Vincent Richard style(pre-commit): autofix --- .../include/autoware_point_types/types.hpp | 2 + .../docs/ring-outlier-filter.md | 13 +- .../ring_outlier_filter_nodelet.hpp | 16 - sensing/pointcloud_preprocessor/package.xml | 1 + .../ring_outlier_filter_nodelet.cpp | 397 +++++++++++++----- 5 files changed, 310 insertions(+), 119 deletions(-) diff --git a/common/autoware_point_types/include/autoware_point_types/types.hpp b/common/autoware_point_types/include/autoware_point_types/types.hpp index 71ea80c5a9733..738493ba41219 100644 --- a/common/autoware_point_types/include/autoware_point_types/types.hpp +++ b/common/autoware_point_types/include/autoware_point_types/types.hpp @@ -35,6 +35,7 @@ struct PointXYZI float x{0.0F}; float y{0.0F}; float z{0.0F}; + float _padding{0}; // compatibility with PCL::PointXYZ* float intensity{0.0F}; friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept { @@ -59,6 +60,7 @@ struct PointXYZIRADRT float x{0.0F}; float y{0.0F}; float z{0.0F}; + float _padding{0}; // compatibility with PCL::PointXYZ* float intensity{0.0F}; uint16_t ring{0U}; float azimuth{0.0F}; diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 42ca694235a00..eedc8e6bcb573 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,13 +22,12 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index a906eb913d863..36e5c9d85aafd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -45,7 +45,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; - size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -53,21 +52,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - bool isCluster( - const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) - { - if (walk_size > num_points_threshold_) return true; - - auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = reinterpret_cast(&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; - const auto z = first_point->z - last_point->z; - - return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; - } - public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 4b406c55251e2..43869c8ca1232 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -32,6 +32,7 @@ pcl_msgs pcl_ros point_cloud_msg_wrapper + range-v3 rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 501583caed7f7..7aa4fc8c57bb9 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,8 +14,18 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include +#include +#include +#include + +#include + #include +#include +#include #include + namespace pointcloud_preprocessor { RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) @@ -38,8 +48,6 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); - max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -56,113 +64,303 @@ void RingOutlierFilterComponent::faster_filter( std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); - output.data.resize(output.point_step * input->width); - size_t output_size = 0; - - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - - std::vector> ring2indices; - ring2indices.reserve(max_rings_num_); - - for (uint16_t i = 0; i < max_rings_num_; i++) { - ring2indices.push_back(std::vector()); - 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(&input->data[data_idx + ring_offset]); - ring2indices[ring].push_back(data_idx); + // Check we can memcpy from input cloud data to PointXYZIRADRT (thus PointXYZI) + // FIXME(VRichardJP) use utils::is_data_layout_compatible_with_PointXYZIRADRT + // once https://github.com/autowarefoundation/autoware.universe/pull/3306 is merged + { + bool correct_layout = true; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, x) == + input->fields.at(static_cast(autoware_point_types::PointIndex::X)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::X)).datatype == + sensor_msgs::msg::PointField::FLOAT32; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::X)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, y) == + input->fields.at(static_cast(autoware_point_types::PointIndex::Y)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Y)).datatype == + sensor_msgs::msg::PointField::FLOAT32; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Y)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, z) == + input->fields.at(static_cast(autoware_point_types::PointIndex::Z)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Z)).datatype == + sensor_msgs::msg::PointField::FLOAT32; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Z)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, intensity) == + input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).datatype == + sensor_msgs::msg::PointField::FLOAT32; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, ring) == + input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).datatype == + sensor_msgs::msg::PointField::UINT16; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, azimuth) == + input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).datatype == + sensor_msgs::msg::PointField::FLOAT32; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, distance) == + input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).datatype == + sensor_msgs::msg::PointField::FLOAT32; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).count == 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, return_type) == + input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)) + .datatype == sensor_msgs::msg::PointField::UINT8; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).count == + 1; + correct_layout &= + offsetof(autoware_point_types::PointXYZIRADRT, time_stamp) == + input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).datatype == + sensor_msgs::msg::PointField::FLOAT64; + correct_layout &= + input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).count == 1; + if (!correct_layout) { + // TODO(VRichardJP) Handle this properly + throw std::runtime_error("Input cloud has invalid data layout"); + } } - // 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; + // The initial implementation of ring outlier filter looked like this: + // 1. Iterate over the input cloud and group point indices by ring + // 2. For each ring: + // 2.1. iterate over the ring points, and group points belonging to the same "walk" + // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long enough. + // + // The problem of this implementation is that it was not cache friendly at all. PointCloud2 data + // is naturally written in "azimut-major order" and not "ring-major order". That means that points + // belonging to the same ring are actually very far from each other on memory. So even though the + // previous algorithm is optimization friendly (for example ring processing could be done in + // parallel), running the old implementation had terrible side effect on the performance of other + // nodes. + // + // To tackle this issue, the algorithm was rewritten so that points would be accessed in order. To + // do so, it is necessary to keep track of the state of all the rings and walks simultaneously. + // This requires a bit more work than before, but it greatly improve the cache performance. + + // ad-hoc struct to store finished walks information (for isCluster()) + struct WalkInfo + { + int num_points; + float first_point_distance; + float first_point_azimuth; + float last_point_distance; + float last_point_azimuth; + }; + + // ad-hoc struct to keep track of each ring current walk + struct RingWalkInfo + { + size_t walk_idx; // index of associated walk in walks vector + float prev_azimuth; // azimuth of previous point on the ring + float prev_distance; // distance of previous point on the ring + }; + + // helper functions + + // extract ring from point raw data + auto getPointRing = [=](const unsigned char * raw_p) { + constexpr size_t ring_offset = offsetof(autoware_point_types::PointXYZIRADRT, ring); + uint16_t ring{}; + std::memcpy(&ring, &raw_p[ring_offset], sizeof(ring)); + return ring; + }; + // extract azimuth from point raw data + auto getPointAzimuth = [=](const unsigned char * raw_p) { + constexpr size_t azimuth_offset = offsetof(autoware_point_types::PointXYZIRADRT, azimuth); + float azimuth{}; + std::memcpy(&azimuth, &raw_p[azimuth_offset], sizeof(azimuth)); + return azimuth; + }; + // extract distance from point raw data + auto getPointDistance = [=](const unsigned char * raw_p) { + constexpr size_t distance_offset = offsetof(autoware_point_types::PointXYZIRADRT, distance); + float distance{}; + std::memcpy(&distance, &raw_p[distance_offset], sizeof(distance)); + return distance; + }; + + // check if walk is a valid cluster + const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_; + auto isCluster = [=](const WalkInfo & walk_info) { + // A cluster is a walk which has many points or is long enough + if (walk_info.num_points > num_points_threshold_) return true; + + // Using the law of cosines, the distance between 2 points can be written as: + // |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a) + // where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between + // the 2 points. + const float dist2 = + walk_info.first_point_distance * walk_info.first_point_distance + + walk_info.last_point_distance * walk_info.last_point_distance - + 2 * walk_info.first_point_distance * walk_info.last_point_distance * + std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0)); + return dist2 > object_length_threshold2; + }; + + // check if 2 points belong to the same walk + auto isSameWalk = + [=](float curr_distance, float curr_azimuth, float prev_distance, float prev_azimuth) { + float azimuth_diff = curr_azimuth - prev_azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + return std::max(curr_distance, prev_distance) < + std::min(curr_distance, prev_distance) * distance_ratio_ && + azimuth_diff < 100.f; + }; + + // tmp vectors to keep track of walk/ring state while processing points in order (cache efficient) + std::vector walks; // finished walks + std::vector rings_curr_walk; // each ring curr walk info + std::vector rings_first_walk_idx; // each ring very first walk idx + std::vector points_walk_idx; // each point walk idx + + walks.reserve(max_rings_num_); + // initialize each ring with empty walk + rings_curr_walk.resize(max_rings_num_, RingWalkInfo{-1U, 0., 0.}); + rings_first_walk_idx.resize(max_rings_num_, -1U); + // points are initially associated to no walk + points_walk_idx.resize(input->width * input->height, -1U); + + int invalid_ring_count = 0; + + // Build walks and classify points + for (const auto & [raw_p, point_walk_idx] : + ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_idx)) { + const uint16_t ring_idx = getPointRing(raw_p.data()); + const float curr_azimuth = getPointAzimuth(raw_p.data()); + const float curr_distance = getPointDistance(raw_p.data()); + + if (ring_idx >= max_rings_num_) { + // Either the data is corrupted or max_rings_num_ is not set correctly + // Note: point_walk_idx == -1 so the point will be filtered out + invalid_ring_count += 1; + continue; + } - walk_first_idx = 0; - walk_last_idx = -1; + auto & ring = rings_curr_walk[ring_idx]; + if (ring.walk_idx == -1U) { + // first walk ever for this ring + walks.push_back(WalkInfo{1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}); + const auto walk_idx = walks.size() - 1; + point_walk_idx = walk_idx; + rings_first_walk_idx[ring_idx] = walk_idx; + ring.walk_idx = walk_idx; + ring.prev_azimuth = curr_azimuth; + ring.prev_distance = curr_distance; + continue; + } - 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; + auto & walk = walks[ring.walk_idx]; + if (isSameWalk(curr_distance, curr_azimuth, ring.prev_distance, ring.prev_azimuth)) { + // current point is part of previous walk + walk.num_points += 1; + walk.last_point_distance = curr_distance; + walk.last_point_azimuth = curr_azimuth; + point_walk_idx = ring.walk_idx; + } else { + // previous walk is finished, start a new one + walks.push_back(WalkInfo{1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}); + point_walk_idx = walks.size() - 1; + ring.walk_idx = walks.size() - 1; + } - // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) + ring.prev_azimuth = curr_azimuth; + ring.prev_distance = curr_distance; + } - const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); - const float & next_azimuth = - *reinterpret_cast(&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; + // So far, we have processed ring points as if rings were not circular. Of course, the last and + // first points of a ring could totally be part of the same walk. When such thing happens, we need + // to merge the two walks + for (const auto & [first_walk_idx, last_walk_idx] : ranges::views::zip( + rings_first_walk_idx, rings_curr_walk | ranges::views::transform([](const auto & ring) { + return ring.walk_idx; + }))) { + if (first_walk_idx == -1U || last_walk_idx == -1U) { + continue; + } + auto & first_walk = walks[first_walk_idx]; + auto & last_walk = walks[last_walk_idx]; + // check if the two walks are connected + if (isSameWalk( + first_walk.first_point_distance, first_walk.first_point_azimuth, + last_walk.last_point_distance, last_walk.last_point_azimuth)) { + // merge + first_walk.first_point_distance = last_walk.first_point_distance; + first_walk.first_point_azimuth = last_walk.first_point_azimuth; + last_walk.last_point_distance = first_walk.last_point_distance; + last_walk.last_point_azimuth = first_walk.last_point_azimuth; + } + } - const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); - const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + // segregate walks by cluster (for fast lookup) + std::vector walk_is_cluster; + walk_is_cluster.reserve(walks.size()); + for (const auto & walk_info : walks) { + walk_is_cluster.push_back(isCluster(walk_info)); + } - 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 + // finally copy points + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width * input->height); + size_t output_size = 0; + if (transform_info.need_transform) { + for (const auto & [raw_p, point_walk_idx] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_idx)) { + if (point_walk_idx == -1U || !walk_is_cluster[point_walk_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(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&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]; - output_ptr->intensity = input_ptr->intensity; - } else { - *output_ptr = *input_ptr; - } - - output_size += output.point_step; - } - } + // assume binary representation of input point is compatible with PointXYZI + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); - walk_first_idx = idx + 1; - } + Eigen::Vector4f p(out_point.x, out_point.y, out_point.z, 1); + p = transform_info.eigen_transform * p; + out_point.x = p[0]; + out_point.y = p[1]; + out_point.z = p[2]; + out_point.intensity = out_point.intensity; - 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(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&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]; - output_ptr->intensity = input_ptr->intensity; - } else { - *output_ptr = *input_ptr; - } - - output_size += output.point_step; + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + output_size += sizeof(PointXYZI); + } + } else { + for (const auto & [raw_p, point_walk_idx] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_idx)) { + if (point_walk_idx == -1U || !walk_is_cluster[point_walk_idx]) { + continue; } + + // assume binary representation of input point is compatible with PointXYZI + std::memcpy(&output.data[output_size], raw_p.data(), sizeof(PointXYZI)); + output_size += sizeof(PointXYZI); } } @@ -172,6 +370,7 @@ void RingOutlierFilterComponent::faster_filter( // == true` output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_; + // assume input first 4 fields are compatible with PointXYZI type output.fields.resize(4); // x, y, z, intensity std::copy( input->fields.begin(), @@ -184,6 +383,12 @@ void RingOutlierFilterComponent::faster_filter( output.width = static_cast(output.data.size() / output.height / output.point_step); output.row_step = static_cast(output.data.size() / output.height); + if (invalid_ring_count > 0) { + RCLCPP_WARN( + get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.", + invalid_ring_count, max_rings_num_); + } + // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);