Merge pull request #866 from iKrishneel/master

Nodelet to perform Sliding Window Object detection

jsk_perception/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS
   image_geometry image_transport image_view2 driver_base dynamic_reconfigure cmake_modules
   roscpp nodelet rostest tf rospack
   jsk_topic_tools pcl_ros)
+
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS filesystem system signals)
 
@@ -44,9 +45,10 @@ generate_dynamic_reconfigure_options(
   cfg/ColorHistogramSlidingMatcher.cfg
   cfg/BackgroundSubstraction.cfg
   cfg/GrabCut.cfg
-  cfg/Fisheye.cfg)
+  cfg/Fisheye.cfg
+  cfg/SlidingWindowObjectDetector.cfg)
 
-add_service_files(FILES EuclideanSegment.srv  SetTemplate.srv  WhiteBalancePoints.srv  WhiteBalance.srv)
+add_service_files(FILES EuclideanSegment.srv  SetTemplate.srv  WhiteBalancePoints.srv  WhiteBalance.srv NonMaximumSuppression.srv)
 
 generate_messages(
   DEPENDENCIES std_msgs sensor_msgs geometry_msgs jsk_recognition_msgs
@@ -118,11 +120,14 @@ jsk_perception_nodelet(src/polygon_to_mask_image.cpp "jsk_perception/PolygonToMa
 jsk_perception_nodelet(src/rect_to_roi.cpp "jsk_perception/RectToROI" "rect_to_roi")
 jsk_perception_nodelet(src/mask_image_generator.cpp "jsk_perception/MaskImageGenerator" "mask_image_generator")
 jsk_perception_nodelet(src/project_image_point.cpp "jsk_perception/ProjectImagePoint" "project_image_point")
+jsk_perception_nodelet(src/skeletonization_nodelet.cpp "jsk_perception/Skeletonization" "skeletonization")
+jsk_perception_nodelet(src/sliding_window_object_detector.cpp "jsk_perception/SlidingWindowObjectDetector" "SlidingWindowObjectDetector")
 
 # compiling jsk_perception library for nodelet
 add_library(${PROJECT_NAME} SHARED ${jsk_perception_nodelet_sources}
   ${CMAKE_CURRENT_BINARY_DIR}/build/patched-SLIC-Superpixels/slic.cpp
-  src/image_utils.cpp)
+  src/image_utils.cpp
+  src/histogram_of_oriented_gradients.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_gencpp)
 

jsk_perception/README.md

@@ -215,6 +215,26 @@ Dilate binary image.
 
   Iterations to dilate image.
 
+### jsk\_perception/Skeletonization
+![](images/skeletonization.png)
+
+The nodelet is used to obtain the single pixel skeleton of either edges, contours or regions. 
+An important use of this nodelet is to fix broken edges on 2D image by first applying Dilation operation to the edges such that the broken edges intersect. By using skeletonization, the dilated image single pixel skeleton structure can be obtained. Note that this is different from erosion, as erosion erodes the edges without preserving the structure. 
+
+For more information refer to [Wikipedia - Zhang-Suen thinning algorithm](http://rosettacode.org/wiki/Zhang-Suen_thinning_algorithm)
+
+
+#### Subscribing Topic
+* `~input` (`sensor_msgs/Image`)
+
+  Currently only supports "MONO8" (single channel 8 bit grayscale)
+
+#### Publishing Topic
+* `~output` (`sensor_msgs/Image`)
+
+  float image containing skeleton info of the input image. 1 presents the skeleton.
+
+
 ### jsk\_perception/GridLabel
 ![](images/grid_label.jpg)
 

jsk_perception/cfg/SlidingWindowObjectDetector.cfg

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'jsk_perception'
+try:
+    import imp
+    imp.find_module(PACKAGE)
+    from dynamic_reconfigure.parameter_generator_catkin import *;
+except:
+    import roslib; roslib.load_manifest(PACKAGE)
+    from dynamic_reconfigure.parameter_generator import *;
+
+from math import pi
+
+gen = ParameterGenerator ()
+
+gen.add("sliding_window_width", int_t, 0,"sliding window detector width", 32, 32, 32)
+gen.add("sliding_window_height", int_t, 0,"sliding window detector height", 64, 64, 64)
+gen.add("image_downsize", int_t, 0,"factor for resizing the image", 1, 1, 2)
+
+gen.add("svm_model_name", str_t, 0, "trained svm model name with path",  "svm.xml")
+gen.add("dataset_directory", str_t, 0, "directory for training data","NO TRAINING SET")
+
+gen.add("scaling_factor", double_t, 0,"sliding window scaling factor in each stack", -0.05, -0.90, 0.9)
+gen.add("stack_size", int_t, 0, "number of image stack in pyramid", 4, 1, 15)
+gen.add("sliding_window_increment", int_t, 0, "number of pixels to increment the window", 8, 1, 32)
+
+
+exit (gen.generate (PACKAGE, "jsk_perception", "SlidingWindowObjectDetector"))

jsk_perception/include/jsk_perception/histogram_of_oriented_gradients.h

@@ -0,0 +1,44 @@
+
+#ifndef _HISTOGRAM_OF_ORIENTED_GRADIENTS_H_
+#define _HISTOGRAM_OF_ORIENTED_GRADIENTS_H_
+
+// OpenCV Header Directives
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <string>
+#include <vector>
+
+class HOGFeatureDescriptor {
+
+    //  HOG Configuration Params
+#define N_BINS 9
+#define ANGLE 180.0
+#define BINS_ANGLE (ANGLE / N_BINS)
+#define CELL 8
+#define BLOCK 2
+
+ private:
+    virtual void bilinearBinVoting(
+       const float &, int &, int &);
+    virtual void imageGradient(
+       const cv::Mat &, cv::Mat &);
+    virtual cv::Mat blockGradient(
+       const int, const int, cv::Mat &);
+    virtual cv::Mat orientationistogram(
+       const cv::Mat&, const int &, const int &, bool = false);
+    virtual void getHOG(
+       const cv::Mat &, cv::Mat &, cv::Mat &);
+    template<typename T>
+    T computeHOGHistogramDistances(
+       const cv::Mat &, std::vector<cv::Mat> &,
+       const int = CV_COMP_BHATTACHARYYA);
+
+ public:
+    HOGFeatureDescriptor();
+    virtual cv::Mat computeHOG(
+       const cv::Mat &);
+};
+
+#endif  // _HISTOGRAM_OF_ORIENTED_GRADIENTS_H_

jsk_perception/include/jsk_perception/skeletonization.h

@@ -0,0 +1,78 @@
+// -*- mode: c++ -*- 
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, 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/o2r 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.
+ *********************************************************************/
+
+#ifndef JSK_PERCEPTION_SKELETONIZATION_H
+#define JSK_PERCEPTION_SKELETONIZATION_H
+
+// ROS Header Directives
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+#include <sensor_msgs/Image.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+// OpenCV Header Directives
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+namespace jsk_perception
+{
+   class Skeletonization: public jsk_topic_tools::DiagnosticNodelet
+   {
+    public:
+      Skeletonization(): DiagnosticNodelet("Skeletonization") {}
+
+    protected:
+      virtual void onInit();
+      virtual void subscribe();
+      virtual void unsubscribe();
+      virtual void imageCallback(const sensor_msgs::Image::ConstPtr&);
+      virtual void skeletonization(cv::Mat &);
+      virtual void iterativeThinning(
+         cv::Mat&, int);
+
+      boost::mutex mutex_;
+      ros::Subscriber sub_;
+      ros::Publisher pub_image_;
+
+    private:
+      enum { EVEN, ODD };
+   };
+}
+
+#endif  // JSK_PERCEPTION_SKELETONIZATION_H
+

jsk_perception/include/jsk_perception/sliding_window_object_detector.h

@@ -0,0 +1,103 @@
+
+#ifndef JSK_PERCEPTION_SLIDING_WINDOW_OBJECT_DETECTOR_H
+#define JSK_PERCEPTION_SLIDING_WINDOW_OBJECT_DETECTOR_H
+
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+#include <jsk_perception/histogram_of_oriented_gradients.h>
+#include <jsk_recognition_msgs/RectArray.h>
+
+#include <jsk_recognition_msgs/ClusterPointIndices.h>
+// #include <jsk_pcl_ros/pcl_conversion_util.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <jsk_perception/SlidingWindowObjectDetectorConfig.h>
+
+#include <pcl/point_types.h>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <fstream>
+#include <map>
+#include <algorithm>
+#include <utility>
+
+namespace jsk_perception
+{
+   class SlidingWindowObjectDetector: public jsk_topic_tools::DiagnosticNodelet,
+                                      public HOGFeatureDescriptor
+   {
+    public:
+      SlidingWindowObjectDetector(): DiagnosticNodelet("SlidingWindowObjectDetector")
+      {
+         // loadTrainedDetectorModel();
+      }
+
+      virtual void loadTrainedDetectorModel();
+      virtual void trainObjectClassifier();
+      virtual void readDataset(
+         std::string, std::vector<cv::Mat> &,
+         cv::Mat &, bool = false, const int = 0);
+      virtual void extractFeatures(
+         const std::vector<cv::Mat> &, cv::Mat &);
+      virtual void trainBinaryClassSVM(
+         const cv::Mat &, const cv::Mat &);
+      virtual std::vector<cv::Rect_<int> > runObjectRecognizer(
+         cv::Mat &, const cv::Size, const float, const int, const int);
+      virtual void objectRecognizer(
+         const cv::Mat &, std::multimap<float, cv::Rect_<int> > &,
+         const cv::Size, const int = 16);
+      virtual void pyramidialScaling(
+         cv::Size &, const float);
+      virtual std::vector<cv::Rect_<int> > nonMaximumSuppression(
+         std::multimap<float, cv::Rect_<int> > &, const float);
+
+      void convertCvRectToJSKRectArray(
+         const std::vector<cv::Rect_<int> > &,
+         jsk_recognition_msgs::RectArray &, const int, const cv::Size );
+      void concatenateCVMat(
+         const cv::Mat &, const cv::Mat &, cv::Mat &, bool = true);
+      virtual void configCallback(
+         jsk_perception::SlidingWindowObjectDetectorConfig &, uint32_t);
+
+    protected:
+      virtual void imageCb(
+       const sensor_msgs::ImageConstPtr&);
+
+      virtual void onInit();
+      virtual void subscribe();
+      virtual void unsubscribe();
+
+      boost::mutex mutex_;
+      ros::Subscriber sub_;
+      ros::Publisher pub_image_;
+      ros::Publisher pub_rects_;
+      ros::ServiceClient nms_client_;
+
+      int swindow_x;
+      int swindow_y;
+      float scale_;
+      int stack_size_;
+      int incrementor_;
+      int downsize_;
+
+      std::string model_name_;
+      std::string dataset_path_;
+      boost::shared_ptr<cv::SVM> supportVectorMachine_;
+      boost::shared_ptr<dynamic_reconfigure::Server<
+         jsk_perception::SlidingWindowObjectDetectorConfig> > srv_;
+
+   };
+
+} // namespace jsk_perception
+
+
+#endif  // JSK_PERCEPTION_SLIDING_WINDOW_OBJECT_DETECTOR_H

jsk_perception/jsk_perception_nodelets.xml

@@ -156,4 +156,16 @@
       Nodelet to convert fisheye image to panorama.
     </description>
   </class>
+  <class name="jsk_perception/Skeletonization" type="jsk_perception::Skeletonization"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet to thin the pixels to single dimension
+    </description>
+  </class>
+  <class name="jsk_perception/SlidingWindowObjectDetector" type="jsk_perception::SlidingWindowObjectDetector"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Base class for object detection via supervised learning
+    </description>
+  </class>
 </library>

jsk_perception/launch/skeletonization.launch

@@ -0,0 +1,15 @@
+<launch>
+  <node pkg="jsk_perception" type="edge_detector" name="edge_detector">
+    <remap from="image" to="/camera/rgb/image_rect_color" />
+  </node>
+  <node pkg="jsk_perception" type="dilate_mask_image" name="dilate_mask_image">
+    <remap from="~input" to="/edge/image" />
+  </node>
+  <node pkg="jsk_perception" type="skeletonization" name="skeletonization">
+    <remap from="~input" to="/dilate_mask_image/output" />
+  </node>
+  <node pkg="image_view" type="image_view" name="edge_view" >
+    <remap from="image" to="/edge/image" />
+  </node>
+  <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="edge_reconfigure" />
+</launch>

jsk_perception/launch/sliding_window_object_detector.launch

@@ -0,0 +1,13 @@
+<launch>
+<group>
+  <node pkg="jsk_perception" type="sliding_window_object_detector" name="sliding_window_object_detector" output="screen">
+  </node>
+  <node pkg="jsk_perception" type="non_maximum_suppression.py" name="non_maximum_suppression" output="screen">
+    <remap from="non_maximum_suppression" to="/sliding_window_object_detector/non_maximum_suppression" />
+  </node>
+</group>
+  <node pkg="image_view" type="image_view" name="detector" >
+    <remap from="image" to="/sliding_window_object_detector/output/image" />
+  </node>
+  <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="detector_reconfigure" />
+</launch>