diff --git a/CMakeLists.txt b/CMakeLists.txt
index b4d309c0..63939b46 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,6 +35,8 @@ generate_dynamic_reconfigure_options(
cfg/GoodfeatureTrack.cfg
cfg/CornerHarris.cfg
#
+ cfg/BackGroundSubtraction.cfg
+ #
cfg/CamShift.cfg
cfg/FBackFlow.cfg
cfg/LKFlow.cfg
@@ -216,7 +218,7 @@ opencv_apps_add_nodelet(corner_harris corner_harris/corner_harris src/nodelet/co
# ./tutorial_code/TrackingMotion/cornerSubPix_Demo.cpp
# videoio
- # ./tutorial_code/video/bg_sub.cpp
+opencv_apps_add_nodelet(background_subtraction background_subtraction/background_subtraction src/nodelet/background_subtraction_nodelet.cpp) # ./tutorial_code/video/bg_sub.cpp
# ./tutorial_code/videoio/video-input-psnr-ssim/video-input-psnr-ssim.cpp
# ./tutorial_code/videoio/video-write/video-write.cpp
diff --git a/cfg/BackGroundSubtraction.cfg b/cfg/BackGroundSubtraction.cfg
new file mode 100755
index 00000000..6aec1073
--- /dev/null
+++ b/cfg/BackGroundSubtraction.cfg
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+
+PACKAGE = 'background_subtraction'
+
+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)
+algorithm_type = gen.enum([ gen.const("MOG", int_t, 0, "BackgroundSubtractorMOG"),
+ gen.const("MOG2", int_t, 1, "BackgroundSubtractorMOG2"),
+ gen.const("GMG", int_t, 2, "BackgroundSubtractorGMG")], "An enum for Background Subtraction Mehtods")
+
+gen.add("algorithm_type", int_t, 0, "Background Subtraction Algorithm Methods", 1, 0, 2, edit_method=algorithm_type)
+
+gen.add("mog_history", int_t, 0, "Length of the history", 200, 0, 2048)
+gen.add("mog_nmixtures", int_t, 0, "Number of Gaussian mixtures", 5, 0, 128)
+gen.add("mog_background_ratio", double_t, 0, "backgroundRatio", 0.7, 0.0, 1.0)
+gen.add("mog_noise_sigma", double_t, 0, "Noise strength", 30*0.5, 0.0, 512.0)
+gen.add("mog_learning_rate", double_t, 0, "Learning rate", 0.0, -1.0, 1.0)
+
+gen.add("mog2_history", int_t, 0, "Length of the history", 500, 0, 2048)
+gen.add("mog2_var_threshold", double_t, 0, "Threshold on the squared Mahalanobis distance to decide whether it is well described by the background model. This parameter does not affect the background update. A typical value could be 4 sigma, that is, varThreshold=4*4=16;", 16.0, 0.0, 256.0)
+gen.add("mog2_shadow_detection", bool_t, 0, "Parameter defining whether shadow detection should be enabled (true or false)", True)
+gen.add("mog2_learning_rate", double_t, 0, "Learning rate", -1.0, -1.0, 1.0)
+
+gen.add("gmg_max_features", int_t, 0, "Total number of distinct colors to maintain in histogram", 64, 1, 512)
+gen.add("gmg_learning_rate", double_t, 0, "Set between 0.0 and 1.0, determines how quickly features are 'forgotten' from histograms", 0.025, 0.0, 1.0);
+gen.add("gmg_num_initialization_frames", int_t, 0, "Number of frames of video to use to initialize histograms", 120, 1, 1024)
+gen.add("gmg_quantization_levels", int_t, 0, "Number of discrete levels in each channel to be used in histograms", 16, 1, 128)
+gen.add("gmg_background_prior", double_t, 0, "Prior probability that any given pixel is a background pixel. A sensitivity parameter", 0.8, 0.0, 1.0)
+gen.add("gmg_decision_threshold", double_t, 0, "Value above which pixel is determined to be FG.", 0.8, 0.0, 1.0)
+gen.add("gmg_smoothing_radius", double_t, 0, "Smoothing radius, in pixels, for cleaning up FG image.", 7, 1, 32)
+gen.add("gmg_update_background_model", bool_t, 0, "Perform background model update", True)
+
+
+exit (gen.generate (PACKAGE, "background_subtraction", "BackGroundSubtraction"))
diff --git a/launch/background_subtraction.launch b/launch/background_subtraction.launch
new file mode 100644
index 00000000..54e876c6
--- /dev/null
+++ b/launch/background_subtraction.launch
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+Mixture-based Gaussian
+
+
+
+
+
+
+
+
+
diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
index 74a7466b..93dfb926 100644
--- a/nodelet_plugins.xml
+++ b/nodelet_plugins.xml
@@ -51,6 +51,10 @@
Nodelet for detecting corners using Harris method
+
+ Nodelet to calculate background subtraction
+
+
Nodelet to show mean-shift based tracking
diff --git a/src/nodelet/background_subtraction_nodelet.cpp b/src/nodelet/background_subtraction_nodelet.cpp
new file mode 100644
index 00000000..aa266910
--- /dev/null
+++ b/src/nodelet/background_subtraction_nodelet.cpp
@@ -0,0 +1,293 @@
+// -*- 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/bgfg_segm.cpp
+// https://github.com/opencv/opencv/blob/2.4/samples/cpp/bgfg_gmg.cpp
+/**
+ * @file bg_sub.cpp
+ * @brief Background subtraction tutorial sample code
+ * @author Domenico D. Bloisi
+ */
+
+#include
+#include "opencv_apps/nodelet.h"
+#include
+#include
+#include
+
+#include
+#include
+
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/video/video.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+#include
+#include "opencv_apps/BackGroundSubtractionConfig.h"
+
+namespace background_subtraction {
+class BackGroundSubtractionNodelet : 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 background_subtraction::BackGroundSubtractionConfig 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_;
+
+ // Global Variables
+ int algorithm_type_;
+ // cv::Ptr did not work only for GMG...
+ cv::Ptr pMOG; //MOG Background subtractor
+ cv::Ptr pMOG2;//MOG2 Background subtractor
+ cv::Ptr pGMG; //GMG Background subtractore
+ int numInitializationFrames_;
+
+ void reconfigureCallback(Config &new_config, uint32_t level)
+ {
+ config_ = new_config;
+ need_config_update_ = true;
+ }
+
+ 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);
+ }
+
+ 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;
+
+ // switch algorithm type
+ if ( need_config_update_ ) {
+ if ( config_.algorithm_type != algorithm_type_ ) {
+ ROS_INFO("Update BackGround Subtraction Algorithm");
+ algorithm_type_ = config_.algorithm_type;
+ switch (algorithm_type_) {
+ case background_subtraction::BackGroundSubtraction_MOG:
+ if ( ! pMOG ) {
+ ROS_INFO("Initialize cv::BackgroundSubtractorMOG()");
+ pMOG = new cv::BackgroundSubtractorMOG();
+ }
+ break;
+ case background_subtraction::BackGroundSubtraction_MOG2:
+ if ( ! pMOG2 ) {
+ ROS_INFO("Initialize cv::BackgroundSubtractorMOG2()");
+ pMOG2 = new cv::BackgroundSubtractorMOG2();
+ }
+ break;
+ case background_subtraction::BackGroundSubtraction_GMG:
+ if ( ! pGMG ) {
+ ROS_INFO("Initialize cv::BackgroundSubtractorGMG()");
+ pGMG = new cv::BackgroundSubtractorGMG();
+ }
+ numInitializationFrames_ = pGMG->numInitializationFrames;
+ break;
+ }
+ }
+ switch ( algorithm_type_ ) {
+ case background_subtraction::BackGroundSubtraction_MOG:
+ ROS_INFO("Update cv::BackgroundSubtractorMOG()");
+ pMOG = new cv::BackgroundSubtractorMOG(config_.mog_history, config_.mog_nmixtures, config_.mog_background_ratio, config_.mog_noise_sigma);
+ // Length of the history
+ ROS_INFO_STREAM("history : " << config_.mog_history);
+ // Number of Gaussian mixtures
+ ROS_INFO_STREAM("nmixtures : " << config_.mog_nmixtures);
+ // Background ratio
+ ROS_INFO_STREAM("backgroundRatio : " << config_.mog_background_ratio);
+ // Noise strength
+ ROS_INFO_STREAM("noiseSigma : " << config_.mog_noise_sigma);
+
+ // Learning rate
+ ROS_INFO_STREAM("larningRate : " << config_.mog_learning_rate);
+
+ break;
+ case background_subtraction::BackGroundSubtraction_MOG2:
+ ROS_INFO("Update cv::BackgroundSubtractorMOG2()");
+ pMOG2 = new cv::BackgroundSubtractorMOG2(config_.mog2_history, config_.mog2_var_threshold, config_.mog2_shadow_detection);
+ // Length of the history
+ ROS_INFO_STREAM("history : " << config_.mog2_history);
+ // Threshold on the squared Mahalanobis distance to decide whether it is well described by the background model (see Cthr??). This parameter does not affect the background update. A typical value could be 4 sigma, that is, varThreshold=4*4=16; (see Tb??)
+ ROS_INFO_STREAM("varThreshold : " << config_.mog2_var_threshold);
+ // Parameter defining whether shadow detection should be enabled (true or false)
+ ROS_INFO_STREAM("bShadowDetection : " << config_.mog2_shadow_detection);
+ // Learning rate
+ ROS_INFO_STREAM("larningRate : " << config_.mog2_learning_rate);
+
+ break;
+ case background_subtraction::BackGroundSubtraction_GMG:
+ ROS_INFO("Update cv::BackgroundSubtractorGMG()");
+
+ pGMG->maxFeatures = config_.gmg_max_features;
+ pGMG->learningRate = config_.gmg_learning_rate;
+ pGMG->numInitializationFrames = config_.gmg_num_initialization_frames;
+ pGMG->quantizationLevels = config_.gmg_quantization_levels;
+ pGMG->backgroundPrior = config_.gmg_background_prior;
+ pGMG->decisionThreshold = config_.gmg_decision_threshold;
+ pGMG->smoothingRadius = config_.gmg_smoothing_radius;
+ pGMG->updateBackgroundModel = config_.gmg_update_background_model;
+
+ //! Total number of distinct colors to maintain in histogram.
+ ROS_INFO_STREAM("maxFeatures : " << pGMG->maxFeatures);
+ //! Set between 0.0 and 1.0, determines how quickly features are "forgotten" from histograms.
+ ROS_INFO_STREAM("learningRate : " << pGMG->learningRate);
+ //! Number of frames of video to use to initialize histograms.
+ ROS_INFO_STREAM("numInitializationFrames : " << pGMG->numInitializationFrames);
+ //! Number of discrete levels in each channel to be used in histograms.
+ ROS_INFO_STREAM("quantizationLevels : " << pGMG->quantizationLevels);
+ //! Prior probability that any given pixel is a background pixel. A sensitivity parameter.
+ ROS_INFO_STREAM("backgroundPrior : " << pGMG->backgroundPrior);
+ //! Value above which pixel is determined to be FG.
+ ROS_INFO_STREAM("decisionThreshold : " << pGMG->decisionThreshold);
+ //! Smoothing radius, in pixels, for cleaning up FG image.
+ ROS_INFO_STREAM("smoothingRadius : " << pGMG->smoothingRadius);
+ //! Perform background model update
+ ROS_INFO_STREAM("updateBackgroundModel : " << pGMG->updateBackgroundModel);
+ break;
+ }
+ need_config_update_ = false;
+ }
+
+ cv::Mat out_image; //fg mask fg mask generated by MOG2 method
+ //update the background model
+ switch (algorithm_type_) {
+ case background_subtraction::BackGroundSubtraction_MOG:
+ pMOG->operator()(in_image, out_image);
+ break;
+ case background_subtraction::BackGroundSubtraction_MOG2:
+ pMOG2->operator()(in_image, out_image, config_.mog2_learning_rate);
+ break;
+ case background_subtraction::BackGroundSubtraction_GMG:
+ pGMG->operator()(in_image, out_image);
+ if ( numInitializationFrames_ >= 0 ) {
+ ROS_INFO_STREAM("Initializing Frames ... " << numInitializationFrames_--);
+ }
+ break;
+ }
+
+ //show the current frame and the fg masks
+ /// Show the image
+ 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, "mono8", 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, &BackGroundSubtractionNodelet::imageCallbackWithInfo, this);
+ else
+ img_sub_ = it_->subscribe("image", 3, &BackGroundSubtractionNodelet::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_ = "BackGroundSubtraction Demo";
+ //create Background Subtractor objects
+ float varThreshold = 16;
+ bool bShadowDetection = true;
+ algorithm_type_ = -1;
+
+ reconfigure_server_ = boost::make_shared >(*pnh_);
+ dynamic_reconfigure::Server::CallbackType f =
+ boost::bind(&BackGroundSubtractionNodelet::reconfigureCallback, this, _1, _2);
+ reconfigure_server_->setCallback(f);
+
+ img_pub_ = advertiseImage(*pnh_, "image", 1);
+
+ onInitPostProcess();
+ }
+};
+bool BackGroundSubtractionNodelet::need_config_update_ = true;
+}
+
+#include
+PLUGINLIB_EXPORT_CLASS(background_subtraction::BackGroundSubtractionNodelet, nodelet::Nodelet);
diff --git a/test/test-background_subtraction.test b/test/test-background_subtraction.test
new file mode 100644
index 00000000..01b06ef4
--- /dev/null
+++ b/test/test-background_subtraction.test
@@ -0,0 +1,70 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+