work on progress : filtreHue

launch/all.launch

@@ -13,6 +13,5 @@
     <remap from="/estimator/input" to="/filtre/output"/>
   </node>
 
-  <include file="$(find hand_control)/launch/commande.launch"/>
 
 </launch>

src/filtreHue.cpp

@@ -2,6 +2,7 @@
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/passthrough.h>
+#include <assert.h>
 
 typedef pcl::PointXYZRGB Point;
 typedef pcl::PointCloud<Point> PointCloud;
@@ -15,31 +16,41 @@ class Callback {
         copy_info(msg, pcl);
         BOOST_FOREACH (const Point& pt, msg->points)
         {
-        int h(0);
-	float rScaled(pt.r/255);
-	float gScaled(pt.g/255);
-	float bScaled(pt.b/255);
-
-        float cMax(std::max(std::max(rScaled, gScaled), bScaled));
-	float cMin(std::min(std::min(rScaled, gScaled), bScaled));
-        float cDelta(cMin-cMax);
-
-	if (cMax == rScaled){
-	  h = (int) 60*(gScaled-bScaled)/cDelta;
-	}else if (cMax = gScaled){
-	  h = (int) 60*(2+(bScaled-rScaled)/cDelta);
-	}else if (cMax = rScaled){
-	  h = (int) 60*(4+(rScaled-gScaled)/cDelta);
+          uint8_t min, max, c;
+          if (pt.r >= pt.g) {
+            if (pt.g >= pt.b) {
+              max = pt.r
+                min = pt.b;
+            } else if (pt.r >= pt.b) {
+              max = pt.r;
+              min = pt.g;
+            } else {
+              max = pt.b;
+              min = pt.g;
+            }
+          } else if (pt.r >= pt.b) {
+            max = pt.g;
+            min = pt.b;
+          } else if (pt.g >= pt.b) {
+            max = pt.g;
+            min = pt.r;
+          } else {
+            max = pt.b;
+            min = pt.r;
           }
           
-	if (h < 0) {
-	  h += 360;
-	}
+          c = max - min;
+
+          assert(c > 0);
+          assert(max > pt.r);
+          assert(max > pt.g);
+          assert(max > pt.b);
+          assert(min < pt.r);
+          assert(min < pt.g);
+          assert(min < pt.b);
+         
+          
 
-	if (abs(h - hue) < delta) {
-	  pcl->push_back(pt);
-	}
-      }
           pcl->height = 1;
           pcl->width = pcl->points.size();
           publisher.publish(pcl);
@@ -64,11 +75,11 @@ class Callback {
             b->sensor_orientation_ = a->sensor_orientation_;
             b->is_dense = a->is_dense;
           }
-};
+      };
 
-int
-main(int argc, char** argv)
-{
+    int
+      main(int argc, char** argv)
+      {
         ros::init(argc, argv, "filtreHue");
         ros::NodeHandle node("filtreHue");
 
@@ -105,4 +116,4 @@ main(int argc, char** argv)
         ros::spin();
         ROS_INFO("exit");
         return 0;
-}
+      }