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 100644
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>