Merge pull request #75 from AutoModality/AM-706/uam_canyon

fix: fixed uninit var bug in dust_filter.

Author: danhennage

include/am_utils/am_pcl_utils.h

@@ -14,6 +14,8 @@ class AMPCLUtils
   static pcl::PointCloud<pcl::PointXYZIR>::ConstPtr convertToPointXYZIR(
       const sensor_msgs::PointCloud2::ConstPtr pc2,
       pcl::PointCloud<pcl::PointXYZIR>::Ptr &pcl);
+  static bool isSetToNaN(pcl::PointXYZIR &point);
+  static void setNaN(pcl::PointXYZIR &point);
 };
 
 }

include/am_utils/dust_filter.h

@@ -34,16 +34,14 @@ class DustFilter
   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);
+  int process(pcl::PointCloud<pcl::PointXYZIR> &pc, bool skip_dust_filter = false);
   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_);

src/am_utils/am_pcl_utils.cpp

@@ -31,5 +31,25 @@ pcl::PointCloud<pcl::PointXYZIR>::ConstPtr AMPCLUtils::convertToPointXYZIR(
   return pcl;
 }
 
+bool AMPCLUtils::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 AMPCLUtils::setNaN(pcl::PointXYZIR &point)
+{
+  point.x = std::nanf("");
+  point.y = std::nanf("");
+  point.z = std::nanf("");
+  point.intensity = std::nanf("");
+  point.range = std::nanf("");
+}
+
 } // namespace
 

src/am_utils/dust_filter.cpp

@@ -3,6 +3,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl_ros/point_cloud.h>
 
+#include <am_utils/am_pcl_utils.h>
 #include <am_utils/dust_filter.h>
 #include <am_utils/point_types.h>
 #include <vb_util_lib/frame_names.h>
@@ -33,10 +34,10 @@ void DustFilter::getParams()
   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.5);
-  nh_.param<float>(name_space_ + "/inside_max_x_m", inside_max_x_m_, 0.5);
-  nh_.param<float>(name_space_ + "/inside_min_y_m", inside_min_y_m_, -0.5);
-  nh_.param<float>(name_space_ + "/inside_max_y_m", inside_max_y_m_, 0.5);
+  nh_.param<float>(name_space_ + "/inside_min_x_m", inside_min_x_m_, -0.7);
+  nh_.param<float>(name_space_ + "/inside_max_x_m", inside_max_x_m_, 0.7);
+  nh_.param<float>(name_space_ + "/inside_min_y_m", inside_min_y_m_, -0.7);
+  nh_.param<float>(name_space_ + "/inside_max_y_m", inside_max_y_m_, 0.7);
   nh_.param<float>(name_space_ + "/inside_min_z_m", inside_min_z_m_, -0.5);
   nh_.param<float>(name_space_ + "/inside_max_z_m", inside_max_z_m_, 0.5);
   nh_.param<float>(name_space_ + "/outside_min_x_m", outside_min_x_m_, std::numeric_limits<float>::lowest());
@@ -59,7 +60,7 @@ void DustFilter::getParams()
 
 void DustFilter::printInsideParams()
 {
-  ROS_INFO_STREAM(name_space_ + "/inside_max_x_m = " << inside_max_x_m_);
+  ROS_INFO_STREAM(name_space_ + "/inside_min_x_m = " << inside_min_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_);
@@ -77,27 +78,27 @@ void DustFilter::printOutsideParams()
   ROS_INFO_STREAM(name_space_ + "/outside_max_z_m = " << outside_max_z_m_);
 }
 
-int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &pcl)
+int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &pcl, bool skip_dust_filter)
 {
-  bool nan_mask[pcl.width * pcl.width];
+  bool nan_mask[pcl.width * pcl.height];
 
   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;
+      nan_mask[index] = false;
       pcl::PointXYZIR &point = pcl.points[index];
 
       // if already nan, continue
-      if(isSetToNaN(point))
+      if(AMPCLUtils::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;
@@ -175,7 +176,7 @@ int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &pcl)
       pcl::PointXYZIR &point = pcl.points[index];
       if(nan_mask[index])
       {
-        setNaN(point);
+        AMPCLUtils::setNaN(point);
         points_removed++;
         is_dense = false;
       }
@@ -192,26 +193,6 @@ int DustFilter::process(pcl::PointCloud<pcl::PointXYZIR> &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_;