output with gray if the point is not within the threshold

ros-perception · Apr 23, 2017 · 233500f · 233500f
1 parent e2ee6bc
commit 233500f
Showing 1 changed file with 25 additions and 0 deletions.
diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
@@ -52,6 +52,19 @@ class RGBColorFilter;
 class HLSColorFilter;
 class HSVColorFilter;
 
+// http://stackoverflow.com/questions/12703305/how-to-compare-scalars-in-opencv
+// Custom operator to compare cv::Scalar class...
+bool operator <(const cv::Scalar &a, const cv::Scalar &b)
+{
+  bool Result = true;
+  // Do whatever you think a Scalar comparison must be.
+  std::cerr << a.depth <<  " " << a.channels << std::endl;
+  for ( size_t i = 0 ; i < 4; i++ ) {
+    if ( a[i] >= b[i] ) Result = false;
+  }
+  return Result;
+}
+
 template <typename Config>
 class ColorFilterNodelet : public opencv_apps::Nodelet
 {
@@ -255,6 +268,10 @@ class RGBColorFilterNodelet
       memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      if ( output_image.at<unsigned char>(i) == 0 ) {
+        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+        r = g = b = gray;
+      }
       unsigned char rgb[4] = {r, g, b, 0};
       memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
@@ -342,6 +359,10 @@ class HLSColorFilterNodelet
       memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      if ( output_image.at<unsigned char>(i) == 0 ) {
+        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+        r = g = b = gray;
+      }
       unsigned char rgb[4] = {r, g, b, 0};
       memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
@@ -428,6 +449,10 @@ class HSVColorFilterNodelet
       memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      if ( output_image.at<unsigned char>(i) == 0 ) {
+        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+        r = g = b = gray;
+      }
       unsigned char rgb[4] = {r, g, b, 0};
       memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }