Moved outlier filter to separate files and removed parameter lookup in callback

Author: svn

CMakeLists.txt

@@ -29,10 +29,10 @@ target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
 add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp)
 target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES})
 
-add_library(ipa_pointcloud_to_laserscan src/ipa_pointcloud_to_laserscan_nodelet.cpp)
+add_library(ipa_pointcloud_to_laserscan src/ipa_pointcloud_to_laserscan_nodelet.cpp src/scan_outlier_removal_filter.cpp)
 target_link_libraries(ipa_pointcloud_to_laserscan ${catkin_LIBRARIES})
 
-add_executable(ipa_pointcloud_to_laserscan_node src/ipa_pointcloud_to_laserscan_node.cpp)
+add_executable(ipa_pointcloud_to_laserscan_node src/ipa_pointcloud_to_laserscan_node.cpp src/scan_outlier_removal_filter.cpp)
 target_link_libraries(ipa_pointcloud_to_laserscan_node ipa_pointcloud_to_laserscan ${catkin_LIBRARIES})
 
 install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node ipa_pointcloud_to_laserscan ipa_pointcloud_to_laserscan_node 

include/pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet.h

@@ -51,6 +51,7 @@
 #include "tf2_ros/message_filter.h"
 #include "message_filters/subscriber.h"
 #include "sensor_msgs/PointCloud2.h"
+#include <pointcloud_to_laserscan/scan_outlier_removal_filter.h>
 
 
 namespace pointcloud_to_laserscan
@@ -65,6 +66,7 @@ namespace pointcloud_to_laserscan
 
   public:
     IpaPointCloudToLaserScanNodelet();
+    void configure_filter();
 
   private:
     virtual void onInit();
@@ -86,12 +88,14 @@ namespace pointcloud_to_laserscan
     message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
     boost::shared_ptr<MessageFilter> message_filter_;
 
+    scan_outlier_filter::ScanOutlierRemovalFilter outlier_filter_;
     // ROS Parameters
     unsigned int input_queue_size_;
     std::string target_frame_;
     double tolerance_;
     double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
     bool use_inf_;
+    bool use_outlier_filter_;
   };
 
 }  // pointcloud_to_laserscan

include/pointcloud_to_laserscan/scan_outlier_removal_filter.h

@@ -0,0 +1,69 @@
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010-2012, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+/*
+ * Author: Sofie Nilsson
+ */
+
+#ifndef IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
+#define IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
+
+#include <sensor_msgs/LaserScan.h>
+
+namespace scan_outlier_filter
+{
+	class ScanOutlierRemovalFilter
+	{
+	private:
+		bool filter_configured_;
+		double cluster_break_distance_; 
+		int max_noise_cluster_size_; 
+		double max_noise_cluster_distance_;
+
+	public:
+		ScanOutlierRemovalFilter(): 
+			filter_configured_(false), 
+			cluster_break_distance_(0.0), 
+			max_noise_cluster_size_(0), 
+			max_noise_cluster_distance_(0.0)
+			{};
+		void configure(double cluster_break_distance, int max_noise_cluster_size, double max_noise_cluster_distance);
+
+		void remove_outliers(sensor_msgs::LaserScan &scan);
+	};
+}
+#endif //IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER

src/ipa_pointcloud_to_laserscan_nodelet.cpp

@@ -51,7 +51,7 @@
 
 namespace pointcloud_to_laserscan
 {
-  void scan_outlier_removal_filter(sensor_msgs::LaserScan &scan, double cluster_break_distance, int max_noise_cluster_size, double max_noise_cluster_distance);
+  //void scan_outlier_removal_filter(sensor_msgs::LaserScan &scan, double cluster_break_distance, int max_noise_cluster_size, double max_noise_cluster_distance);
 
   IpaPointCloudToLaserScanNodelet::IpaPointCloudToLaserScanNodelet() {}
 
@@ -76,6 +76,8 @@ namespace pointcloud_to_laserscan
     private_nh_.param<int>("concurrency_level", concurrency_level, 1);
     private_nh_.param<bool>("use_inf", use_inf_, true);
 
+    configure_filter();
+
     //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size
     if (concurrency_level == 1)
     {
@@ -115,6 +117,23 @@ namespace pointcloud_to_laserscan
                                                  boost::bind(&IpaPointCloudToLaserScanNodelet::disconnectCb, this));
   }
 
+  void IpaPointCloudToLaserScanNodelet::configure_filter()
+  {
+    NODELET_DEBUG_STREAM("configuring filter");
+    // Get filter related parameters
+    
+    private_nh_.param<bool>("use_outlier_filter", use_outlier_filter_, false);
+
+    double max_noise_cluster_distance;
+    double cluster_break_distance;
+    int max_noise_cluster_size;
+    private_nh_.param<double>("max_noise_cluster_distance", max_noise_cluster_distance, 2.0);
+    private_nh_.param<double>("cluster_break_distance", cluster_break_distance, 0.3);
+    private_nh_.param<int>("max_noise_cluster_size", max_noise_cluster_size, 5);
+
+    outlier_filter_.configure(cluster_break_distance, max_noise_cluster_size, max_noise_cluster_distance);
+  }
+
   void IpaPointCloudToLaserScanNodelet::connectCb()
   {
     boost::mutex::scoped_lock lock(connect_mutex_);
@@ -148,6 +167,7 @@ namespace pointcloud_to_laserscan
     NODELET_INFO_STREAM("PC with timestamp " << cloud_msg->header.stamp << " recevied at time " << start_time);
     // TODO move to config and add dynamic reconfiguring
     // Get filter related parameters
+    /*
     bool use_outlier_filter;
     double max_noise_cluster_distance;
     double cluster_break_distance;
@@ -156,6 +176,7 @@ namespace pointcloud_to_laserscan
     private_nh_.param<double>("max_noise_cluster_distance", max_noise_cluster_distance, 2.0);
     private_nh_.param<double>("cluster_break_distance", cluster_break_distance, 0.3);
     private_nh_.param<int>("max_noise_cluster_size", max_noise_cluster_size, 5);
+    */
 
     // convert const ptr to ptr to support downsampling
     sensor_msgs::PointCloud2Ptr cloud(boost::const_pointer_cast<sensor_msgs::PointCloud2>(cloud_msg));
@@ -310,9 +331,10 @@ namespace pointcloud_to_laserscan
 
     }
 
-    if(use_outlier_filter)
+    if(use_outlier_filter_)
     {
-        scan_outlier_removal_filter(output, cluster_break_distance, max_noise_cluster_size, max_noise_cluster_distance);
+        //scan_outlier_removal_filter(output, cluster_break_distance, max_noise_cluster_size, max_noise_cluster_distance);
+      outlier_filter_.remove_outliers(output);
     }
 
     ros::Time end_time = ros::Time::now();

src/scan_outlier_removal_filter.cpp

@@ -0,0 +1,88 @@
+
+#include <pointcloud_to_laserscan/scan_outlier_removal_filter.h>
+
+using namespace scan_outlier_filter; 
+
+void ScanOutlierRemovalFilter::configure(double cluster_break_distance, int max_noise_cluster_size, double max_noise_cluster_distance)
+{
+	filter_configured_ = true;
+	cluster_break_distance_ = cluster_break_distance;
+	max_noise_cluster_size_ = max_noise_cluster_size;
+	max_noise_cluster_distance_ = max_noise_cluster_distance;
+}
+
+void ScanOutlierRemovalFilter::remove_outliers(sensor_msgs::LaserScan &scan)
+{
+	if (!filter_configured_)
+		return;
+
+	// help function initialization
+      int ranges_size = scan.ranges.size();
+      int i_current_cluster = 0;
+      bool cluster_further_away = false;
+      std::vector<int> cluster_indecies;
+      double diffs[ranges_size-1];
+
+      // calculate diffs
+      for (int i = 1; i< ranges_size; i++)
+      {
+          diffs[i-1] = scan.ranges[i] - scan.ranges[i-1];
+      }
+
+      // find and remove outliers
+      for (int i = 0; i< ranges_size-1; i++)
+      {
+          // add point to cluster
+          cluster_indecies.push_back(i);
+          i_current_cluster ++;
+
+          // check if last point in cluster; find diff larger than border -> cluster separation
+          if (diffs[i] > cluster_break_distance_ || diffs[i] < -cluster_break_distance_)
+          {
+              bool new_cluster_further_away = (diffs[i] > 0);
+
+              // Only remove cluster if it is closer than surrounding clusters
+              if ((i_current_cluster < max_noise_cluster_size_) && (new_cluster_further_away != cluster_further_away))
+              {
+                  // check if all cluster points are closer than the max noise distance
+                  bool is_noise_cluster = true;
+                  for (int k = 0; k < i_current_cluster; k++)
+                  {
+                      if (scan.ranges[cluster_indecies[k]] > max_noise_cluster_distance_)
+                      {
+                          is_noise_cluster = false;
+                          break;
+                      }
+                  }
+
+                  // Only remove cluster points if all points are closer than the max noise distance
+                  if(is_noise_cluster)
+                  {
+                      for (int k = 0; k < i_current_cluster; k++)
+                      {
+                          if (scan.ranges[cluster_indecies[k]] < max_noise_cluster_distance_)
+                              scan.ranges[cluster_indecies[k]] = scan.range_max;
+                      }
+                  }
+              }
+
+              // initialize new cluster
+              cluster_further_away = new_cluster_further_away;
+              cluster_indecies.clear();
+              i_current_cluster = 0;
+          }
+      }
+
+
+      // remove last cluster if small enough
+      if ((i_current_cluster < cluster_break_distance_) && !cluster_further_away)
+      {
+          for (int k = 0; k < i_current_cluster; k++)
+          {
+              scan.ranges[cluster_indecies[k]] = scan.range_max;
+          }
+          // remove last point as well
+          scan.ranges[ranges_size-1] = scan.range_max;
+      }
+  
+}