add video to scene

add example to pubilsh images only when scene transaction detection

based on  https://github.com/zzingobomi/Opencv/blob/8dc7692f680e84b5bfa4c41ee06251ec3190bf26/src/01_tutorial/03_poencv_advanced_project/13_scene_transition_detection/detect_scene.py
see https://stackoverflow.com/questions/5317744/what-is-the-best-way-to-divide-a-video-into-scenes-segments for backgrounds

jsk_perception/CMakeLists.txt

@@ -151,6 +151,7 @@ generate_dynamic_reconfigure_options(
   cfg/VirtualCameraMono.cfg
   cfg/AWSDetectFaces.cfg
   cfg/RemoveBlurredFrames.cfg
+  cfg/VideoToScene.cfg
 )
 
 message(WARNING "DEPRECATION WARNING: srv files in jsk_pcl_ros are deprecated."
@@ -290,6 +291,7 @@ jsk_add_nodelet(src/gaussian_blur.cpp "jsk_perception/GaussianBlur" "gaussian_bl
 jsk_add_nodelet(src/kmeans.cpp "jsk_perception/KMeans" "kmeans")
 jsk_add_nodelet(src/draw_rects.cpp "jsk_perception/DrawRects" "draw_rects")
 jsk_add_nodelet(src/remove_blurred_frames.cpp "jsk_perception/RemoveBlurredFrames" "remove_blurred_frames")
+jsk_add_nodelet(src/video_to_scene.cpp "jsk_perception/VideoToScene" "video_to_scene")
 if("${OpenCV_VERSION}" VERSION_GREATER "2.9.9")  # >= 3.0.0
   jsk_add_nodelet(src/bing.cpp "jsk_perception/Bing" "bing")
 endif()
@@ -590,6 +592,7 @@ if(CATKIN_ENABLE_TESTING)
     jsk_add_rostest(test/ycc_decomposer.test)
     jsk_add_rostest(test/depth_image_filter.test)
     jsk_add_rostest(test/remove_blurred_frames.test)
+    jsk_add_rostest(test/video_to_scene.test)
     if(TARGET JSK_NODELET_${PROJECT_NAME}_bing)
       jsk_add_rostest(test/bing.test)
     endif()

jsk_perception/cfg/VideoToScene.cfg

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+PACKAGE='jsk_perception'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("min_percent", int_t, 0, "The percentage of forgrand when the motion has stopped and publish image", 5, 0, 100)
+gen.add("max_percent", int_t, 0, "The percentage of forgrand when the scene is not settled", 20, 0, 100)
+
+exit(gen.generate(PACKAGE, "jsk_perception", "VideoToScene"))

jsk_perception/include/jsk_perception/video_to_scene.h

@@ -0,0 +1,81 @@
+// -*- mode: C++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, 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.
+ *********************************************************************/
+/*
+ * video_to_scene.h
+ * Author: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
+ */
+
+#ifndef VIDEO_TO_SCENE_H_
+#define VIDEO_TO_SCENE_H_
+
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+#include <dynamic_reconfigure/server.h>
+#include <jsk_perception/VideoToSceneConfig.h>
+#include <sensor_msgs/Image.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/bgsegm.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <std_msgs/Float64.h>
+#include <image_transport/image_transport.h>
+
+
+namespace jsk_perception{
+    class VideoToScene: public jsk_topic_tools::DiagnosticNodelet{
+    public:
+        typedef VideoToSceneConfig Config;
+    VideoToScene() : DiagnosticNodelet("VideoToScene") {}
+    protected:
+        virtual void onInit();
+        virtual void subscribe();
+        virtual void unsubscribe();
+        virtual void work(const sensor_msgs::Image::ConstPtr& image_msg);
+        virtual void configCallback(Config &config, uint32_t level);
+
+        boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
+        image_transport::Subscriber sub_;
+        boost::shared_ptr<image_transport::ImageTransport> it_;
+        image_transport::Publisher pub_;
+        boost::mutex mutex_;
+    private:
+        cv::Ptr<cv::bgsegm::BackgroundSubtractorGMG> bgsubtractor;
+        int min_percent_;
+        int max_percent_;
+        bool captured_;
+
+    };
+}
+
+#endif // VIDEO_TO_SCENE_H_

jsk_perception/launch/video_to_scene.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+  <arg name="INPUT_IMAGE" />
+  <arg name="OUTPUT_IMAGE" default="$(arg INPUT_IMAGE)/output" />
+  <arg name="IMAGE_TRANSPORT" default="raw" />
+  <arg name="DEFAULT_NAMESPACE" default="video_to_scene" />
+  <arg name="launch_manager" default="true" />
+  <arg name="manager" default="manager" />
+  <arg name="min_percent" default="5" />
+  <arg name="max_percent" default="20" />
+
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)"
+        args="manager" output="screen" if="$(arg launch_manager)" />
+
+  <node pkg="nodelet" type="nodelet" name="$(arg DEFAULT_NAMESPACE)"
+        args="load jsk_perception/VideoToScene $(arg manager)"
+        respawn="true"
+        output="screen">
+    <param name="~image_transport" value="$(arg IMAGE_TRANSPORT)" />
+    <param name="~min_percent" value="$(arg min_percent)" />
+    <param name="~max_percent" value="$(arg max_percent)" />
+    <remap from="~input" to="$(arg INPUT_IMAGE)" />
+    <remap from="~output" to="$(arg OUTPUT_IMAGE)" />
+  </node>
+</launch>

jsk_perception/plugins/nodelet/libjsk_perception.xml

@@ -294,4 +294,8 @@
          type="jsk_perception::RemoveBlurredFrames"
          base_class_type="nodelet::Nodelet">
   </class>
+  <class name="jsk_perception/VideoToScene"
+         type="jsk_perception::VideoToScene"
+         base_class_type="nodelet::Nodelet">
+  </class>
 </library>

jsk_perception/sample/sample_video_to_scene.launch

@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="min_percent" default="8" />
+  <arg name="max_percent" default="20" />
+
+  <param name="/use_sim_time" value="true"/>
+  <node name="rosbag_play" pkg="rosbag" type="play"
+        args="$(find jsk_perception)/sample/data/2019-07-18-15-37-50_pr2_self_see.bag --clock --loop --topics /kinect_head/rgb/image_rect_color/compressed"/>
+
+  <include file="$(find jsk_perception)/launch/video_to_scene.launch" >
+    <arg name="INPUT_IMAGE" value="/kinect_head/rgb/image_rect_color" />
+    <arg name="IMAGE_TRANSPORT" default="compressed" />
+    <arg name="launch_manager" value="true" />
+    <arg name="manager" value="rosbag_play_nodelet_manager" />
+    <arg name="min_percent" value="$(arg min_percent)" />
+    <arg name="max_percent" value="$(arg max_percent)" />
+  </include>
+  <node pkg="rosservice" type="rosservice" name="set_logger_level"
+        args="call /rosbag_play_nodelet_manager/set_logger_level ros.jsk_perception./video_to_scene debug" />
+
+  <group if="$(arg gui)">
+    <node name="image_view_rgb"
+          pkg="image_view" type="image_view">
+      <remap from="image" to="/kinect_head/rgb/image_rect_color"/>
+      <param name="image_transport" value="compressed" />
+    </node>
+    <node name="image_view_fg"
+          pkg="image_view" type="image_view">
+      <remap from="image" to="/kinect_head/rgb/image_rect_color/output"/>
+      <param name="image_transport" value="compressed" />
+    </node>
+  </group>
+
+</launch>

jsk_perception/src/video_to_scene.cpp

@@ -0,0 +1,112 @@
+// -*- mode: C++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, 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.
+ *********************************************************************/
+/*
+ * video_to_scene.cpp
+ * Author: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
+ */
+
+#include "jsk_perception/video_to_scene.h"
+#include <boost/assign.hpp>
+
+namespace jsk_perception{
+    void VideoToScene::onInit(){
+        DiagnosticNodelet::onInit();
+
+        bgsubtractor = cv::bgsegm::createBackgroundSubtractorGMG();
+        pnh_->param("min_percent", min_percent_,  5);
+        pnh_->param("max_percent", max_percent_, 20);
+        captured_ = false;
+
+        srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> > (*pnh_);
+        dynamic_reconfigure::Server<Config>::CallbackType f =
+            boost::bind(&VideoToScene::configCallback, this, _1, _2);
+        srv_->setCallback (f);
+
+        //pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
+        pub_ = advertiseImage(*pnh_, "output", 1);
+
+        onInitPostProcess();
+    }
+
+    void VideoToScene::subscribe(){
+        std::string transport;
+        pnh_->param("image_transport", transport, std::string("raw"));
+        NODELET_INFO_STREAM("Using transport \"" << transport << "\"");
+        image_transport::TransportHints hints(transport, ros::TransportHints(), *pnh_);
+        //
+        it_.reset(new image_transport::ImageTransport(*pnh_));
+        sub_ = it_->subscribe(pnh_->resolveName("input"), 1, &VideoToScene::work, this, hints);
+        ros::V_string names = boost::assign::list_of("input");
+        jsk_topic_tools::warnNoRemap(names);
+    }
+
+    void VideoToScene::unsubscribe(){
+        sub_.shutdown();
+    }
+
+    void VideoToScene::work(const sensor_msgs::Image::ConstPtr& image_msg){
+        cv::Mat image;
+
+        vital_checker_ -> poke();
+        boost::mutex::scoped_lock lock(mutex_);
+
+        image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::RGB8) -> image;
+        cv::resize(image, image, cv::Size(), 300.0/image.cols, 300.0/image.cols);
+
+        cv::Mat bgmask;
+        bgsubtractor->apply(image, bgmask);
+        cv::erode(bgmask, bgmask, cv::Mat(), cv::Point(-1, -1), 2);
+        cv::dilate(bgmask, bgmask, cv::Mat(), cv::Point(-1, -1), 2);
+
+        int p = cv::countNonZero(bgmask) / float(bgmask.cols * bgmask.rows) * 100;
+        NODELET_DEBUG_STREAM("p = " << p << ", min_percent = " << min_percent_ << ", max_percent = " << max_percent_ << ", captured = " << captured_);
+
+        if ( p < min_percent_ && !captured_ ) {
+          captured_ = true;
+          pub_.publish(image_msg);
+        } else if ( captured_ && p >= max_percent_ ) {
+          captured_ = false;
+        }
+    }
+
+    void VideoToScene::configCallback(Config &config, uint32_t level){
+        boost::mutex::scoped_lock lock(mutex_);
+        min_percent_ = config.min_percent;
+        max_percent_ = config.max_percent;
+    }
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS (jsk_perception::VideoToScene, nodelet::Nodelet);

jsk_perception/test/video_to_scene.test

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+  <include file="$(find jsk_perception)/sample/sample_video_to_scene.launch">
+    <arg name="gui" value="false" />
+  </include>
+
+  <test test-name="test_video_to_scene"
+        name="test_video_to_scene"
+        pkg="jsk_tools" type="test_topic_published.py"
+        time-limit="20" retry="3">
+    <rosparam>
+      topic_0: /kinect_head/rgb/image_rect_color/output
+      timeout_0: 20
+      topic_1: /kinect_head/rgb/image_rect_color/output/compressed
+      timeout_1: 20
+      check_after_kill_node: true
+      node_names: [video_to_scene,]
+    </rosparam>
+  </test>
+
+</launch>