Add adding_image

CMakeLists.txt

@@ -17,6 +17,7 @@ endif()
 
 # generate the dynamic_reconfigure config file
 generate_dynamic_reconfigure_options(
+  cfg/AddingImages.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
@@ -109,7 +110,7 @@ endmacro()
   # ./tutorial_code/calib3d/stereoBM/SBM_Sample.cpp
 
 # core
-  # ./tutorial_code/core/AddingImages/AddingImages.cpp
+opencv_apps_add_nodelet(adding_images adding_images/adding_images src/nodelet/adding_images_nodelet.cpp) # ./tutorial_code/core/AddingImages/AddingImages.cpp
   # ./tutorial_code/core/discrete_fourier_transform/discrete_fourier_transform.cpp
   # ./tutorial_code/core/file_input_output/file_input_output.cpp
   # ./tutorial_code/core/how_to_scan_images/how_to_scan_images.cpp

cfg/AddingImages.cfg

@@ -0,0 +1,14 @@
+#! /usr/bin/env python
+
+PACKAGE='adding_images'
+
+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)
+
+gen.add("alpha", double_t, 0, "weight of the first array elements.", 0.5, 0.0, 1.0)
+gen.add("gamma", double_t, 0, "scalar added to each sum.", 0, 0, 255)
+
+exit(gen.generate(PACKAGE, "adding_images", "AddingImages"))

launch/adding_images.launch

@@ -0,0 +1,23 @@
+<launch>
+  <arg name="node_name" default="adding_images" />
+
+  <arg name="image1" default="image1" doc="The image topic. Should be remapped to the name of the real image topic." />
+  <arg name="image2" default="image2" 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="alpha" default="0.5" doc="Weight of the first array elements" />
+  <arg name="gamma" default="0" doc="Scalar added to each sum" />
+  <arg name="approximate_sync" default="false" doc="Flag of approximate_sync. If this value is false, image1 and image2 synchronize exact time." />
+
+  <!-- adding_images.cpp -->
+  <node name="$(arg node_name)" pkg="opencv_apps" type="adding_images" >
+    <remap from="image1" to="$(arg image1)" />
+    <remap from="image2" to="$(arg image2)" />
+    <param name="use_camera_info" value="$(arg use_camera_info)" />
+    <param name="debug_view" value="$(arg debug_view)" />
+    <param name="alpha" value="$(arg alpha)" />
+    <param name="gamma" value="$(arg gamma)" />
+    <param name="approximate_sync" value="$(arg approximate_sync)" />
+  </node>
+</launch>

nodelet_plugins.xml

@@ -1,5 +1,9 @@
 <library path="lib/libopencv_apps">
 
+  <class name="adding_images/adding_images" type="adding_images::AddingImagesNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet to combine two images</description>
+  </class>
+
   <class name="edge_detection/edge_detection" type="edge_detection::EdgeDetectionNodelet" base_class_type="nodelet::Nodelet">
     <description>Nodelet to find edges</description>
   </class>
@@ -63,11 +67,11 @@
   <class name="simple_flow/simple_flow" type="simple_flow::SimpleFlowNodelet" base_class_type="nodelet::Nodelet">
     <description>Nodelet of SimpleFlow optical flow algorithm</description>
   </class>
-  
+
  <class name="simple_example/simple_example" type="simple_example::SimpleExampleNodelet" base_class_type="nodelet::Nodelet">
     <description>Nodelet of Simple Example from wiki</description>
   </class>
-  
+
    <class name="simple_compressed_example/simple_compressed_example" type="simple_compressed_example::SimpleCompressedExampleNodelet" base_class_type="nodelet::Nodelet">
     <description>Nodelet of Simple Example from wiki</description>
   </class>

src/nodelet/adding_images_nodelet.cpp

@@ -0,0 +1,234 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) JSK, 2016 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/tree/2.4/samples/cpp/tutorial_code/ImgProc/AddingImages.cpp
+/**
+ * This is a demo of adding image (linear blending).
+ */
+
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include "opencv_apps/AddingImagesConfig.h"
+#include "opencv_apps/nodelet.h"
+
+namespace adding_images {
+  class AddingImagesNodelet : public opencv_apps::Nodelet {
+  private:
+    boost::shared_ptr<image_transport::ImageTransport> it_;
+
+    typedef message_filters::sync_policies::ExactTime<
+      sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicyWithCameraInfo;
+    typedef message_filters::sync_policies::ApproximateTime<
+      sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ApproxSyncPolicyWithCameraInfo;
+    typedef message_filters::sync_policies::ExactTime<
+      sensor_msgs::Image, sensor_msgs::Image> SyncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<
+      sensor_msgs::Image, sensor_msgs::Image> ApproxSyncPolicy;
+
+    ////////////////////////////////////////////////////////
+    // Dynamic Reconfigure
+    ////////////////////////////////////////////////////////
+    typedef adding_images::AddingImagesConfig 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_;
+
+    image_transport::SubscriberFilter sub_image1_, sub_image2_;
+    image_transport::Publisher img_pub_;
+    message_filters::Subscriber<sensor_msgs::CameraInfo> sub_camera_info_;
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicyWithCameraInfo> > sync_with_info_;
+    boost::shared_ptr<message_filters::Synchronizer<ApproxSyncPolicyWithCameraInfo> > async_with_info_;
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
+    boost::shared_ptr<message_filters::Synchronizer<ApproxSyncPolicy> > async_;
+    boost::mutex mutex_;
+
+    bool approximate_sync_;
+    double alpha_;
+    double beta_;
+    double gamma_;
+
+    void imageCallbackWithInfo(
+      const sensor_msgs::ImageConstPtr& msg1,
+      const sensor_msgs::ImageConstPtr& msg2,
+      const sensor_msgs::CameraInfoConstPtr& cam_info) {
+      do_work(msg1, msg2, cam_info->header.frame_id);
+    }
+
+    void imageCallback(const sensor_msgs::ImageConstPtr& msg1,
+                       const sensor_msgs::ImageConstPtr& msg2) {
+      do_work(msg1, msg2, msg1->header.frame_id);
+    }
+
+    void subscribe() {
+      NODELET_DEBUG("Subscribing to image topic.");
+      sub_image1_.subscribe(*it_, "image1", 3);
+      sub_image2_.subscribe(*it_, "image2", 3);
+      sub_camera_info_.subscribe(*pnh_, "info", 3);
+      if (config_.use_camera_info) {
+        if (approximate_sync_) {
+          async_with_info_ = boost::make_shared<
+            message_filters::Synchronizer<ApproxSyncPolicyWithCameraInfo> >(100);
+          async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_);
+          async_with_info_->registerCallback(boost::bind(
+            &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3));
+        } else {
+          sync_with_info_ =
+            boost::make_shared<message_filters::Synchronizer<SyncPolicyWithCameraInfo> >(100);
+          sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_);
+          sync_with_info_->registerCallback(boost::bind(
+            &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3));
+        }
+      } else {
+        if (approximate_sync_) {
+          async_ = boost::make_shared<
+            message_filters::Synchronizer<ApproxSyncPolicy> >(100);
+          async_->connectInput(sub_image1_, sub_image2_);
+          async_->registerCallback(
+            boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2));
+        } else {
+          sync_ =
+            boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
+          sync_->connectInput(sub_image1_, sub_image2_);
+          sync_->registerCallback(
+            boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2));
+        }
+      }
+    }
+
+    void unsubscribe() {
+      NODELET_DEBUG("Unsubscribing from image topic.");
+      sub_image1_.unsubscribe();
+      sub_image2_.unsubscribe();
+      sub_camera_info_.unsubscribe();
+    }
+
+    void reconfigureCallback(Config& config, uint32_t level) {
+      boost::mutex::scoped_lock lock(mutex_);
+      config_ = config;
+      alpha_ = config.alpha;
+      beta_ = 1.0 - alpha_;
+      gamma_ = config.gamma;
+    }
+
+    void do_work(const sensor_msgs::Image::ConstPtr& image_msg1,
+                 const sensor_msgs::Image::ConstPtr& image_msg2,
+                 const std::string input_frame_from_msg) {
+        boost::mutex::scoped_lock lock(mutex_);
+      // Work on the image.
+      try {
+        cv::Mat image1 =
+          cv_bridge::toCvShare(image_msg1, image_msg1->encoding)->image;
+        cv::Mat image2 =
+          cv_bridge::toCvShare(image_msg2, image_msg2->encoding)->image;
+
+        // convert image to bgr8
+        if (image_msg1->encoding == sensor_msgs::image_encodings::RGB8 ||
+            image_msg1->encoding == sensor_msgs::image_encodings::RGB16) {
+          cv::cvtColor(image1, image1, CV_RGB2BGR);
+        }
+        if (image_msg2->encoding == sensor_msgs::image_encodings::RGB8 ||
+            image_msg2->encoding == sensor_msgs::image_encodings::RGB16) {
+          cv::cvtColor(image2, image2, CV_RGB2BGR);
+        }
+
+        cv::Mat result_image;
+        cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image);
+        //-- Show what you got
+        if (debug_view_) {
+          cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
+          cv::imshow(window_name_, result_image);
+          int c = cv::waitKey(1);
+        }
+        // publish bgr8 image
+        img_pub_.publish(cv_bridge::CvImage(image_msg1->header,
+                                            sensor_msgs::image_encodings::BGR8,
+                                            result_image).toImageMsg());
+
+      } 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_ = image_msg1->header.stamp;
+    }
+
+  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_ = "AddingImages Demo";
+      ////////////////////////////////////////////////////////
+      // Dynamic Reconfigure
+      ////////////////////////////////////////////////////////
+      reconfigure_server_ =
+        boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
+      dynamic_reconfigure::Server<Config>::CallbackType f =
+        boost::bind(&AddingImagesNodelet::reconfigureCallback, this, _1, _2);
+      reconfigure_server_->setCallback(f);
+
+      pnh_->param("approximate_sync", approximate_sync_, true);
+      img_pub_ = advertiseImage(*pnh_, "image", 1);
+      onInitPostProcess();
+    }
+  };
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(adding_images::AddingImagesNodelet, nodelet::Nodelet);

test/CMakeLists.txt

@@ -15,6 +15,7 @@ add_custom_target(vslam_tutorial_bag DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/vslam_t
 add_dependencies(tests vslam_tutorial_bag)
 
 #add_rostest(test-apps.test)
+add_rostest(test-adding_images.test ARGS gui:=false)
 add_rostest(test-edge_detection.test ARGS gui:=false)
 
 add_rostest(test-hough_lines.test ARGS gui:=false)

test/test-adding_images.test

@@ -0,0 +1,37 @@
+<launch>
+  <arg name="gui" default="true" />
+  <param name="use_sim_time" value="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_view0" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+    <node name="image_view1" pkg="image_view" type="image_view" args="image:=hough_lines/image" if="$(arg gui)" />
+
+    <node name="hough_circles"
+          pkg="opencv_apps" type="hough_circles">
+      <remap from="image" to="image_rect_color" />
+    </node>
+
+    <!-- adding_images.cpp -->
+    <include file="$(find opencv_apps)/launch/adding_images.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image1" value="image_rect_color" />
+      <arg name="image2" value="hough_circles/image" />
+      <arg name="alpha" value="0.5" />
+      <arg name="gamma" value="0" />
+      <arg name="approximate_sync" value="false" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="adding_images_saver_result" pkg="image_view" type="image_saver" args="image:=adding_images/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/adding_images_result.png"/>
+    </node>
+    <param name="adding_images_test/topic" value="adding_images/image" />
+    <test test-name="adding_images_test" pkg="rostest" type="hztest" name="adding_images_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" />
+    </test>
+  </group>
+</launch>