ros-perception · k-okada · Sep 12, 2016 · Aug 30, 2016
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -18,6 +18,7 @@ endif()
 # generate the dynamic_reconfigure config file
 generate_dynamic_reconfigure_options(
   cfg/AddingImages.cfg
+  cfg/Smoothing.cfg
   cfg/EdgeDetection.cfg cfg/HoughLines.cfg cfg/HoughCircles.cfg
   cfg/FindContours.cfg cfg/ConvexHull.cfg cfg/GeneralContours.cfg cfg/ContourMoments.cfg
   cfg/FaceDetection.cfg
@@ -148,7 +149,7 @@ opencv_apps_add_nodelet(adding_images adding_images/adding_images src/nodelet/ad
   # ./tutorial_code/ImgProc/Morphology_2.cpp
   # ./tutorial_code/ImgProc/Morphology_3.cpp
   # ./tutorial_code/ImgProc/Pyramids.cpp
-  # ./tutorial_code/ImgProc/Smoothing.cpp
+opencv_apps_add_nodelet(smoothing smoothing/smoothing src/nodelet/smoothing_nodelet.cpp)  # ./tutorial_code/ImgProc/Smoothing.cpp
 opencv_apps_add_nodelet(threshold threshold/threshold src/nodelet/threshold_nodelet.cpp)  # ./tutorial_code/ImgProc/Threshold.cpp
   # ./tutorial_code/ImgProc/Threshold_inRange.cpp
 

diff --git a/cfg/Smoothing.cfg b/cfg/Smoothing.cfg
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+PACKAGE = 'smoothing'
+
+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.", True)
+filter_type = gen.enum([ gen.const("Homogeneous_Blur",   int_t,  0, "Homogeneous blur"),
+                         gen.const("Gaussian_Blur",   int_t,  1, "Gaussian blur"),
+                         gen.const("Median_Blur",   int_t,  2, "Median blur"),
+                         gen.const("Bilateral_Filter",   int_t,  3, "Bilateral Filter")], "An enum for Smoothing Filter Mehtods")
+
+gen.add("filter_type", int_t, 0, "Smoothing Filter Methods", 1, 0, 3, edit_method=filter_type)
+gen.add("kernel_size", int_t, 0, "Size of the kernel (only one because we use a square window). Must be odd.", 7, 1, 31)
+
+exit (gen.generate (PACKAGE, "smoothing", "Smoothing"))
diff --git a/launch/smoothing.launch b/launch/smoothing.launch
@@ -0,0 +1,20 @@
+<launch>
+  <arg name="node_name" default="smoothing" />
+
+  <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 edge image" />
+
+  <arg name="filter_type" default="0" doc="Smoothing Filter Methods. 0:Homogeneous blur, 1:Gaussian blur, 2:Median blur, 3:Bilateral Filter." />
+  <arg name="kernel_size" default="7" doc="Size of the kernel (only one because we use a square window). Must be odd." />
+
+  <!-- smoothing.cpp  -->
+  <node name="$(arg node_name)" pkg="opencv_apps" type="smoothing" >
+    <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="filter_type" value="$(arg filter_type)" />
+    <param name="kernel_size" value="$(arg kernel_size)" />
+  </node>
+</launch>
diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
@@ -4,6 +4,10 @@
     <description>Nodelet to combine two images</description>
   </class>
 
+  <class name="smoothing/smoothing" type="smoothing::SmoothingNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet to smoothing by homogeneous filter, bilateral filter, gaussian filter, median filter</description>
+  </class>
+
   <class name="edge_detection/edge_detection" type="edge_detection::EdgeDetectionNodelet" base_class_type="nodelet::Nodelet">
     <description>Nodelet to find edges</description>
   </class>

diff --git a/src/nodelet/smoothing_nodelet.cpp b/src/nodelet/smoothing_nodelet.cpp
@@ -0,0 +1,232 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, JSK Lab
+ *  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 JSK Lab 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/ImgProc/Smoothing.cpp
+/**
+ * file Smoothing.cpp
+ * brief Sample code for simple filters
+ * 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>
+
+#include <iostream>
+#include <vector>
+
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/features2d/features2d.hpp"
+
+#include <dynamic_reconfigure/server.h>
+#include "opencv_apps/SmoothingConfig.h"
+
+/// Global Variables
+int MAX_KERNEL_LENGTH = 31;
+
+namespace smoothing {
+class SmoothingNodelet : public opencv_apps::Nodelet
+{
+  image_transport::Publisher img_pub_;
+  image_transport::Subscriber img_sub_;
+  image_transport::CameraSubscriber cam_sub_;
+  ros::Publisher msg_pub_;
+
+  boost::shared_ptr<image_transport::ImageTransport> it_;
+
+  typedef smoothing::SmoothingConfig Config;
+  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+  Config config_;
+  boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+
+  bool debug_view_;
+  ros::Time prev_stamp_;
+
+  std::string window_name_;
+  static bool need_config_update_;
+
+  int kernel_size_;
+
+  void reconfigureCallback(Config &new_config, uint32_t level)
+  {
+    config_         = new_config;
+    kernel_size_    = (config_.kernel_size/2)*2+1; // kernel_size must be odd number
+  }
+
+  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
+  {
+    if (frame.empty())
+      return image_frame;
+    return frame;
+  }
+
+  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
+  {
+    do_work(msg, cam_info->header.frame_id);
+  }
+
+  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    do_work(msg, msg->header.frame_id);
+  }
+
+  static void trackbarCallback( int, void* )
+  {
+    need_config_update_ = true;
+  }
+
+  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
+  {
+    // Work on the image.
+    try
+    {
+      // Convert the image into something opencv can handle.
+      cv::Mat in_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
+
+
+      if( debug_view_) {
+        /// Create Trackbars for Thresholds
+        char kernel_label[] = "Kernel Size : ";
+
+        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
+        cv::createTrackbar( kernel_label, window_name_, &kernel_size_, MAX_KERNEL_LENGTH, trackbarCallback);
+        if (need_config_update_) {
+          kernel_size_    = (kernel_size_/2)*2+1; // kernel_size must be odd number
+          config_.kernel_size = kernel_size_;
+          reconfigure_server_->updateConfig(config_);
+          need_config_update_ = false;
+        }
+      }
+
+      cv::Mat out_image = in_image.clone();
+      int i = kernel_size_;
+      switch (config_.filter_type) {
+        case smoothing::Smoothing_Homogeneous_Blur:
+          {
+            /// Applying Homogeneous blur
+            ROS_DEBUG_STREAM("Applying Homogeneous blur with kernel size " << i );
+            cv::blur( in_image, out_image, cv::Size( i, i ), cv::Point(-1,-1) );
+            break;
+          }
+        case smoothing::Smoothing_Gaussian_Blur:
+          {
+            /// Applying Gaussian blur
+            ROS_DEBUG_STREAM("Applying Gaussian blur with kernel size " << i );
+            cv::GaussianBlur( in_image, out_image, cv::Size( i, i ), 0, 0 );
+            break;
+          }
+        case smoothing::Smoothing_Median_Blur:
+          {
+            /// Applying Median blur
+            ROS_DEBUG_STREAM("Applying Median blur with kernel size " << i );
+            cv::medianBlur ( in_image, out_image, i );
+            break;
+          }
+        case smoothing::Smoothing_Bilateral_Filter:
+          {
+            /// Applying Bilateral Filter
+            ROS_DEBUG_STREAM("Applying Bilateral blur with kernel size " << i );
+            cv::bilateralFilter ( in_image, out_image, i, i*2, i/2 );
+            break;
+          }
+      }
+
+      //-- Show what you got
+      if( debug_view_) {
+        cv::imshow( window_name_, out_image );
+        int c = cv::waitKey(1);
+      }
+
+      // Publish the image.
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).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);
+    }
+
+    prev_stamp_ = msg->header.stamp;
+  }
+
+  void subscribe()
+  {
+    NODELET_DEBUG("Subscribing to image topic.");
+    if (config_.use_camera_info)
+      cam_sub_ = it_->subscribeCamera("image", 3, &SmoothingNodelet::imageCallbackWithInfo, this);
+    else
+      img_sub_ = it_->subscribe("image", 3, &SmoothingNodelet::imageCallback, this);
+  }
+
+  void unsubscribe()
+  {
+    NODELET_DEBUG("Unsubscribing from image topic.");
+    img_sub_.shutdown();
+    cam_sub_.shutdown();
+  }
+
+public:
+  virtual void onInit()
+  {
+    Nodelet::onInit();
+    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
+
+    pnh_->param("debug_view", debug_view_, false);
+    if (debug_view_) {
+      always_subscribe_ = true;
+    }
+    prev_stamp_ = ros::Time(0, 0);
+
+    window_name_ = "Smoothing Demo";
+    kernel_size_ = 7;
+
+    reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
+    dynamic_reconfigure::Server<Config>::CallbackType f =
+      boost::bind(&SmoothingNodelet::reconfigureCallback, this, _1, _2);
+    reconfigure_server_->setCallback(f);
+
+    img_pub_ = advertiseImage(*pnh_, "image", 1);
+
+    onInitPostProcess();
+  }
+};
+bool SmoothingNodelet::need_config_update_ = false;
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(smoothing::SmoothingNodelet, nodelet::Nodelet);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -16,6 +16,7 @@ add_dependencies(tests vslam_tutorial_bag)
 
 #add_rostest(test-apps.test)
 add_rostest(test-adding_images.test ARGS gui:=false)
+add_rostest(test-smoothing.test ARGS gui:=false)
 add_rostest(test-threshold.test ARGS gui:=false)
 add_rostest(test-edge_detection.test ARGS gui:=false)
 

diff --git a/test/test-smoothing.test b/test/test-smoothing.test
@@ -0,0 +1,89 @@
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="use_sim_time" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="--clock -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)" />
+
+    <!-- smoothing.cpp  -->
+    <!-- 0: Homogeneous blur -->
+    <include file="$(find opencv_apps)/launch/smoothing.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect" />
+      <arg name="filter_type" value="0" />
+      <arg name="node_name" value="smooth_homogeneous_filter" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="smooth_homogeneous_filter_saver" pkg="image_view" type="image_saver" args="image:=smooth_homogeneous_filter/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/smooth_homogeneous_filter.png"/>
+    </node>
+    <param name="smooth_homogeneous_filter_test/topic" value="smooth_homogeneous_filter/image" />    <!-- opencv_apps/FaceArrayStamped -->
+    <test test-name="smooth_homogeneous_filter_test" pkg="rostest" type="hztest" name="smooth_homogeneous_filter_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="10" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+
+    <!-- 1: Gaussian blur -->
+    <include file="$(find opencv_apps)/launch/smoothing.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect" />
+      <arg name="filter_type" value="1" />
+      <arg name="node_name" value="smooth_gaussian_filter" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="smooth_gaussian_filter_saver" pkg="image_view" type="image_saver" args="image:=smooth_gaussian_filter/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/smooth_gaussian_filter.png"/>
+    </node>
+    <param name="smooth_gaussian_filter_test/topic" value="smooth_gaussian_filter/image" />
+    <test test-name="smooth_gaussian_filter_test" pkg="rostest" type="hztest" name="smooth_gaussian_filter_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="10" />
+      <param name="test_duration" value="5.0" />
+    </test>
+
+    <!-- 2: Median blur -->
+    <include file="$(find opencv_apps)/launch/smoothing.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect" />
+      <arg name="filter_type" value="2" />
+      <arg name="node_name" value="smooth_median_filter" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="smooth_median_filter_saver" pkg="image_view" type="image_saver" args="image:=smooth_median_filter/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/smooth_median_filter.png"/>
+    </node>
+    <param name="smooth_median_filter_test/topic" value="smooth_median_filter/image" />
+    <test test-name="smooth_median_filter_test" pkg="rostest" type="hztest" name="smooth_median_filter_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" />
+    </test>
+
+    <!-- 3: Bilateral blur -->
+    <include file="$(find opencv_apps)/launch/smoothing.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect" />
+      <arg name="filter_type" value="3" />
+      <arg name="node_name" value="smooth_bilateral_filter" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="smooth_bilateral_filter_saver" pkg="image_view" type="image_saver" args="image:=smooth_bilateral_filter/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/smooth_bilateral_filter.png"/>
+    </node>
+    <param name="smooth_bilateral_filter_test/topic" value="smooth_bilateral_filter/image" />
+    <test test-name="smooth_bilateral_filter_test" pkg="rostest" type="hztest" name="smooth_bilateral_filter_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" />
+    </test>
+
+
+  </group>
+</launch>