Merge pull request ros-perception#126 from k-okada/feature/histogram-…

…equalization

Add histogram equalization default and clahe

CMakeLists.txt

@@ -52,6 +52,7 @@ generate_dynamic_reconfigure_options(
   cfg/GoodfeatureTrack.cfg
   cfg/CornerHarris.cfg
   #
+  cfg/EqualizeHistogram.cfg
   cfg/CamShift.cfg
   cfg/FBackFlow.cfg
   cfg/LKFlow.cfg
@@ -169,7 +170,7 @@ opencv_apps_add_nodelet(discrete_fourier_transform src/nodelet/discrete_fourier_
   # ./tutorial_code/Histograms_Matching/calcBackProject_Demo2.cpp
   # ./tutorial_code/Histograms_Matching/calcHist_Demo.cpp
   #  ./tutorial_code/Histograms_Matching/compareHist_Demo.cpp
-  # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp
+  opencv_apps_add_nodelet(equalize_histogram src/nodelet/equalize_histogram_nodelet.cpp) # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp
   # ./tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp
 
 # imagecodecs
@@ -330,6 +331,18 @@ opencv_apps_add_nodelet(watershed_segmentation src/nodelet/watershed_segmentatio
 opencv_apps_add_nodelet(simple_example src/nodelet/simple_example_nodelet.cpp)
 opencv_apps_add_nodelet(simple_compressed_example src/nodelet/simple_compressed_example_nodelet.cpp)
 
+
+# TApi
+# ./tapi/bgfg_segm.cpp
+# ./tapi/camshift.cpp
+# opencv_apps_add_nodelet(equalize_histogram src/nodelet/equalize_histogram_nodelet.cpp) ./tapi/clahe.cpp
+# ./tapi/dense_optical_flow.cpp
+# ./tapi/hog.cpp
+# ./tapi/opencl_custom_kernel.cpp
+# ./tapi/pyrlk_optical_flow.cpp
+# ./tapi/squares.cpp
+# ./tapi/ufacedetect.cpp
+
 # https://github.com/Itseez/opencv/blob/2.4/samples/cpp/simpleflow_demo.cpp
 # simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108
 if(OPENCV_HAVE_OPTFLOW)

cfg/EqualizeHistogram.cfg

@@ -0,0 +1,50 @@
+#! /usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Ivan Tarifa.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Kei Okada nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+PACKAGE = "opencv_apps"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", False)
+
+histogram_equalization_type = gen.enum([ gen.const("EqualizeHist",   int_t,  0, "Uses equalizeHist() Method"),
+                       gen.const("Clahe", int_t,  1, "Contrast Limited Adaptive Histogram Equalization")], "An Enum for histogram equaliztion methdos")
+gen.add("histogram_equalization_type", int_t, 1, "Histogram Equalization Methods", 0, 0, 1, edit_method=histogram_equalization_type)
+
+gen.add("clahe_clip_limit", double_t,    2, "Clip Limit for Clahe filter", 20, 0.0, 255.0)
+gen.add("clahe_tile_size_x", int_t,    3, "X Tile Size for Clahe filter", 32, 0, 300)
+gen.add("clahe_tile_size_y", int_t,    4, "Y Tile Size for Clahe filter", 32, 0, 300)
+
+exit(gen.generate(PACKAGE, "equalize_histogram", "EqualizeHistogram"))

launch/equalize_histogram.launch

@@ -0,0 +1,30 @@
+<launch>
+  <arg name="node_name" default="equalize_histogram" />
+
+  <arg name="image" default="image" doc="The image topic. Should be remapped to the name of the real image topic." />
+
+  <arg name="use_camera_info" default="false" doc="Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used." />
+  <arg name="debug_view" default="true" doc="Specify whether the node displays a window to show image" />
+  <arg name="queue_size" default="3" doc="Specigy queue_size of input image subscribers" />
+
+  <arg name="histogram_equalization_type" default="0" doc="Specify histogram equaliztion methods. 0: Default, 1: Clahe" />
+
+  <arg name="clahe_clip_limit" default="255.0" doc="The maximum allowed field value Clip Limit" />
+  <arg name="clahe_tile_size_x" default="300" doc="The maximum allowed field value Threshold X Limit" />
+  <arg name="clahe_tile_size_y" default="300" doc="The maximum allowed field value Threshold Y Limit" />
+
+  <arg name="use_opencl" default="true" doc="Enable or disable OpenCL" />
+
+  <!-- equalize_histogram.cpp  -->
+  <node name="$(arg node_name)" pkg="opencv_apps" type="equalize_histogram" output="screen">
+    <remap from="image" to="$(arg image)" />
+    <param name="use_camera_info" value="$(arg use_camera_info)" />
+    <param name="debug_view" value="$(arg debug_view)" />
+    <param name="queue_size" value="$(arg queue_size)" />
+    <param name="clahe_clip_limit" value="$(arg clahe_clip_limit)" />
+    <param name="clahe_tile_size_x" value="$(arg clahe_tile_size_x)" />
+    <param name="clahe_tile_size_y" value="$(arg clahe_tile_size_y)" />
+    <param name="use_opencl" value="$(arg use_opencl)" />
+    <param name="histogram_equalization_type" value="$(arg histogram_equalization_type)" />
+  </node>
+</launch>

nodelet_plugins.xml

@@ -123,6 +123,10 @@
     <description>Nodelet to demonstrate the famous watershed segmentation algorithm in OpenCV: watershed()</description>
   </class>
 
+  <class name="opencv_apps/equalize_histogram" type="opencv_apps::EqualizeHistogramNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet for histogram equalization</description>
+  </class>
+
   <!--
       for backward compatibility, can be removed in M-turtle
   -->

src/nodelet/equalize_histogram_nodelet.cpp

@@ -0,0 +1,232 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2020, Ivan Tarifa.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Ivan Tarifa nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+// https://github.com/opencv/opencv/blob/2.4/samples/cpp/tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp
+/**
+ * @function EqualizeHist_Demo.cpp
+ * @brief Demo code for equalizeHist function
+ * @author OpenCV team
+ */
+
+// https://github.com/opencv/opencv/blob/3.4/samples/tapi/clahe.cpp
+/**
+ * @function clahe.cpp
+ * @brief Demo TApi code for clahe filter
+ * @author OpenCV team
+ */
+
+#include <ros/ros.h>
+#include "opencv_apps/nodelet.h"
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+
+#if CV_MAJOR_VERSION >= 3
+#include "opencv2/core/ocl.hpp"
+#else  // OpenCV Version 2 does not have UMat, use Mat instead
+namespace cv
+{
+typedef Mat UMat;
+}
+#endif
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <dynamic_reconfigure/server.h>
+
+#include "opencv_apps/EqualizeHistogramConfig.h"
+
+namespace opencv_apps
+{
+class EqualizeHistogramNodelet : public opencv_apps::Nodelet
+{
+  std::string window_name_;
+  image_transport::Publisher img_pub_;
+  image_transport::Subscriber img_sub_;
+  image_transport::CameraSubscriber cam_sub_;
+
+  cv::Ptr<cv::CLAHE> clahe_;
+
+  boost::shared_ptr<image_transport::ImageTransport> it_;
+
+  typedef opencv_apps::EqualizeHistogramConfig Config;
+  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+
+  Config config_;
+  boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+
+  int queue_size_;
+  bool debug_view_;
+#if CV_MAJOR_VERSION >= 3
+  bool use_opencl_;
+#endif
+
+  cv::Size clahe_tile_size_;
+  double clahe_clip_limit_;
+
+  void reconfigureCallback(Config& new_config, uint32_t level)
+  {
+    config_ = new_config;
+    clahe_tile_size_ = cv::Size(config_.clahe_tile_size_x, config_.clahe_tile_size_y);
+    clahe_clip_limit_ = config_.clahe_clip_limit;
+  }
+
+  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
+  {
+    doWork(msg, cam_info->header.frame_id);
+  }
+
+  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    doWork(msg, msg->header.frame_id);
+  }
+
+  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
+  {
+    try
+    {
+      // Convert the image into something opencv can handle.
+      cv::UMat frame;
+#if CV_MAJOR_VERSION >= 3
+      if (msg->encoding == sensor_msgs::image_encodings::BGR8)
+        frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image.getUMat(cv::ACCESS_RW);
+      else if (msg->encoding == sensor_msgs::image_encodings::MONO8)
+        frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8)->image.getUMat(cv::ACCESS_RW);
+#else
+      if (msg->encoding == sensor_msgs::image_encodings::BGR8)
+        frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
+      else if (msg->encoding == sensor_msgs::image_encodings::MONO8)
+        frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8)->image;
+#endif
+
+      if (debug_view_)
+      {
+        cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
+      }
+
+      // Do the work
+      cv::UMat gray, dst;
+      if (frame.channels() > 1)
+      {
+        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
+      }
+      else
+      {
+        frame.copyTo(gray);
+      }
+
+      switch (config_.histogram_equalization_type)
+      {
+        case opencv_apps::EqualizeHistogram_Clahe:
+          if (clahe_ == nullptr)
+            clahe_ = cv::createCLAHE();
+          clahe_->setTilesGridSize(clahe_tile_size_);
+          clahe_->setClipLimit(clahe_clip_limit_);
+          clahe_->apply(gray, dst);
+          break;
+        case opencv_apps::EqualizeHistogram_EqualizeHist:
+          equalizeHist(gray, dst);
+          break;
+      }
+
+      //-- Show what you got
+      if (debug_view_)
+      {
+        cv::imshow(window_name_, dst);
+        int c = cv::waitKey(1);
+      }
+
+      // Publish the image.
+#if CV_MAJOR_VERSION >= 3
+      sensor_msgs::Image::Ptr out_img =
+          cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, dst.getMat(cv::ACCESS_READ)).toImageMsg();
+#else
+      sensor_msgs::Image::Ptr out_img =
+          cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, dst).toImageMsg();
+#endif
+      out_img->header.frame_id = input_frame_from_msg;
+      img_pub_.publish(out_img);
+    }
+    catch (cv::Exception& e)
+    {
+      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
+    }
+  }
+
+  void subscribe()  // NOLINT(modernize-use-override)
+  {
+    NODELET_DEBUG("Subscribing to image topic.");
+    if (config_.use_camera_info)
+      cam_sub_ = it_->subscribeCamera("image", queue_size_, &EqualizeHistogramNodelet::imageCallbackWithInfo, this);
+    else
+      img_sub_ = it_->subscribe("image", queue_size_, &EqualizeHistogramNodelet::imageCallback, this);
+  }
+
+  void unsubscribe()  // NOLINT(modernize-use-override)
+  {
+    NODELET_DEBUG("Unsubscribing from image topic.");
+    img_sub_.shutdown();
+    cam_sub_.shutdown();
+  }
+
+  void onInit()  // NOLINT(modernize-use-override)
+  {
+    Nodelet::onInit();
+    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
+    pnh_->param("queue_size", queue_size_, 3);
+    pnh_->param("debug_view", debug_view_, false);
+#if CV_MAJOR_VERSION >= 3
+    pnh_->param("use_opencl", use_opencl_, true);
+#endif
+
+    window_name_ = "Equalize Histogram Window (" + ros::this_node::getName() + ")";
+
+    reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
+    dynamic_reconfigure::Server<Config>::CallbackType f = boost::bind(
+        &EqualizeHistogramNodelet::reconfigureCallback, this, boost::placeholders::_1, boost::placeholders::_2);
+    reconfigure_server_->setCallback(f);
+
+    img_pub_ = advertiseImage(*pnh_, "image", 1);
+
+#if CV_MAJOR_VERSION >= 3
+    cv::ocl::setUseOpenCL(use_opencl_);
+#endif
+
+    onInitPostProcess();
+  }
+};
+}  // namespace opencv_apps
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(opencv_apps::EqualizeHistogramNodelet, nodelet::Nodelet);

test/CMakeLists.txt

@@ -56,6 +56,7 @@ add_rostest(test-hls_color_filter.test ARGS gui:=false)
 add_rostest(test-hsv_color_filter.test ARGS gui:=false)
 add_rostest(test-lab_color_filter.test ARGS gui:=false)
 add_rostest(test-edge_detection.test ARGS gui:=false)
+add_rostest(test-equalize_histogram.test ARGS gui:=false)
 
 add_rostest(test-hough_lines.test ARGS gui:=false)
 add_rostest(test-hough_circles.test ARGS gui:=false)
@@ -108,5 +109,3 @@ else()
     roslaunch_add_file_check(${LAUNCH_FILE})
   endforeach()
 endif()
-
-

test/test-blob-extraction.test

@@ -83,7 +83,7 @@
     <param name="blob_extraction_test/topic" value="blob_extraction/ellipses" />
     <test test-name="blob_extraction_test" pkg="rostest" type="hztest" name="blob_extraction_test" >
       <param name="hz" value="30" />
-      <param name="hzerror" value="10.0" />
+      <param name="hzerror" value="15.0" />
       <param name="test_duration" value="1.0" />
     </test>
   </group>