Merge pull request #72 from AutoModality/AM-633/culvert_passageway

feat: moved dust filter from am_perception

Author: danhennage

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 

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 <sensor_msgs/PointCloud2.h>
+#include <am_utils/point_types.h>
+
+namespace am
+{
+
+class AMPCLUtils
+{
+
+public:
+  static pcl::PointCloud<pcl::PointXYZIR>::ConstPtr convertToPointXYZIR(
+      const sensor_msgs::PointCloud2::ConstPtr pc2,
+      pcl::PointCloud<pcl::PointXYZIR>::Ptr &pcl);
+};
+
+}
+
+#endif /* AM_UTILS_INCLUDE_AM_UTILS_AM_PCL_UTILS_H_ */

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 <pcl_ros/point_cloud.h>
+
+#include <am_utils/point_types.h>
+
+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<pcl::PointXYZIR> &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_ */

package.xml

@@ -22,5 +22,6 @@
    <depend>std_msgs</depend>
    <depend>tf2</depend>
    <depend>tf2_ros</depend>   
+   <depend>pcl_conversions</depend>   
 
 </package>

src/am_utils/am_pcl_utils.cpp

@@ -0,0 +1,35 @@
+#include <ros/ros.h>
+
+#include <am_utils/am_pcl_utils.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace am
+{
+
+pcl::PointCloud<pcl::PointXYZIR>::ConstPtr AMPCLUtils::convertToPointXYZIR(
+    const sensor_msgs::PointCloud2::ConstPtr pc2, pcl::PointCloud<pcl::PointXYZIR>::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<float> 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
+

src/am_utils/dust_filter.cpp

@@ -0,0 +1,236 @@
+#include <ros/ros.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+
+#include <am_utils/dust_filter.h>
+#include <am_utils/point_types.h>
+#include <vb_util_lib/frame_names.h>
+#include <vb_util_lib/topics.h>
+
+#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<pcl::PointCloud<pcl::PointXYZIR>>("/debug/dust_filter/pcl", 10);
+}
+
+DustFilter::~DustFilter()
+{
+}
+
+void DustFilter::getParams()
+{
+  static bool first_run = true;
+
+  nh_.param<float>(name_space_ + "/range_threshold_m", range_threshold_m_, DEFAULT_RANGE_THRESHHOLD_M);
+  nh_.param<int>(name_space_ + "/min_neighbors", min_neighbors_, DEFAULT_MIN_NEIGHBORS);
+  nh_.param<float>(name_space_ + "/max_range_m", max_range_m_, DEFAULT_MAX_RANGE_M);
+  nh_.param<float>(name_space_ + "/inside_min_x_m", inside_min_x_m_, 0);
+  nh_.param<float>(name_space_ + "/inside_max_x_m", inside_max_x_m_, 0);
+  nh_.param<float>(name_space_ + "/inside_min_y_m", inside_min_y_m_, 0);
+  nh_.param<float>(name_space_ + "/inside_max_y_m", inside_max_y_m_, 0);
+  nh_.param<float>(name_space_ + "/inside_min_z_m", inside_min_z_m_, 0);
+  nh_.param<float>(name_space_ + "/inside_max_z_m", inside_max_z_m_, 0);
+  nh_.param<float>(name_space_ + "/outside_min_x_m", outside_min_x_m_, std::numeric_limits<double>::min());
+  nh_.param<float>(name_space_ + "/outside_max_x_m", outside_max_x_m_, std::numeric_limits<double>::max());
+  nh_.param<float>(name_space_ + "/outside_min_y_m", outside_min_y_m_, std::numeric_limits<double>::min());
+  nh_.param<float>(name_space_ + "/outside_max_y_m", outside_max_y_m_, std::numeric_limits<double>::max());
+  nh_.param<float>(name_space_ + "/outside_min_z_m", outside_min_z_m_, std::numeric_limits<double>::min());
+  nh_.param<float>(name_space_ + "/outside_max_z_m", outside_max_z_m_, std::numeric_limits<double>::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::PointXYZIR> &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
+