From 7f8e6c72620fe051abc2d70ded9fa5b1d6ef88a4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Tue, 14 Apr 2020 15:27:30 +0200
Subject: [PATCH 1/5] Add the intensity histogram app
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 CMakeLists.txt                      |   2 +-
 launch/intensity_histogram.launch   |  16 ++
 nodelet_plugins.xml                 |   4 +
 src/nodelet/intensity_histogram.cpp | 221 ++++++++++++++++++++++++++++
 4 files changed, 242 insertions(+), 1 deletion(-)
 create mode 100644 launch/intensity_histogram.launch
 create mode 100644 src/nodelet/intensity_histogram.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e4f8f6ff..4bdd4a53 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -263,7 +263,7 @@ opencv_apps_add_nodelet(camshift src/nodelet/camshift_nodelet.cpp) # ./camshiftd
 # ./create_mask.cpp
 # ./dbt_face_detection.cpp
 # ./delaunay2.cpp
-# ./demhist.cpp
+opencv_apps_add_nodelet(intensity_histogram src/nodelet/intensity_histogram.cpp) # ./demhist.cpp
 # ./detect_blob.cpp
 # ./detect_mser.cpp
 # ./dft.cpp
diff --git a/launch/intensity_histogram.launch b/launch/intensity_histogram.launch
new file mode 100644
index 00000000..9eb8185c
--- /dev/null
+++ b/launch/intensity_histogram.launch
@@ -0,0 +1,16 @@
+<launch>
+  <arg name="node_name" default="intensity_histogram" />
+
+  <arg name="image" default="image" doc="The image topic. Should be remapped to the name of the real image topic." />
+
+  <arg name="debug_view" default="true" doc="Specify whether the node displays a window to show edge image" />
+  <arg name="queue_size" default="3" doc="Specigy queue_size of input image subscribers" />
+
+
+  <!-- intensity_histogram.cpp  -->
+  <node name="$(arg node_name)" pkg="opencv_apps" type="intensity_histogram" >
+    <remap from="image" to="$(arg image)" />
+    <param name="debug_view" value="$(arg debug_view)" />
+    <param name="queue_size" value="$(arg queue_size)" />
+  </node>
+</launch>
diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
index 62f76e3d..9620bb73 100644
--- a/nodelet_plugins.xml
+++ b/nodelet_plugins.xml
@@ -127,6 +127,10 @@
     <description>Nodelet for histogram equalization</description>
   </class>
 
+   <class name="opencv_apps/intensity_histogram" type="opencv_apps::IntensityHistogramNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet of intensity histogram</description>
+  </class>
+
   <!--
       for backward compatibility, can be removed in M-turtle
   -->
diff --git a/src/nodelet/intensity_histogram.cpp b/src/nodelet/intensity_histogram.cpp
new file mode 100644
index 00000000..b8f0df91
--- /dev/null
+++ b/src/nodelet/intensity_histogram.cpp
@@ -0,0 +1,221 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2020, Gaël Écorchard, Czech Technical University.
+*  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 Gaël Écorchard 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.
+*********************************************************************/
+/**
+ * Compute the histogram of intensities and publish it as an image.
+ */
+
+#include <vector>
+
+#include <cv_bridge/cv_bridge.h>
+//#include <dynamic_reconfigure/server.h>
+#include <image_transport/image_transport.h>
+#include <opencv_apps/nodelet.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+namespace opencv_apps
+{
+
+namespace intensity_histogram
+{
+
+static const std::string OPENCV_WINDOW = "Image histogram";
+
+cv::Mat grayHistogram(cv_bridge::CvImageConstPtr img)
+{
+  /* Inspired by https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
+  constexpr bool uniform = true;
+  constexpr bool accumulate = false;
+  constexpr int bin_count = 256;
+
+  float range[] = {0, 256}; //the upper boundary is exclusive
+  const float* hist_range = {range};
+  cv::Mat intensity_hist;
+  cv::calcHist(&img->image, 1, 0, cv::Mat(), intensity_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  int hist_w = 512;
+  int hist_h = 400;
+  int bin_w = cvRound(static_cast<double>(hist_w / bin_count));
+  cv::Mat hist_image(hist_h, hist_w, CV_8UC1, cv::Scalar(0));
+  cv::normalize(intensity_hist, intensity_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
+  for (unsigned int i = 1; i < bin_count; i++)
+  {
+    cv::line(hist_image,
+        cv::Point(bin_w * (i-1), hist_h - cvRound(intensity_hist.at<float>(i-1))),
+        cv::Point(bin_w * (i), hist_h - cvRound(intensity_hist.at<float>(i))),
+        cv::Scalar(255), 2, 8, 0);
+  }
+  return hist_image;
+}
+
+cv::Mat bgrHistogram(cv_bridge::CvImageConstPtr img)
+{
+  /* Inspired by https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
+  constexpr bool uniform = true;
+  constexpr bool accumulate = false;
+  constexpr int bin_count = 256;
+
+  std::vector<cv::Mat> bgr_planes;
+  cv::split(img->image, bgr_planes);
+
+  float range[] = {0, 256}; //the upper boundary is exclusive
+  const float* hist_range = {range};
+  cv::Mat b_hist;
+  cv::Mat g_hist;
+  cv::Mat r_hist;
+  cv::calcHist(&bgr_planes[0], 1, 0, cv::Mat(), b_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  cv::calcHist(&bgr_planes[1], 1, 0, cv::Mat(), g_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  cv::calcHist(&bgr_planes[2], 1, 0, cv::Mat(), r_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  int hist_w = 512;
+  int hist_h = 400;
+  int bin_w = cvRound(static_cast<double>(hist_w / bin_count));
+  cv::Mat hist_image(hist_h, hist_w, CV_8UC3, cv::Scalar(0, 0, 0));
+  cv::normalize(b_hist, b_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
+  cv::normalize(g_hist, g_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
+  cv::normalize(r_hist, r_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
+  for (unsigned int i = 1; i < bin_count; i++)
+  {
+    cv::line(hist_image,
+        cv::Point(bin_w * (i-1), hist_h - cvRound(b_hist.at<float>(i-1))),
+        cv::Point(bin_w * (i), hist_h - cvRound(b_hist.at<float>(i))),
+        cv::Scalar(255, 0, 0), 2, 8, 0);
+    cv::line(hist_image,
+        cv::Point(bin_w * (i-1), hist_h - cvRound(g_hist.at<float>(i-1))),
+        cv::Point(bin_w * (i), hist_h - cvRound(g_hist.at<float>(i))),
+        cv::Scalar(0, 255, 0), 2, 8, 0);
+    cv::line(hist_image,
+        cv::Point(bin_w * (i-1), hist_h - cvRound(r_hist.at<float>(i-1))),
+        cv::Point(bin_w * (i), hist_h - cvRound(r_hist.at<float>(i))),
+        cv::Scalar(0, 0, 255), 2, 8, 0);
+  }
+  return hist_image;
+}
+
+class IntensityHistogram
+{
+
+  public:
+
+    IntensityHistogram() :
+      it_(nh_)
+    {
+      image_sub_ = it_.subscribe("image", queue_size_, &IntensityHistogram::imageCallback, this);
+      hist_pub_ = it_.advertise("image_hist", 1);
+
+      ros::NodeHandle private_nh("~");
+      private_nh.param("queue_size", queue_size_, 1);
+      private_nh.param("debug_view", debug_view_, false);
+      if (debug_view_)
+      {
+        cv::namedWindow(OPENCV_WINDOW);
+      }
+    }
+
+    ~IntensityHistogram()
+    {
+      if (debug_view_)
+      {
+        cv::destroyWindow(OPENCV_WINDOW);
+      }
+    }
+
+    void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+    {
+      cv_bridge::CvImageConstPtr cv_ptr;
+      try
+      {
+        cv_ptr = cv_bridge::toCvShare(msg);
+      }
+      catch (cv_bridge::Exception& e)
+      {
+        ROS_ERROR("cv_bridge exception: %s", e.what());
+        return;
+      }
+
+      cv::Mat out_img;
+      sensor_msgs::Image::Ptr out_img_msg;
+      if (cv_ptr->image.channels() == 1)
+      {
+        out_img = grayHistogram(cv_ptr);
+        out_img_msg = cv_bridge::CvImage(
+          msg->header, sensor_msgs::image_encodings::MONO8, out_img).toImageMsg();
+
+      }
+      else
+      {
+        out_img = bgrHistogram(cv_ptr);
+        out_img_msg = cv_bridge::CvImage(
+          msg->header, sensor_msgs::image_encodings::RGB8, out_img).toImageMsg();
+      }
+
+      if (debug_view_)
+      {
+        // Update GUI Window
+        cv::imshow(OPENCV_WINDOW, out_img);
+        cv::waitKey(3);
+      }
+
+      hist_pub_.publish(out_img_msg);
+    }
+
+  private:
+
+    ros::NodeHandle nh_;
+    image_transport::ImageTransport it_;
+    image_transport::Subscriber image_sub_;
+    image_transport::Publisher hist_pub_;
+    int queue_size_;
+    bool debug_view_;
+};
+
+}  /* namespace intensity_histogram. */
+
+class IntensityHistogramNodelet : public nodelet::Nodelet
+{
+
+  public:
+
+    virtual void onInit()  // NOLINT(modernize-use-override)
+    {
+      intensity_histogram::IntensityHistogram ih;
+      ros::spin();
+    }
+};
+
+}  /* namespace opencv_apps. */
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(opencv_apps::IntensityHistogramNodelet, nodelet::Nodelet);

From 5c2b2253cb6b30d0422915debd2d52fe9e0444c8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Wed, 15 Apr 2020 14:46:42 +0200
Subject: [PATCH 2/5] Try to satisfy clang-format and clang-tidy
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 src/nodelet/intensity_histogram.cpp | 177 ++++++++++++++--------------
 1 file changed, 90 insertions(+), 87 deletions(-)

diff --git a/src/nodelet/intensity_histogram.cpp b/src/nodelet/intensity_histogram.cpp
index b8f0df91..567aa697 100644
--- a/src/nodelet/intensity_histogram.cpp
+++ b/src/nodelet/intensity_histogram.cpp
@@ -55,17 +55,18 @@ namespace intensity_histogram
 
 static const std::string OPENCV_WINDOW = "Image histogram";
 
-cv::Mat grayHistogram(cv_bridge::CvImageConstPtr img)
+cv::Mat grayHistogram(const cv_bridge::CvImageConstPtr& img)
 {
-  /* Inspired by https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
+  /* Inspired by
+   * https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
   constexpr bool uniform = true;
   constexpr bool accumulate = false;
   constexpr int bin_count = 256;
 
-  float range[] = {0, 256}; //the upper boundary is exclusive
-  const float* hist_range = {range};
+  float range[] = { 0, 256 }; //the upper boundary is exclusive
+  const float* hist_range = { range };
   cv::Mat intensity_hist;
-  cv::calcHist(&img->image, 1, 0, cv::Mat(), intensity_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  cv::calcHist(&img->image, 1, nullptr, cv::Mat(), intensity_hist, 1, &bin_count, &hist_range, uniform, accumulate);
   int hist_w = 512;
   int hist_h = 400;
   int bin_w = cvRound(static_cast<double>(hist_w / bin_count));
@@ -74,16 +75,17 @@ cv::Mat grayHistogram(cv_bridge::CvImageConstPtr img)
   for (unsigned int i = 1; i < bin_count; i++)
   {
     cv::line(hist_image,
-        cv::Point(bin_w * (i-1), hist_h - cvRound(intensity_hist.at<float>(i-1))),
-        cv::Point(bin_w * (i), hist_h - cvRound(intensity_hist.at<float>(i))),
+        cv::Point(bin_w * (i - 1), hist_h - cvRound(intensity_hist.at<float>(i - 1))),
+        cv::Point(bin_w * i, hist_h - cvRound(intensity_hist.at<float>(i))),
         cv::Scalar(255), 2, 8, 0);
   }
   return hist_image;
 }
 
-cv::Mat bgrHistogram(cv_bridge::CvImageConstPtr img)
+cv::Mat bgrHistogram(const cv_bridge::CvImageConstPtr& img)
 {
-  /* Inspired by https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
+  /* Inspired by
+   * https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
   constexpr bool uniform = true;
   constexpr bool accumulate = false;
   constexpr int bin_count = 256;
@@ -96,9 +98,9 @@ cv::Mat bgrHistogram(cv_bridge::CvImageConstPtr img)
   cv::Mat b_hist;
   cv::Mat g_hist;
   cv::Mat r_hist;
-  cv::calcHist(&bgr_planes[0], 1, 0, cv::Mat(), b_hist, 1, &bin_count, &hist_range, uniform, accumulate);
-  cv::calcHist(&bgr_planes[1], 1, 0, cv::Mat(), g_hist, 1, &bin_count, &hist_range, uniform, accumulate);
-  cv::calcHist(&bgr_planes[2], 1, 0, cv::Mat(), r_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  cv::calcHist(&bgr_planes[0], 1, nullptr, cv::Mat(), b_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  cv::calcHist(&bgr_planes[1], 1, nullptr, cv::Mat(), g_hist, 1, &bin_count, &hist_range, uniform, accumulate);
+  cv::calcHist(&bgr_planes[2], 1, nullptr, cv::Mat(), r_hist, 1, &bin_count, &hist_range, uniform, accumulate);
   int hist_w = 512;
   int hist_h = 400;
   int bin_w = cvRound(static_cast<double>(hist_w / bin_count));
@@ -109,16 +111,16 @@ cv::Mat bgrHistogram(cv_bridge::CvImageConstPtr img)
   for (unsigned int i = 1; i < bin_count; i++)
   {
     cv::line(hist_image,
-        cv::Point(bin_w * (i-1), hist_h - cvRound(b_hist.at<float>(i-1))),
-        cv::Point(bin_w * (i), hist_h - cvRound(b_hist.at<float>(i))),
+        cv::Point(bin_w * (i - 1), hist_h - cvRound(b_hist.at<float>(i - 1))),
+        cv::Point(bin_w * i, hist_h - cvRound(b_hist.at<float>(i))),
         cv::Scalar(255, 0, 0), 2, 8, 0);
     cv::line(hist_image,
-        cv::Point(bin_w * (i-1), hist_h - cvRound(g_hist.at<float>(i-1))),
-        cv::Point(bin_w * (i), hist_h - cvRound(g_hist.at<float>(i))),
+        cv::Point(bin_w * (i - 1), hist_h - cvRound(g_hist.at<float>(i - 1))),
+        cv::Point(bin_w * i, hist_h - cvRound(g_hist.at<float>(i))),
         cv::Scalar(0, 255, 0), 2, 8, 0);
     cv::line(hist_image,
-        cv::Point(bin_w * (i-1), hist_h - cvRound(r_hist.at<float>(i-1))),
-        cv::Point(bin_w * (i), hist_h - cvRound(r_hist.at<float>(i))),
+        cv::Point(bin_w * (i - 1), hist_h - cvRound(r_hist.at<float>(i - 1))),
+        cv::Point(bin_w * i, hist_h - cvRound(r_hist.at<float>(i))),
         cv::Scalar(0, 0, 255), 2, 8, 0);
   }
   return hist_image;
@@ -127,95 +129,96 @@ cv::Mat bgrHistogram(cv_bridge::CvImageConstPtr img)
 class IntensityHistogram
 {
 
-  public:
+public:
+
+  IntensityHistogram() :
+    it_(nh_)
+  {
+    ros::NodeHandle private_nh("~");
+    private_nh.param("queue_size", queue_size_, 1);
+    private_nh.param("debug_view", debug_view_, false);
+
+    image_sub_ = it_.subscribe("image", queue_size_, &IntensityHistogram::imageCallback, this);
+    hist_pub_ = it_.advertise("image_hist", queue_size_);
+
+    if (debug_view_)
+    {
+      cv::namedWindow(OPENCV_WINDOW);
+    }
+  }
+
+  ~IntensityHistogram()
+  {
+    if (debug_view_)
+    {
+      cv::destroyWindow(OPENCV_WINDOW);
+    }
+  }
 
-    IntensityHistogram() :
-      it_(nh_)
+  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    cv_bridge::CvImageConstPtr cv_ptr;
+    try
+    {
+      cv_ptr = cv_bridge::toCvShare(msg);
+    }
+    catch (cv_bridge::Exception& e)
     {
-      image_sub_ = it_.subscribe("image", queue_size_, &IntensityHistogram::imageCallback, this);
-      hist_pub_ = it_.advertise("image_hist", 1);
-
-      ros::NodeHandle private_nh("~");
-      private_nh.param("queue_size", queue_size_, 1);
-      private_nh.param("debug_view", debug_view_, false);
-      if (debug_view_)
-      {
-        cv::namedWindow(OPENCV_WINDOW);
-      }
+      ROS_ERROR("cv_bridge exception: %s", e.what());
+      return;
     }
 
-    ~IntensityHistogram()
+    cv::Mat out_img;
+    sensor_msgs::Image::Ptr out_img_msg;
+    if (cv_ptr->image.channels() == 1)
+    {
+      out_img = grayHistogram(cv_ptr);
+      out_img_msg = cv_bridge::CvImage(
+        msg->header, sensor_msgs::image_encodings::MONO8, out_img).toImageMsg();
+
+    }
+    else
     {
-      if (debug_view_)
-      {
-        cv::destroyWindow(OPENCV_WINDOW);
-      }
+      out_img = bgrHistogram(cv_ptr);
+      out_img_msg = cv_bridge::CvImage(
+        msg->header, sensor_msgs::image_encodings::RGB8, out_img).toImageMsg();
     }
 
-    void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+    if (debug_view_)
     {
-      cv_bridge::CvImageConstPtr cv_ptr;
-      try
-      {
-        cv_ptr = cv_bridge::toCvShare(msg);
-      }
-      catch (cv_bridge::Exception& e)
-      {
-        ROS_ERROR("cv_bridge exception: %s", e.what());
-        return;
-      }
-
-      cv::Mat out_img;
-      sensor_msgs::Image::Ptr out_img_msg;
-      if (cv_ptr->image.channels() == 1)
-      {
-        out_img = grayHistogram(cv_ptr);
-        out_img_msg = cv_bridge::CvImage(
-          msg->header, sensor_msgs::image_encodings::MONO8, out_img).toImageMsg();
-
-      }
-      else
-      {
-        out_img = bgrHistogram(cv_ptr);
-        out_img_msg = cv_bridge::CvImage(
-          msg->header, sensor_msgs::image_encodings::RGB8, out_img).toImageMsg();
-      }
-
-      if (debug_view_)
-      {
-        // Update GUI Window
-        cv::imshow(OPENCV_WINDOW, out_img);
-        cv::waitKey(3);
-      }
-
-      hist_pub_.publish(out_img_msg);
+      // Update GUI Window
+      cv::imshow(OPENCV_WINDOW, out_img);
+      cv::waitKey(3);
     }
 
-  private:
+    hist_pub_.publish(out_img_msg);
+  }
+
+private:
 
-    ros::NodeHandle nh_;
-    image_transport::ImageTransport it_;
-    image_transport::Subscriber image_sub_;
-    image_transport::Publisher hist_pub_;
-    int queue_size_;
-    bool debug_view_;
+  ros::NodeHandle nh_;
+  image_transport::ImageTransport it_;
+  image_transport::Subscriber image_sub_;
+  image_transport::Publisher hist_pub_;
+  int queue_size_;
+  bool debug_view_;
 };
 
-}  /* namespace intensity_histogram. */
+} /* namespace intensity_histogram. */
 
 class IntensityHistogramNodelet : public nodelet::Nodelet
 {
 
-  public:
+public:
 
-    virtual void onInit()  // NOLINT(modernize-use-override)
-    {
-      intensity_histogram::IntensityHistogram ih;
-      ros::spin();
-    }
+  virtual void onInit()  // NOLINT(modernize-use-override)
+  {
+    intensity_histogram::IntensityHistogram ih;
+    ros::spin();
+  }
 };
 
-}  /* namespace opencv_apps. */
+} /* namespace opencv_apps. */
 
 #include <pluginlib/class_list_macros.h>
 PLUGINLIB_EXPORT_CLASS(opencv_apps::IntensityHistogramNodelet, nodelet::Nodelet);

From 44d39941c56d4595be2684e39e1c1622cc18dfbd Mon Sep 17 00:00:00 2001
From: Kei Okada <kei.okada@gmail.com>
Date: Mon, 14 Feb 2022 16:07:31 +0900
Subject: [PATCH 3/5] add CalcHist, CompareHist example, rewrited from #104

---
 CMakeLists.txt                                |   5 +-
 cfg/CompareHistogram.cfg                      |  53 ++++
 ...togram.launch => compare_histogram.launch} |   8 +-
 nodelet_plugins.xml                           |   8 +-
 src/nodelet/compare_histogram_nodelet.cpp     | 299 ++++++++++++++++++
 src/nodelet/intensity_histogram.cpp           | 224 -------------
 test/CMakeLists.txt                           |   1 +
 test/test-compare_histogram.test              |  66 ++++
 8 files changed, 433 insertions(+), 231 deletions(-)
 create mode 100755 cfg/CompareHistogram.cfg
 rename launch/{intensity_histogram.launch => compare_histogram.launch} (59%)
 create mode 100644 src/nodelet/compare_histogram_nodelet.cpp
 delete mode 100644 src/nodelet/intensity_histogram.cpp
 create mode 100644 test/test-compare_histogram.test

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4bdd4a53..ce3e3e2d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,7 @@ generate_dynamic_reconfigure_options(
   cfg/GoodfeatureTrack.cfg
   cfg/CornerHarris.cfg
   #
+  cfg/CompareHistogram.cfg
   cfg/EqualizeHistogram.cfg
   cfg/CamShift.cfg
   cfg/FBackFlow.cfg
@@ -169,7 +170,7 @@ opencv_apps_add_nodelet(discrete_fourier_transform src/nodelet/discrete_fourier_
   # ./tutorial_code/Histograms_Matching/calcBackProject_Demo1.cpp
   # ./tutorial_code/Histograms_Matching/calcBackProject_Demo2.cpp
   # ./tutorial_code/Histograms_Matching/calcHist_Demo.cpp
-  #  ./tutorial_code/Histograms_Matching/compareHist_Demo.cpp
+  opencv_apps_add_nodelet(compare_histogram src/nodelet/compare_histogram_nodelet.cpp)  #  ./tutorial_code/Histograms_Matching/compareHist_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
 
@@ -263,7 +264,7 @@ opencv_apps_add_nodelet(camshift src/nodelet/camshift_nodelet.cpp) # ./camshiftd
 # ./create_mask.cpp
 # ./dbt_face_detection.cpp
 # ./delaunay2.cpp
-opencv_apps_add_nodelet(intensity_histogram src/nodelet/intensity_histogram.cpp) # ./demhist.cpp
+# ./demhist.cpp
 # ./detect_blob.cpp
 # ./detect_mser.cpp
 # ./dft.cpp
diff --git a/cfg/CompareHistogram.cfg b/cfg/CompareHistogram.cfg
new file mode 100755
index 00000000..769ffc72
--- /dev/null
+++ b/cfg/CompareHistogram.cfg
@@ -0,0 +1,53 @@
+#! /usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2022, Kei Okada
+# 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_comparison_type = gen.enum([ gen.const("Correlation", int_t, 0, "Correlation"),
+                                       gen.const("Chi_Square",  int_t, 1, "Chi-Square"),
+                                       gen.const("Intersection", int_t, 2, "Intersection"),
+                                       gen.const("Bhattacharyya",  int_t, 3, "Bhattacharyya distance")],
+                                     "Histogram Comparison method")
+gen.add("histogram_comparison_type", int_t, 1, "Histogram Comparison method", 0, 0, 3, edit_method=histogram_comparison_type)
+
+gen.add("histogram_size", int_t,    2, "Array of histogram sizes in each dimension", 256, 1, 1024)
+gen.add("uniform", bool_t,    3, "Flag indicating whether the histogram is uniform or not", True)
+gen.add("accumulate", bool_t,    4, "Accumulation flag. If it is set, the histogram is not cleared in the beginning when it is allocated", False)
+
+exit(gen.generate(PACKAGE, "compare_histogram", "CompareHistogram"))
diff --git a/launch/intensity_histogram.launch b/launch/compare_histogram.launch
similarity index 59%
rename from launch/intensity_histogram.launch
rename to launch/compare_histogram.launch
index 9eb8185c..3b729369 100644
--- a/launch/intensity_histogram.launch
+++ b/launch/compare_histogram.launch
@@ -5,12 +5,18 @@
 
   <arg name="debug_view" default="true" doc="Specify whether the node displays a window to show edge image" />
   <arg name="queue_size" default="3" doc="Specigy queue_size of input image subscribers" />
+  <arg name="histogram_size" default="64" doc="number of bins" />
+  <arg name="reference_histogram" default="" doc="reference of histogram comparison" />
 
 
   <!-- intensity_histogram.cpp  -->
-  <node name="$(arg node_name)" pkg="opencv_apps" type="intensity_histogram" >
+  <node name="$(arg node_name)" pkg="opencv_apps" type="compare_histogram" >
     <remap from="image" to="$(arg image)" />
     <param name="debug_view" value="$(arg debug_view)" />
     <param name="queue_size" value="$(arg queue_size)" />
+    <param name="histogram_size" value="$(arg histogram_size)" />
+    <rosparam param="reference_histogram" subst_value="true">
+      $(arg reference_histogram)
+    </rosparam>
   </node>
 </launch>
diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
index 9620bb73..430ac892 100644
--- a/nodelet_plugins.xml
+++ b/nodelet_plugins.xml
@@ -123,12 +123,12 @@
     <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 name="opencv_apps/compare_histogram" type="opencv_apps::CompareHistogramNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet of compare histogram</description>
   </class>
 
-   <class name="opencv_apps/intensity_histogram" type="opencv_apps::IntensityHistogramNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet of intensity histogram</description>
+  <class name="opencv_apps/equalize_histogram" type="opencv_apps::EqualizeHistogramNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet for histogram equalization</description>
   </class>
 
   <!--
diff --git a/src/nodelet/compare_histogram_nodelet.cpp b/src/nodelet/compare_histogram_nodelet.cpp
new file mode 100644
index 00000000..e862f0f1
--- /dev/null
+++ b/src/nodelet/compare_histogram_nodelet.cpp
@@ -0,0 +1,299 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2020, Gaël Écorchard, Czech Technical University.
+*  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 Gaël Écorchard 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.
+*********************************************************************/
+/**
+ * Compute the histogram of intensities and publish it as an image.
+ */
+
+// https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp
+/**
+ * @function calcHist_Demo.cpp
+ * @brief Demo code to use the function calcHist
+ * @author OpenCV team
+ */
+
+// https://github.com/opencv/opencv/blob/3.4/samples/cpp/tutorial_code/Histograms_Matching/compareHist_Demo.cpp
+/**
+ * @file compareHist_Demo.cpp
+ * @brief Sample code to use the function compareHist
+ * @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 <std_msgs/Float64MultiArray.h>
+#include <std_msgs/Float64.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <dynamic_reconfigure/server.h>
+
+#include "opencv_apps/CompareHistogramConfig.h"
+
+namespace opencv_apps
+{
+class CompareHistogramNodelet : public opencv_apps::Nodelet
+{
+  std::string window_name_;
+  image_transport::Publisher img_pub_;
+  image_transport::Subscriber img_sub_;
+  image_transport::CameraSubscriber cam_sub_;
+  ros::Publisher msg_pub_, distance_pub_;
+
+  boost::shared_ptr<image_transport::ImageTransport> it_;
+
+  typedef opencv_apps::CompareHistogramConfig Config;
+  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+
+  Config config_;
+  boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+
+  int queue_size_;
+  bool debug_view_;
+  int histogram_size_;
+  bool uniform_;
+  bool accumulate_;
+  std::vector<cv::Mat> reference_histogram_;
+
+  void reconfigureCallback(Config& new_config, uint32_t level)
+  {
+    config_ = new_config;
+    histogram_size_ = config_.histogram_size;
+    uniform_ = config_.uniform;
+    accumulate_ = config_.accumulate;
+  }
+
+  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::Mat frame;
+      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;
+
+      if (debug_view_)
+      {
+        cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
+      }
+
+      //  Draw contours
+      cv::Mat drawing;
+      bool publish_drawing = (img_pub_.getNumSubscribers() > 0);
+
+      //  Separate the image
+      std::vector<cv::Mat> planes;
+      cv::split(frame, planes);
+
+      //  Set the range
+      float range[] = { 0, 256 };  // the upper boundary is exclusive
+      const float* hist_range[] = { range };
+
+      // Compute the histograms
+      std::vector<cv::Mat> hist;
+      hist.resize(planes.size());
+      for (unsigned int j = 0; j < hist.size(); j++)
+      {
+        cv::calcHist(&planes[j], 1, nullptr, cv::Mat(), hist[j], 1, &histogram_size_, hist_range, uniform_, accumulate_);
+      }
+
+      // Publishe the results
+      std_msgs::Float64MultiArray hist_msg;
+      hist_msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
+      hist_msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
+      hist_msg.layout.dim[0].size = hist.size();
+      hist_msg.layout.dim[0].stride = hist.size() * histogram_size_;
+      hist_msg.layout.dim[1].size = histogram_size_;
+      hist_msg.layout.dim[1].stride = histogram_size_;
+      for (auto& j : hist)
+      {
+        cv::normalize(j, j, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());
+        for (unsigned int i = 0; i < histogram_size_; i++)
+        {
+          hist_msg.data.push_back(j.at<float>(i));
+        }
+      }
+      msg_pub_.publish(hist_msg);
+
+      // Apply the histogram comparison methods
+      if (!reference_histogram_.empty())
+      {
+        int compare_method;
+        switch (config_.histogram_comparison_type)
+        {
+          case opencv_apps::CompareHistogram_Correlation:
+            compare_method = cv::HISTCMP_CORREL;
+            break;
+          case opencv_apps::CompareHistogram_Chi_Square:
+            compare_method = cv::HISTCMP_CHISQR;
+            break;
+          case opencv_apps::CompareHistogram_Intersection:
+            compare_method = cv::HISTCMP_INTERSECT;
+            break;
+          case opencv_apps::CompareHistogram_Bhattacharyya:
+            compare_method = cv::HISTCMP_BHATTACHARYYA;
+            break;
+        }
+        double distance = 1;
+        for (unsigned int j = 0; j < hist.size(); j++)
+        {
+          distance = distance * cv::compareHist(reference_histogram_[j], hist[j], compare_method);
+        }
+        std_msgs::Float64 distance_msg;
+        distance_msg.data = distance;
+        distance_pub_.publish(distance_msg);
+      }
+
+      // Draw the histograms
+      if (debug_view_ || publish_drawing)
+      {
+        int hist_w = 512, hist_h = 400;
+        int bin_w = cvRound((double)hist_w / histogram_size_);
+
+        drawing = cv::Mat(hist_h, hist_w, CV_8UC3, cv::Scalar(0, 0, 0));
+
+        //  Normalize the result to ( 0, drawing.rows )
+        for (auto& j : hist)
+        {
+          cv::normalize(j, j, 0, hist_h, cv::NORM_MINMAX, -1, cv::Mat());
+        }
+
+        //  Draw for each channel
+        for (unsigned int j = 0; j < hist.size(); j++)
+        {
+          for (unsigned int i = 1; i < histogram_size_; i++)
+          {
+            cv::line(drawing, cv::Point(bin_w * (i - 1), hist_h - cvRound(hist[j].at<float>(i - 1))),
+                     cv::Point(bin_w * i, hist_h - cvRound(hist[j].at<float>(i))),
+                     cv::Scalar((j == 2 ? 255 : 0), (j == 1 ? 255 : 0), (j == 0 ? 255 : 0)), 2, 8, 0);
+          }
+        }
+      }
+
+      //-- Show what you got
+      if (debug_view_)
+      {
+        cv::imshow(window_name_, drawing);
+        int c = cv::waitKey(1);
+      }
+
+      // Publish the image.
+      if (publish_drawing)
+      {
+        sensor_msgs::Image::Ptr out_img =
+            cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
+        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_, &CompareHistogramNodelet::imageCallbackWithInfo, this);
+    else
+      img_sub_ = it_->subscribe("image", queue_size_, &CompareHistogramNodelet::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 (pnh_->hasParam("reference_histogram"))
+    {
+      XmlRpc::XmlRpcValue param_val;
+      pnh_->getParam("reference_histogram", param_val);
+      if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray)
+      {
+        XmlRpc::XmlRpcValue param = param_val[0];
+        reference_histogram_.resize(param_val.size());
+        for (int j = 0; j < param_val.size(); j++)
+        {
+          reference_histogram_[j] = cv::Mat::zeros(param_val[0].size(), 1, CV_32F);
+          for (int i = 0; i < param_val[0].size(); i++)
+          {
+            reference_histogram_[j].at<float>(0, i) = static_cast<double>(param_val[j][i]);
+          }
+          ROS_INFO_STREAM("reference_histgram[" << j << "] : " << reference_histogram_[j]);
+        }
+      }
+    }
+
+    window_name_ = "Histogram Window (" + ros::this_node::getName() + ")";
+
+    reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
+    dynamic_reconfigure::Server<Config>::CallbackType f = boost::bind(
+        &CompareHistogramNodelet::reconfigureCallback, this, boost::placeholders::_1, boost::placeholders::_2);
+    reconfigure_server_->setCallback(f);
+
+    img_pub_ = advertiseImage(*pnh_, "image", 1);
+    msg_pub_ = advertise<std_msgs::Float64MultiArray>(*pnh_, "histogram", 1);
+    distance_pub_ = advertise<std_msgs::Float64>(*pnh_, "distance", 1);
+
+    onInitPostProcess();
+  }
+};
+}  // namespace opencv_apps
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(opencv_apps::CompareHistogramNodelet, nodelet::Nodelet);
diff --git a/src/nodelet/intensity_histogram.cpp b/src/nodelet/intensity_histogram.cpp
deleted file mode 100644
index 567aa697..00000000
--- a/src/nodelet/intensity_histogram.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2020, Gaël Écorchard, Czech Technical University.
-*  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 Gaël Écorchard 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.
-*********************************************************************/
-/**
- * Compute the histogram of intensities and publish it as an image.
- */
-
-#include <vector>
-
-#include <cv_bridge/cv_bridge.h>
-//#include <dynamic_reconfigure/server.h>
-#include <image_transport/image_transport.h>
-#include <opencv_apps/nodelet.h>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <ros/ros.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/image_encodings.h>
-
-namespace opencv_apps
-{
-
-namespace intensity_histogram
-{
-
-static const std::string OPENCV_WINDOW = "Image histogram";
-
-cv::Mat grayHistogram(const cv_bridge::CvImageConstPtr& img)
-{
-  /* Inspired by
-   * https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
-  constexpr bool uniform = true;
-  constexpr bool accumulate = false;
-  constexpr int bin_count = 256;
-
-  float range[] = { 0, 256 }; //the upper boundary is exclusive
-  const float* hist_range = { range };
-  cv::Mat intensity_hist;
-  cv::calcHist(&img->image, 1, nullptr, cv::Mat(), intensity_hist, 1, &bin_count, &hist_range, uniform, accumulate);
-  int hist_w = 512;
-  int hist_h = 400;
-  int bin_w = cvRound(static_cast<double>(hist_w / bin_count));
-  cv::Mat hist_image(hist_h, hist_w, CV_8UC1, cv::Scalar(0));
-  cv::normalize(intensity_hist, intensity_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
-  for (unsigned int i = 1; i < bin_count; i++)
-  {
-    cv::line(hist_image,
-        cv::Point(bin_w * (i - 1), hist_h - cvRound(intensity_hist.at<float>(i - 1))),
-        cv::Point(bin_w * i, hist_h - cvRound(intensity_hist.at<float>(i))),
-        cv::Scalar(255), 2, 8, 0);
-  }
-  return hist_image;
-}
-
-cv::Mat bgrHistogram(const cv_bridge::CvImageConstPtr& img)
-{
-  /* Inspired by
-   * https://github.com/opencv/opencv/tree/3.4/samples/cpp/tutorial_code/Histograms_Matching/calcHist_Demo.cpp. */
-  constexpr bool uniform = true;
-  constexpr bool accumulate = false;
-  constexpr int bin_count = 256;
-
-  std::vector<cv::Mat> bgr_planes;
-  cv::split(img->image, bgr_planes);
-
-  float range[] = {0, 256}; //the upper boundary is exclusive
-  const float* hist_range = {range};
-  cv::Mat b_hist;
-  cv::Mat g_hist;
-  cv::Mat r_hist;
-  cv::calcHist(&bgr_planes[0], 1, nullptr, cv::Mat(), b_hist, 1, &bin_count, &hist_range, uniform, accumulate);
-  cv::calcHist(&bgr_planes[1], 1, nullptr, cv::Mat(), g_hist, 1, &bin_count, &hist_range, uniform, accumulate);
-  cv::calcHist(&bgr_planes[2], 1, nullptr, cv::Mat(), r_hist, 1, &bin_count, &hist_range, uniform, accumulate);
-  int hist_w = 512;
-  int hist_h = 400;
-  int bin_w = cvRound(static_cast<double>(hist_w / bin_count));
-  cv::Mat hist_image(hist_h, hist_w, CV_8UC3, cv::Scalar(0, 0, 0));
-  cv::normalize(b_hist, b_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
-  cv::normalize(g_hist, g_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
-  cv::normalize(r_hist, r_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat());
-  for (unsigned int i = 1; i < bin_count; i++)
-  {
-    cv::line(hist_image,
-        cv::Point(bin_w * (i - 1), hist_h - cvRound(b_hist.at<float>(i - 1))),
-        cv::Point(bin_w * i, hist_h - cvRound(b_hist.at<float>(i))),
-        cv::Scalar(255, 0, 0), 2, 8, 0);
-    cv::line(hist_image,
-        cv::Point(bin_w * (i - 1), hist_h - cvRound(g_hist.at<float>(i - 1))),
-        cv::Point(bin_w * i, hist_h - cvRound(g_hist.at<float>(i))),
-        cv::Scalar(0, 255, 0), 2, 8, 0);
-    cv::line(hist_image,
-        cv::Point(bin_w * (i - 1), hist_h - cvRound(r_hist.at<float>(i - 1))),
-        cv::Point(bin_w * i, hist_h - cvRound(r_hist.at<float>(i))),
-        cv::Scalar(0, 0, 255), 2, 8, 0);
-  }
-  return hist_image;
-}
-
-class IntensityHistogram
-{
-
-public:
-
-  IntensityHistogram() :
-    it_(nh_)
-  {
-    ros::NodeHandle private_nh("~");
-    private_nh.param("queue_size", queue_size_, 1);
-    private_nh.param("debug_view", debug_view_, false);
-
-    image_sub_ = it_.subscribe("image", queue_size_, &IntensityHistogram::imageCallback, this);
-    hist_pub_ = it_.advertise("image_hist", queue_size_);
-
-    if (debug_view_)
-    {
-      cv::namedWindow(OPENCV_WINDOW);
-    }
-  }
-
-  ~IntensityHistogram()
-  {
-    if (debug_view_)
-    {
-      cv::destroyWindow(OPENCV_WINDOW);
-    }
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    cv_bridge::CvImageConstPtr cv_ptr;
-    try
-    {
-      cv_ptr = cv_bridge::toCvShare(msg);
-    }
-    catch (cv_bridge::Exception& e)
-    {
-      ROS_ERROR("cv_bridge exception: %s", e.what());
-      return;
-    }
-
-    cv::Mat out_img;
-    sensor_msgs::Image::Ptr out_img_msg;
-    if (cv_ptr->image.channels() == 1)
-    {
-      out_img = grayHistogram(cv_ptr);
-      out_img_msg = cv_bridge::CvImage(
-        msg->header, sensor_msgs::image_encodings::MONO8, out_img).toImageMsg();
-
-    }
-    else
-    {
-      out_img = bgrHistogram(cv_ptr);
-      out_img_msg = cv_bridge::CvImage(
-        msg->header, sensor_msgs::image_encodings::RGB8, out_img).toImageMsg();
-    }
-
-    if (debug_view_)
-    {
-      // Update GUI Window
-      cv::imshow(OPENCV_WINDOW, out_img);
-      cv::waitKey(3);
-    }
-
-    hist_pub_.publish(out_img_msg);
-  }
-
-private:
-
-  ros::NodeHandle nh_;
-  image_transport::ImageTransport it_;
-  image_transport::Subscriber image_sub_;
-  image_transport::Publisher hist_pub_;
-  int queue_size_;
-  bool debug_view_;
-};
-
-} /* namespace intensity_histogram. */
-
-class IntensityHistogramNodelet : public nodelet::Nodelet
-{
-
-public:
-
-  virtual void onInit()  // NOLINT(modernize-use-override)
-  {
-    intensity_histogram::IntensityHistogram ih;
-    ros::spin();
-  }
-};
-
-} /* namespace opencv_apps. */
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(opencv_apps::IntensityHistogramNodelet, nodelet::Nodelet);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 7e38dc04..0276c333 100644
--- a/test/CMakeLists.txt
+++ b/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-compare_histogram.test ARGS gui:=false)
 add_rostest(test-equalize_histogram.test ARGS gui:=false)
 
 add_rostest(test-hough_lines.test ARGS gui:=false)
diff --git a/test/test-compare_histogram.test b/test/test-compare_histogram.test
new file mode 100644
index 00000000..c147217f
--- /dev/null
+++ b/test/test-compare_histogram.test
@@ -0,0 +1,66 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- calcHist for Color Image -->
+    <include file="$(find opencv_apps)/launch/compare_histogram.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect_color" />
+      <arg name="histogram_size" value="64" />
+      <arg name="node_name" value="compare_histogram_color" />
+    </include>
+
+    <!-- calcHist for Gray Image -->
+    <include file="$(find opencv_apps)/launch/compare_histogram.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect" />
+      <arg name="histogram_size" value="32" />
+      <arg name="reference_histogram" value="
+         [[0.        , 0.0123726 , 0.43492851, 0.69198507, 0.55949336, 0.36090258,
+           0.20119113, 0.23650548, 0.57807326, 0.74101412, 0.70817435, 0.72729945,
+           0.68506479, 0.49020678, 0.32500106, 0.26422849, 0.46420333, 0.47376588,
+           0.21146668, 0.18026255, 0.21356374, 0.24858449, 0.21540914, 0.24682297,
+           0.30197543, 0.27785933, 0.27802709, 0.32470748, 0.40351465, 0.62638932,
+           1.        , 0.40129179]]" />
+      <arg name="node_name" value="compare_histogram_gray" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="compare_histogram_color_saver" pkg="image_view" type="image_saver" >
+      <param name="filename_format" value="$(find opencv_apps)/test/compare_histogram_color.png"/>
+      <param name="queue_size" value="5" />
+      <remap from="image" to="compare_histogram_color/image" />
+      <remap from="compare_histogram_color/camera_info" to="camera_info" />
+    </node>
+    <node name="compare_histogram_gray_saver" pkg="image_view" type="image_saver" >
+      <param name="filename_format" value="$(find opencv_apps)/test/compare_histogram_gray.png"/>
+      <param name="queue_size" value="5" />
+      <remap from="image" to="compare_histogram_gray/image" />
+      <remap from="compare_histogram_gray/camera_info" to="camera_info" />
+    </node>
+
+    <param name="compare_histogram_color_test/topic" value="compare_histogram_color/image" />
+    <test test-name="compare_histogram_color_test" pkg="rostest" type="hztest" name="compare_histogram_color_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" />
+    </test>
+    <param name="compare_histogram_gray_test/topic" value="compare_histogram_gray/image" />
+    <test test-name="compare_histogram_gray_test" pkg="rostest" type="hztest" name="compare_histogram_gray_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" />
+    </test>
+    <param name="compare_histogram_gray_distance_test/topic" value="compare_histogram_gray/distance" />
+    <test test-name="compare_histogram_gray_distance_test" pkg="rostest" type="hztest" name="compare_histogram_gray_distance_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" />
+    </test>
+
+  </group>
+</launch>

From 49e4e7241033a30a319b3d92ad6942b75f9f04d0 Mon Sep 17 00:00:00 2001
From: Kei Okada <kei.okada@gmail.com>
Date: Wed, 16 Feb 2022 10:51:00 +0900
Subject: [PATCH 4/5] HISTCMP_CORREL only available since OpenCV 3.0
 (https://github.com/opencv/opencv/pull/785)

---
 CMakeLists.txt      | 4 +++-
 test/CMakeLists.txt | 4 +++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ce3e3e2d..3c41f2dd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -170,8 +170,10 @@ opencv_apps_add_nodelet(discrete_fourier_transform src/nodelet/discrete_fourier_
   # ./tutorial_code/Histograms_Matching/calcBackProject_Demo1.cpp
   # ./tutorial_code/Histograms_Matching/calcBackProject_Demo2.cpp
   # ./tutorial_code/Histograms_Matching/calcHist_Demo.cpp
+if(OpenCV_VERSION VERSION_GREATER "3.0")
   opencv_apps_add_nodelet(compare_histogram src/nodelet/compare_histogram_nodelet.cpp)  #  ./tutorial_code/Histograms_Matching/compareHist_Demo.cpp
-  opencv_apps_add_nodelet(equalize_histogram src/nodelet/equalize_histogram_nodelet.cpp) # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp
+endif()
+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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 0276c333..da616de7 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -56,7 +56,9 @@ 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-compare_histogram.test ARGS gui:=false)
+if(OpenCV_VERSION VERSION_GREATER "3.0")
+  add_rostest(test-compare_histogram.test ARGS gui:=false)
+endif()
 add_rostest(test-equalize_histogram.test ARGS gui:=false)
 
 add_rostest(test-hough_lines.test ARGS gui:=false)

From 5f9777b77e2c267192fe3c77b4d7e2cb0cd5bebe Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 18 Feb 2022 14:25:55 +0900
Subject: [PATCH 5/5] OpenCV < 3 : remove compare_histogram from
 roslaunch_add_file_check

---
 CMakeLists.txt      | 3 +++
 test/CMakeLists.txt | 3 +++
 2 files changed, 6 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3c41f2dd..af543659 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -390,6 +390,9 @@ if(CATKIN_ENABLE_TESTING)
     message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}")
   else()
     file(GLOB LAUNCH_FILES launch/*.launch)
+    if(NOT OpenCV_VERSION VERSION_GREATER "3.0")
+      list(REMOVE_ITEM LAUNCH_FILES ${CMAKE_CURRENT_SOURCE_DIR}/launch/compare_histogram.launch)
+    endif()
     foreach(LAUNCH_FILE ${LAUNCH_FILES})
       roslaunch_add_file_check(${LAUNCH_FILE})
     endforeach()
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index da616de7..3cd329b9 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -108,6 +108,9 @@ if(roslaunch_VERSION VERSION_LESS "1.11.1")
   message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}")
 else()
   file(GLOB LAUNCH_FILES *.test)
+  if(NOT OpenCV_VERSION VERSION_GREATER "3.0")
+    list(REMOVE_ITEM LAUNCH_FILES ${CMAKE_CURRENT_SOURCE_DIR}/test-compare_histogram.test)
+  endif()
   foreach(LAUNCH_FILE ${LAUNCH_FILES})
     roslaunch_add_file_check(${LAUNCH_FILE})
   endforeach()