From 233500fbc9f9f93b08ae05a75e46e5ee2007fb41 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 23 Apr 2017 18:22:20 +0900 Subject: [PATCH] output with gray if the point is not within the threshold --- src/nodelet/color_filter_nodelet.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp index e7626380..02b703b8 100644 --- 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 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(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(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(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)); }