PCODAR: various fixes / tuning

perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg

@@ -9,8 +9,8 @@ gen = ParameterGenerator()
 gen.add("accumulator_number_persistant_clouds", int_t, 1, "", 10, 0, 100)
 
 # Filter
-gen.add("persistant_cloud_filter_mean_k", double_t, 2, "", 15., 1., 100.)
-gen.add("persistant_cloud_filter_stddev", double_t, 2, "", 0.05, 0., 100.)
+gen.add("persistant_cloud_filter_radius", double_t, 2, "", 0.5, 0., 100.)
+gen.add("persistant_cloud_filter_min_neighbors", int_t, 2, "", 20, 0, 1000)
 
 # Clusterer
 gen.add("cluster_tolerance_m", double_t, 4, "", 4.4, 0.1, 100)

perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/persistent_cloud_filter.hpp

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
 #include <pcl/point_types.h>
 
 #include "pcodar_types.hpp"
@@ -15,7 +15,7 @@ class PersistentCloudFilter
   void update_config(Config const& config);
 
 private:
-  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> outlier_filter_;
+  pcl::RadiusOutlierRemoval<point_t> outlier_filter_;
 };
 
 }  // namespace pcodar

perception/point_cloud_object_detection_and_recognition/launch/pcodar.yaml

@@ -1,17 +1,17 @@
 # Point cloud builder
-accumulator_number_persistant_clouds : 10
+accumulator_number_persistant_clouds : 15
 
 # Filter
-persistant_cloud_filter_mean_k : 10
-persistant_cloud_filter_stddev : 0.05
+persistant_cloud_filter_min_neighbors : 20
+persistant_cloud_filter_radius : 0.25
 
 # Clusterer
 cluster_tolerance_m : 4.4
-cluster_min_points : 2
+cluster_min_points : 20
 cluster_max_points : 1000
 
 # Associator
-associator_max_distance : 2
+associator_max_distance : 5
 
 # Ogrid
 ogrid_height_meters : 300

perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp

@@ -28,6 +28,8 @@ void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clust
       int index = 0;
       float distance = 0.;
       search.approxNearestSearch((*pair).second.center_, index, distance);
+      // Search returns squared distance, so sqrt it here
+      distance = sqrt(distance);
       if (distance < min && distance < max_distance_)
       {
         min = distance;

perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp

@@ -170,11 +170,17 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud)
   (*filtered_accrued).header.frame_id = "enu";
   pub_pcl_.publish(filtered_accrued);
 
-  // Get object clusters from persistent pointcloud
-  clusters_t clusters = detector_.get_clusters(filtered_accrued);
+  // Skip object detection if all points where filtered out
+  if (!(*filtered_accrued).empty())
+  {
+    // Get object clusters from persistent pointcloud
+    clusters_t clusters = detector_.get_clusters(filtered_accrued);
 
-  // Associate current clusters with old ones
-  ass.associate(objects_, *filtered_accrued, clusters);
+    // Associate current clusters with old ones
+    ass.associate(objects_, *filtered_accrued, clusters);
+  }
+  else
+    ROS_WARN_ONCE("Filtered pointcloud had no points. Consider changing filter parameters.");
 
   UpdateObjects();
 }

perception/point_cloud_object_detection_and_recognition/src/persistent_cloud_filter.cpp

@@ -14,8 +14,8 @@ void PersistentCloudFilter::filter(point_cloud_const_ptr in, point_cloud& pc)
 
 void PersistentCloudFilter::update_config(Config const& config)
 {
-  outlier_filter_.setStddevMulThresh(config.persistant_cloud_filter_stddev);
-  outlier_filter_.setMeanK(config.persistant_cloud_filter_mean_k);
+  outlier_filter_.setRadiusSearch(config.persistant_cloud_filter_radius);
+  outlier_filter_.setMinNeighborsInRadius(config.persistant_cloud_filter_min_neighbors);
 }
 
 }  // namespace pcodar