add hue filter to «filtre» node

CMakeLists.txt

@@ -53,8 +53,8 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR
 add_executable(filtreRGB src/filtreRGB.cpp)
 target_link_libraries(filtreRGB ${catkin_LIBRARIES})
 
-add_executable(filtreHue src/filtreHue.cpp)
-target_link_libraries(filtreHue ${catkin_LIBRARIES})
+#add_executable(filtreHue src/filtreHue.cpp)
+#target_link_libraries(filtreHue ${catkin_LIBRARIES})
 
 add_executable(normal_estimator src/normal_estimator.cpp)
 target_link_libraries(normal_estimator ${catkin_LIBRARIES})

src/filtre.cpp

@@ -1,6 +1,8 @@
 #include <ros/ros.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/tracking/impl/hsv_color_coherence.hpp>
+#include <assert.h>
 
 
 typedef pcl::PointXYZRGB Point;
@@ -15,7 +17,7 @@ class Callback {
       copy_info(msg, pcl);
       BOOST_FOREACH (const Point& pt, msg->points)
       {
-        if (pt.z < zmax)
+        if (pt.z < zmax and hue_dist(pt) < delta_hue)
           pcl->push_back(pt);
       }
       pcl->height = 1;
@@ -23,12 +25,18 @@ class Callback {
       publisher.publish(pcl);
     }
 
-    Callback(ros::Publisher& pub, float z)
-    : publisher(pub), zmax(z) {}
+    Callback(ros::Publisher& pub, float z, float h, float delta_h)
+    : publisher(pub), zmax(z), hue(h), delta_hue(delta_h)
+    {
+      assert(delta_hue > 0);
+      assert(zmax > 0);
+      assert(hue >= 0);
+      assert(hue <= 360.);
+    }
 
   private:
     ros::Publisher publisher;
-    float zmax;
+    float zmax, hue, delta_hue;
 
     inline
     void
@@ -40,6 +48,21 @@ class Callback {
       b->sensor_orientation_ = a->sensor_orientation_;
       b->is_dense = a->is_dense;
     }
+
+    inline
+    float
+    hue_dist(const Point& pt)
+    {
+      float h, s, v, diff1, diff2;
+      pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
+      h *= 360.0f ;
+      diff1 = std::fabs(h - hue);
+      if (h < hue)
+        diff2 = std::fabs(360.0f + h - hue);
+      else
+        diff2 = std::fabs(360.0f + hue - h);
+      return std::max(diff1, diff2);
+    }
 };
 
 int
@@ -59,9 +82,29 @@ main(int argc, char** argv)
     ROS_INFO("zmax : %f (default value)", zmax);
   }
 
+  double hue(0);
+  if (node.getParam("hue", hue))
+  {
+    ROS_INFO("hue : %f" , hue);
+  } else {
+    node.setParam("hue", 0.0);
+    node.getParam("hue", hue);
+    ROS_INFO("hue : %f (default value)", hue);
+  }
+
+  double delta_hue(0);
+  if (node.getParam("delta_hue", delta_hue))
+  {
+    ROS_INFO("delta_hue : %f" , delta_hue);
+  } else {
+    node.setParam("delta_hue", 10.0);
+    node.getParam("delta_hue", delta_hue);
+    ROS_INFO("delta_hue : %f (default value)", delta_hue);
+  }
+
   // initialisatio
   ros::Publisher  publisher = node.advertise<PointCloud>("output", 1);
-  Callback callback(publisher, (float) zmax);
+  Callback callback(publisher, (float) zmax, (float) hue, (float) delta_hue);
   ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
 
   // démarrage