From 35ff8e6d594b329357706ad4eb487f595980d74d Mon Sep 17 00:00:00 2001 From: dan hennage Date: Wed, 17 Mar 2021 10:22:04 -0700 Subject: [PATCH] feat: moved dust filter from am_perception --- CMakeLists.txt | 4 + include/am_utils/am_pcl_utils.h | 21 +++ include/am_utils/dust_filter.h | 54 ++++++++ package.xml | 1 + src/am_utils/am_pcl_utils.cpp | 35 +++++ src/am_utils/dust_filter.cpp | 236 ++++++++++++++++++++++++++++++++ 6 files changed, 351 insertions(+) create mode 100644 include/am_utils/am_pcl_utils.h create mode 100644 include/am_utils/dust_filter.h create mode 100644 src/am_utils/am_pcl_utils.cpp create mode 100644 src/am_utils/dust_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 77f05b3..37f9678 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs tf2 tf2_ros + pcl_conversions ) find_package(OpenCV REQUIRED) @@ -44,6 +45,7 @@ catkin_package( std_msgs tf2 tf2_ros + pcl_conversions ) # TODO: why is the explicit eigen3 include @@ -61,12 +63,14 @@ add_library(am_utils src/am_utils/am_onlinestatistics.cpp src/am_utils/am_operator_utils.cpp src/am_utils/am_statistics.cpp + src/am_utils/dust_filter.cpp src/am_utils/cansocket.cpp src/am_utils/fcu_mode_type.cpp src/am_utils/flightplan_type.cpp src/am_utils/latency_wrapper.cpp src/am_utils/message_util.cpp src/am_utils/mission_cmd_type.cpp + src/am_utils/am_pcl_utils.cpp src/am_utils/operator_msg_type.cpp src/vb_util_lib/control_util.cpp src/vb_util_lib/object_state.cpp diff --git a/include/am_utils/am_pcl_utils.h b/include/am_utils/am_pcl_utils.h new file mode 100644 index 0000000..7dc2c4d --- /dev/null +++ b/include/am_utils/am_pcl_utils.h @@ -0,0 +1,21 @@ +#ifndef AM_UTILS_INCLUDE_AM_UTILS_AM_PCL_UTILS_H_ +#define AM_UTILS_INCLUDE_AM_UTILS_AM_PCL_UTILS_H_ + +#include +#include + +namespace am +{ + +class AMPCLUtils +{ + +public: + static pcl::PointCloud::ConstPtr convertToPointXYZIR( + const sensor_msgs::PointCloud2::ConstPtr pc2, + pcl::PointCloud::Ptr &pcl); +}; + +} + +#endif /* AM_UTILS_INCLUDE_AM_UTILS_AM_PCL_UTILS_H_ */ diff --git a/include/am_utils/dust_filter.h b/include/am_utils/dust_filter.h new file mode 100644 index 0000000..d792c14 --- /dev/null +++ b/include/am_utils/dust_filter.h @@ -0,0 +1,54 @@ +#ifndef AM_UTILS_INCLUDE_AM_UTILS_DUST_FILTER_H_ +#define AM_UTILS_INCLUDE_AM_UTILS_DUST_FILTER_H_ + +#include + +#include + +namespace am +{ + +class DustFilter +{ +private: + + ros::NodeHandle nh_; + + ros::Publisher debug_pcl_pub_; + + float range_threshold_m_; + float max_range_m_; + int min_neighbors_; + std::string name_space_; + + float inside_min_x_m_; + float inside_max_x_m_; + float inside_min_y_m_; + float inside_max_y_m_; + float inside_min_z_m_; + float inside_max_z_m_; + float outside_min_x_m_; + float outside_max_x_m_; + float outside_min_y_m_; + float outside_max_y_m_; + float outside_min_z_m_; + float outside_max_z_m_; + + bool isSetToNaN(pcl::PointXYZIR &point); + void setNaN(pcl::PointXYZIR &point); + void printInsideParams(); + void printOutsideParams(); + +public: + DustFilter(ros::NodeHandle &nh, std::string name_space); + ~DustFilter(); + + int process(pcl::PointCloud &pc); + void getParams(); + void setInsideFilter(float min_x_m_, float max_x_m_, float min_y_m_, float max_y_m_, float min_z_m_, float max_z_m_); + void setOutsideFilter(float min_x_m_, float max_x_m_, float min_y_m_, float max_y_m_, float min_z_m_, float max_z_m_); +}; + +} + +#endif /* AM_UTILS_INCLUDE_AM_UTILS_DUST_FILTER_H_ */ diff --git a/package.xml b/package.xml index 00e44c5..10a1e06 100644 --- a/package.xml +++ b/package.xml @@ -22,5 +22,6 @@ std_msgs tf2 tf2_ros + pcl_conversions diff --git a/src/am_utils/am_pcl_utils.cpp b/src/am_utils/am_pcl_utils.cpp new file mode 100644 index 0000000..2d416fb --- /dev/null +++ b/src/am_utils/am_pcl_utils.cpp @@ -0,0 +1,35 @@ +#include + +#include +#include +#include + +namespace am +{ + +pcl::PointCloud::ConstPtr AMPCLUtils::convertToPointXYZIR( + const sensor_msgs::PointCloud2::ConstPtr pc2, pcl::PointCloud::Ptr &pcl) +{ + pcl->header = pcl_conversions::toPCL(pc2->header); + pcl->resize(pc2->height * pc2->width); + pcl->height = pc2->height; + pcl->width = pc2->width; + int index = 0; + for (sensor_msgs::PointCloud2ConstIterator it(*pc2, "x"); it != it.end(); ++it) { + float x = it[0]; + float y = it[1]; + float z = it[2]; + float intensity = it[4]; + pcl->points[index].x = x; + pcl->points[index].y = y; + pcl->points[index].z = z; + pcl->points[index].intensity = intensity; + pcl->points[index].range = std::sqrt(x * x + y * y + z * z); + index++; + } + + return pcl; +} + +} // namespace + diff --git a/src/am_utils/dust_filter.cpp b/src/am_utils/dust_filter.cpp new file mode 100644 index 0000000..43800ba --- /dev/null +++ b/src/am_utils/dust_filter.cpp @@ -0,0 +1,236 @@ +#include + +#include +#include + +#include +#include +#include +#include + +#define DEFAULT_RANGE_THRESHHOLD_M (0.1) +#define DEFAULT_MIN_NEIGHBORS (2) +#define DEFAULT_MAX_RANGE_M (7.5) + +namespace am +{ + +DustFilter::DustFilter(ros::NodeHandle &nh, std::string name_space) : nh_(nh), name_space_(name_space) +{ + getParams(); + + debug_pcl_pub_ = nh_.advertise>("/debug/dust_filter/pcl", 10); +} + +DustFilter::~DustFilter() +{ +} + +void DustFilter::getParams() +{ + static bool first_run = true; + + nh_.param(name_space_ + "/range_threshold_m", range_threshold_m_, DEFAULT_RANGE_THRESHHOLD_M); + nh_.param(name_space_ + "/min_neighbors", min_neighbors_, DEFAULT_MIN_NEIGHBORS); + nh_.param(name_space_ + "/max_range_m", max_range_m_, DEFAULT_MAX_RANGE_M); + nh_.param(name_space_ + "/inside_min_x_m", inside_min_x_m_, 0); + nh_.param(name_space_ + "/inside_max_x_m", inside_max_x_m_, 0); + nh_.param(name_space_ + "/inside_min_y_m", inside_min_y_m_, 0); + nh_.param(name_space_ + "/inside_max_y_m", inside_max_y_m_, 0); + nh_.param(name_space_ + "/inside_min_z_m", inside_min_z_m_, 0); + nh_.param(name_space_ + "/inside_max_z_m", inside_max_z_m_, 0); + nh_.param(name_space_ + "/outside_min_x_m", outside_min_x_m_, std::numeric_limits::min()); + nh_.param(name_space_ + "/outside_max_x_m", outside_max_x_m_, std::numeric_limits::max()); + nh_.param(name_space_ + "/outside_min_y_m", outside_min_y_m_, std::numeric_limits::min()); + nh_.param(name_space_ + "/outside_max_y_m", outside_max_y_m_, std::numeric_limits::max()); + nh_.param(name_space_ + "/outside_min_z_m", outside_min_z_m_, std::numeric_limits::min()); + nh_.param(name_space_ + "/outside_max_z_m", outside_max_z_m_, std::numeric_limits::max()); + + + if (first_run) + { + first_run = false; + ROS_INFO_STREAM(name_space_ + "/range_threshold_m = " << range_threshold_m_); + ROS_INFO_STREAM(name_space_ + "/neighbors = " << min_neighbors_); + printInsideParams(); + printOutsideParams(); + } +} + +void DustFilter::printInsideParams() +{ + ROS_INFO_STREAM(name_space_ + "/inside_max_x_m = " << inside_max_x_m_); + ROS_INFO_STREAM(name_space_ + "/inside_max_x_m = " << inside_max_x_m_); + ROS_INFO_STREAM(name_space_ + "/inside_min_y_m = " << inside_min_y_m_); + ROS_INFO_STREAM(name_space_ + "/inside_max_y_m = " << inside_max_y_m_); + ROS_INFO_STREAM(name_space_ + "/inside_min_z_m = " << inside_min_z_m_); + ROS_INFO_STREAM(name_space_ + "/inside_max_z_m = " << inside_max_z_m_); +} + +void DustFilter::printOutsideParams() +{ + ROS_INFO_STREAM(name_space_ + "/outside_min_x_m = " << outside_min_x_m_); + ROS_INFO_STREAM(name_space_ + "/outside_max_x_m = " << outside_max_x_m_); + ROS_INFO_STREAM(name_space_ + "/outside_min_y_m = " << outside_min_y_m_); + ROS_INFO_STREAM(name_space_ + "/outside_max_y_m = " << outside_max_y_m_); + ROS_INFO_STREAM(name_space_ + "/outside_min_z_m = " << outside_min_z_m_); + ROS_INFO_STREAM(name_space_ + "/outside_max_z_m = " << outside_max_z_m_); +} + +int DustFilter::process(pcl::PointCloud &pcl) +{ + bool nan_mask[pcl.width * pcl.width]; + + bool is_dense = pcl.is_dense; + for (uint32_t width = 0; width < pcl.width; width++) + { + for (uint32_t height = 0; height < pcl.height; height++) + { + int index = width * pcl.height + height; + pcl::PointXYZIR &point = pcl.points[index]; + + // if already nan, continue + if(isSetToNaN(point)) + { + nan_mask[index] = true; + continue; + } + + // if beyond the max range, don't filter dust + bool skip_dust_filter = false; + if(point.range > max_range_m_) + { + skip_dust_filter = true; + } + + // filter anything close to the center + if(point.x < inside_max_x_m_ && point.x > inside_min_x_m_ && + point.y < inside_max_y_m_ && point.y > inside_min_y_m_ && + point.z < inside_max_z_m_ && point.z > inside_min_z_m_) + { + nan_mask[index] = true; + continue; + } + + // filter anything too far away + if(point.x > outside_max_x_m_ || point.x < outside_min_x_m_ || + point.y > outside_max_y_m_ || point.y < outside_min_y_m_ || + point.z > outside_max_z_m_ || point.z < outside_min_z_m_) + { + nan_mask[index] = true; + continue; + } + + if(!skip_dust_filter) + { + // filter points without neighbors + // width wraps, height doesn't + int neighbor_width_start = width - 1; + neighbor_width_start = neighbor_width_start < 0 ? pcl.width + neighbor_width_start : neighbor_width_start; + int neighbor_width_end = width + 2; + neighbor_width_end = neighbor_width_end > pcl.width - 1 ? neighbor_width_end - pcl.width : neighbor_width_end; + int neighbor_height_start = height == 0 ? height : height - 1; + int neighbor_height_end = height == pcl.height - 1 ? height : height + 1; + int neighbors = 0; + int nw = neighbor_width_start; + while(nw != neighbor_width_end) + { + for(int nh = neighbor_height_start; nh <= neighbor_height_end; nh++) + { + // don't compare to itself + if(nh == height && nw == width) + { + continue; + } + int neighbor_index = nw * pcl.height + nh; + pcl::PointXYZIR &neighbor_point = pcl.points[neighbor_index]; + if(std::abs(neighbor_point.range - point.range) < range_threshold_m_) + { + neighbors++; + } + } + nw++; + nw = nw > pcl.width - 1 ? nw - pcl.width : nw; + } + if(neighbors < min_neighbors_) + { + nan_mask[index] = true; + is_dense = false; + } + else + { + nan_mask[index] = false; + } + } + } + + } + + int points_removed = 0; + for (uint32_t width = 0; width < pcl.width; width++) + { + for (uint32_t height = 0; height < pcl.height; height++) + { + int index = width * pcl.height + height; + pcl::PointXYZIR &point = pcl.points[index]; + if(nan_mask[index]) + { + setNaN(point); + points_removed++; + is_dense = false; + } + } + } + + pcl.is_dense = is_dense; + + if (debug_pcl_pub_.getNumSubscribers() > 0) + { + debug_pcl_pub_.publish(pcl); + } + + return points_removed; +} + +bool DustFilter::isSetToNaN(pcl::PointXYZIR &point) +{ + if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) || + std::isnan(point.intensity) || std::isnan(point.intensity)) + { + setNaN(point); + return true; + } + return false; +} + +void DustFilter::setNaN(pcl::PointXYZIR &point) +{ + point.x = std::nanf(""); + point.y = std::nanf(""); + point.z = std::nanf(""); + point.intensity = std::nanf(""); + point.range = std::nanf(""); +} + +void DustFilter::setInsideFilter(float min_x_m_, float max_x_m_, float min_y_m_, float max_y_m_, float min_z_m_, float max_z_m_) +{ + inside_min_x_m_ = min_x_m_; + inside_max_x_m_ = max_x_m_; + inside_min_y_m_ = min_y_m_; + inside_max_y_m_ = max_y_m_; + inside_min_z_m_ = min_z_m_; + inside_max_z_m_ = max_z_m_; +} + +void DustFilter::setOutsideFilter(float min_x_m_, float max_x_m_, float min_y_m_, float max_y_m_, float min_z_m_, float max_z_m_) +{ + outside_min_x_m_ = min_x_m_; + outside_max_x_m_ = max_x_m_; + outside_min_y_m_ = min_y_m_; + outside_max_y_m_ = max_y_m_; + outside_min_z_m_ = min_z_m_; + outside_max_z_m_ = max_z_m_; +} + +} // namespace +