Skip to content

Commit

Permalink
calculate visibility score in ring outlier filter
Browse files Browse the repository at this point in the history 
Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara committed May 14, 2024
1 parent ea009b8 commit 113ad51
Show file tree
Hide file tree
Showing 3 changed files with 150 additions and 33 deletions.
23 changes: 23 additions & 0 deletions ...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,18 @@
#include "pointcloud_preprocessor/filter.hpp"
#include "pointcloud_preprocessor/transform_info.hpp"

#include <image_transport/image_transport.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

#if __has_include(<cv_bridge/cv_bridge.hpp>)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif

#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

Expand All @@ -42,6 +51,9 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
const TransformInfo & transform_info);

image_transport::Publisher image_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr visibility_pub_;

private:
/** \brief publisher of excluded pointcloud for debug reason. **/
rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_publisher_;
Expand All @@ -53,6 +65,16 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
size_t max_points_num_per_ring_;
bool publish_outlier_pointcloud_;

// for visibility score
int noise_threshold_;
int vertical_bins_;
int horizontal_bins_;
float max_azimuth_diff_;

float min_azimuth_deg_;
float max_azimuth_deg_;
float max_distance_;

/** \brief Parameter service callback result : needed to be hold */
OnSetParametersCallbackHandle::SharedPtr set_param_res_;

Expand All @@ -77,6 +99,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
void setUpPointCloudFormat(
const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
size_t num_fields);
float calculateVisibilityScore(const PointCloud2 & input);

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
1 change: 0 additions & 1 deletion sensing/pointcloud_preprocessor/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_geometry</depend>
<depend>autoware_point_types</depend>
<depend>cgal</depend>
<depend>cv_bridge</depend>
Expand Down
159 changes: 127 additions & 32 deletions sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,16 @@

#include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp"

#include "autoware_auto_geometry/common_3d.hpp"
#include "autoware_point_types/types.hpp"

#include <sensor_msgs/point_cloud2_iterator.hpp>

#include <pcl/search/pcl_search.h>

#include <algorithm>
#include <vector>
namespace pointcloud_preprocessor
{
using autoware_point_types::PointXYZIRADRT;

RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options)
: Filter("RingOutlierFilter", options)
{
Expand All @@ -35,6 +35,10 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
outlier_pointcloud_publisher_ =
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
image_pub_ =
image_transport::create_publisher(this, "ring_outlier_filter/debug/frequency_image");
visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
}
Expand All @@ -50,6 +54,13 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
publish_outlier_pointcloud_ =
static_cast<bool>(declare_parameter("publish_outlier_pointcloud", false));

min_azimuth_deg_ = static_cast<float>(declare_parameter("min_azimuth_deg", 0.0));
max_azimuth_deg_ = static_cast<float>(declare_parameter("max_azimuth_deg", 360.0));
max_distance_ = static_cast<float>(declare_parameter("max_distance", 12.0));
vertical_bins_ = static_cast<int>(declare_parameter("vertical_bins", 128));
horizontal_bins_ = static_cast<int>(declare_parameter("horizontal_bins", 36));
noise_threshold_ = static_cast<int>(declare_parameter("noise_threshold", 2));
}

using std::placeholders::_1;
Expand All @@ -73,13 +84,7 @@ void RingOutlierFilterComponent::faster_filter(
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.data.resize(outlier_points.point_step * input->width);
}
pcl::PointCloud<PointXYZIRADRT>::Ptr outlier_pcl(new pcl::PointCloud<PointXYZIRADRT>);

const auto ring_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Ring)).offset;
Expand All @@ -89,6 +94,10 @@ void RingOutlierFilterComponent::faster_filter(
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;
const auto time_stamp_offset =
input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::TimeStamp)).offset;

std::vector<std::vector<size_t>> ring2indices;
ring2indices.reserve(max_rings_num_);
Expand Down Expand Up @@ -163,24 +172,32 @@ void RingOutlierFilterComponent::faster_filter(
}
} 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]);
PointXYZIRADRT outlier_point;
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];
outlier_point.x = p[0];
outlier_point.y = p[1];
outlier_point.z = p[2];
} else {
*outlier_ptr = *input_ptr;
outlier_point = *input_ptr;
}
const float & intensity = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + intensity_offset]);
outlier_ptr->intensity = intensity;

outlier_points_size += outlier_points.point_step;
outlier_point.intensity = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + intensity_offset]);
outlier_point.ring = *reinterpret_cast<const uint16_t *>(
&input->data[indices[walk_first_idx] + ring_offset]);
outlier_point.azimuth = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + azimuth_offset]);
outlier_point.distance = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + distance_offset]);
outlier_point.return_type = *reinterpret_cast<const uint8_t *>(
&input->data[indices[walk_first_idx] + return_type_offset]);
outlier_point.time_stamp = *reinterpret_cast<const float *>(
&input->data[indices[walk_first_idx] + time_stamp_offset]);
outlier_pcl->push_back(outlier_point);
}
}

Expand Down Expand Up @@ -213,30 +230,47 @@ void RingOutlierFilterComponent::faster_filter(
}
} 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]]);
PointXYZIRADRT outlier_point;

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];
outlier_point.x = p[0];
outlier_point.y = p[1];
outlier_point.z = p[2];
} else {
*outlier_ptr = *input_ptr;
outlier_point = *input_ptr;
}
const float & intensity =
outlier_point.intensity =
*reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
outlier_ptr->intensity = intensity;
outlier_points_size += outlier_points.point_step;
outlier_point.ring =
*reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
outlier_point.azimuth =
*reinterpret_cast<const float *>(&input->data[indices[i] + azimuth_offset]);
outlier_point.distance =
*reinterpret_cast<const float *>(&input->data[indices[i] + distance_offset]);
outlier_point.return_type =
*reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
outlier_point.time_stamp =
*reinterpret_cast<const float *>(&input->data[indices[i] + time_stamp_offset]);
outlier_pcl->push_back(outlier_point);
}
}
}

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

if (publish_outlier_pointcloud_) {
setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
outlier_pointcloud_publisher_->publish(outlier_points);
PointCloud2 outlier;
pcl::toROSMsg(*outlier_pcl, outlier);
outlier.header = input->header;
outlier_pointcloud_publisher_->publish(outlier);

tier4_debug_msgs::msg::Float32Stamped visibility_msg;
visibility_msg.data = calculateVisibilityScore(outlier);
visibility_msg.stamp = input->header.stamp;
visibility_pub_->publish(visibility_msg);
}

// add processing time for debug
Expand Down Expand Up @@ -288,6 +322,12 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
RCLCPP_DEBUG(
get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_);
}
if (get_param(p, "vertical_bins", vertical_bins_)) {
RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_);
}
if (get_param(p, "max_azimuth_diff", max_azimuth_diff_)) {
RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_diff to: %f.", max_azimuth_diff_);
}

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
Expand Down Expand Up @@ -319,6 +359,61 @@ void RingOutlierFilterComponent::setUpPointCloudFormat(
"intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
}

float RingOutlierFilterComponent::calculateVisibilityScore(
const sensor_msgs::msg::PointCloud2 & input)
{
pcl::PointCloud<PointXYZIRADRT>::Ptr input_cloud(new pcl::PointCloud<PointXYZIRADRT>);
pcl::fromROSMsg(input, *input_cloud);

const uint32_t vertical_bins = vertical_bins_;
const uint32_t horizontal_bins = horizontal_bins_;
const float max_azimuth = max_azimuth_deg_ * 100.0;
const float min_azimuth = min_azimuth_deg_ * 100.0;

const uint32_t horizontal_resolution =
static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);

std::vector<pcl::PointCloud<PointXYZIRADRT>> ring_point_clouds(vertical_bins);
cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));

// Split points into rings
for (const auto & point : input_cloud->points) {
ring_point_clouds.at(point.ring).push_back(point);
}

// Calculate frequency for each bin in each ring
for (const auto & ring_points : ring_point_clouds) {
if (ring_points.empty()) continue;

const uint ring_id = ring_points.front().ring;
std::vector<int> frequency_in_ring(horizontal_bins, 0);

for (const auto & point : ring_points.points) {
if (point.azimuth < min_azimuth || point.azimuth >= max_azimuth) continue;
if (point.distance >= max_distance_) continue;

const uint bin_index =
static_cast<uint>((point.azimuth - min_azimuth) / horizontal_resolution);

frequency_in_ring[bin_index]++;
frequency_in_ring[bin_index] =
std::min(frequency_in_ring[bin_index], 255); // Ensure value is within uchar range

frequency_image.at<uchar>(ring_id, bin_index) =
static_cast<uchar>(frequency_in_ring[bin_index]);
}
}

cv::Mat binary_image;
cv::inRange(frequency_image, noise_threshold_, 255, binary_image);

const int num_pixels = cv::countNonZero(frequency_image);
const float num_filled_pixels =
static_cast<float>(num_pixels) / static_cast<float>(vertical_bins * horizontal_bins);

return 1.0f - num_filled_pixels;
}

} // namespace pointcloud_preprocessor

#include <rclcpp_components/register_node_macro.hpp>
Expand Down

0 comments on commit 113ad51

Please sign in to comment.