From a9bf752566450b9ffe7d8107145a553caec44557 Mon Sep 17 00:00:00 2001 From: iory Date: Tue, 30 Aug 2016 18:47:59 +0900 Subject: [PATCH] [smoothing] Add smoothing filter code, test, launch files --- CMakeLists.txt | 3 +- cfg/Smoothing.cfg | 18 +++ launch/smoothing.launch | 20 +++ nodelet_plugins.xml | 4 + src/nodelet/smoothing_nodelet.cpp | 232 ++++++++++++++++++++++++++++++ test/CMakeLists.txt | 1 + test/test-smoothing.test | 89 ++++++++++++ 7 files changed, 366 insertions(+), 1 deletion(-) create mode 100755 cfg/Smoothing.cfg create mode 100644 launch/smoothing.launch create mode 100644 src/nodelet/smoothing_nodelet.cpp create mode 100644 test/test-smoothing.test diff --git a/CMakeLists.txt b/CMakeLists.txt index c1e4b83d..2086e71a 100644 --- 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 new file mode 100755 index 00000000..3bc5650c --- /dev/null +++ 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 new file mode 100644 index 00000000..876800a5 --- /dev/null +++ b/launch/smoothing.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 9cd71cce..a6ca40ca 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -4,6 +4,10 @@ Nodelet to combine two images + + Nodelet to smoothing by homogeneous filter, bilateral filter, gaussian filter, median filter + + Nodelet to find edges diff --git a/src/nodelet/smoothing_nodelet.cpp b/src/nodelet/smoothing_nodelet.cpp new file mode 100644 index 00000000..8709207a --- /dev/null +++ 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 +#include "opencv_apps/nodelet.h" +#include +#include +#include + +#include +#include + +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/features2d/features2d.hpp" + +#include +#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 it_; + + typedef smoothing::SmoothingConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + Config config_; + boost::shared_ptr 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(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 >(*pnh_); + dynamic_reconfigure::Server::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_EXPORT_CLASS(smoothing::SmoothingNodelet, nodelet::Nodelet); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 53f987a8..b73ce322 100644 --- 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 new file mode 100644 index 00000000..21c0291a --- /dev/null +++ b/test/test-smoothing.test @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +