Add line segment detector(lsd)

CMakeLists.txt

@@ -26,6 +26,7 @@ generate_dynamic_reconfigure_options(
   #
   cfg/CamShift.cfg
   cfg/FBackFlow.cfg
+  cfg/LineSegmentDetector.cfg
   cfg/LKFlow.cfg
   cfg/PeopleDetect.cfg
   cfg/PhaseCorr.cfg
@@ -87,6 +88,11 @@ catkin_package(CATKIN_DEPENDS std_msgs
 include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
 
 ## Declare a cpp library
+if(OpenCV_VERSION VERSION_LESS "3.0")
+else()
+    # 3.0 version
+    list(APPEND ${PROJECT_NAME}_EXTRA_FILES src/nodelet/line_segment_detector_nodelet.cpp)
+endif()
 if(OPENCV_HAVE_OPTFLOW)
   list(APPEND ${PROJECT_NAME}_EXTRA_FILES src/nodelet/simple_flow_nodelet.cpp)
 endif()
@@ -258,7 +264,10 @@ opencv_apps_add_nodelet(fback_flow fback_flow/fback_flow src/nodelet/fback_flow_
 # ./letter_recog.cpp
 opencv_apps_add_nodelet(lk_flow lk_flow/lk_flow src/nodelet/lk_flow_nodelet.cpp) # ./lkdemo.cpp
 # ./logistic_regression.cpp
-# ./lsd_lines.cpp
+if(OpenCV_VERSION VERSION_LESS "3.0")
+else()
+  opencv_apps_add_nodelet(line_segment_detector line_segment_detector/line_segment_detector src/nodelet/line_segment_detector_nodelet.cpp) # ./lsd_lines.cpp
+endif()
 # ./mask_tmpl.cpp
 # ./matchmethod_orb_akaze_brisk.cpp
 # ./minarea.cpp

cfg/LineSegmentDetector.cfg

@@ -0,0 +1,55 @@
+#! /usr/bin/env python
+# 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 Kei Okada 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.
+
+PACKAGE='line_segment_detector'
+
+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)
+
+lsd_refine_type = gen.enum([ gen.const("REFINE_NONE", int_t,  0, "No refinement applied"),
+                             gen.const("REFINE_STD",  int_t,  1, "Standard refinement is applied. E.g. breaking arches into smaller straighter line approximations."),
+                             gen.const("REFINE_ADV",  int_t,  2, "Advanced refinement. Number of false alarms is calculated, lines are refined through increase of precision, decrement in size, etc."),
+], "An enum for Line Segment Detector Modes")
+gen.add("lsd_refine_type", int_t, 0, "Line Segment Detector Modes", 0, 0, 2, edit_method=lsd_refine_type)
+gen.add("lsd_scale", double_t,    0, "The scale of the image that will be used to find the lines. Range (0..1]", 0.8, 0.1, 1.0);
+gen.add("lsd_sigma_scale", double_t,    0, "Sigma for Gaussian filter. It is computed as sigma = _sigma_scale/_scale", 0.6, 0.1, 10.0);
+gen.add("lsd_quant", double_t,    0, "Bound to the quantization error on the gradient norm", 2.0, 0.0, 100.0);
+gen.add("lsd_angle_threshold", double_t,    0, "Gradient angle tolerance in degrees", 22.5, 0.1, 179.0);
+gen.add("lsd_log_eps", double_t,    0, "Detection threshold: -log10(NFA) > log_eps. Used only when advancent refinement is chosen", 0.0, 0.0, 360.0);
+gen.add("lsd_density_threshold", double_t,    0, "Minimal density of aligned region points in the enclosing rectangle", 0.7, 0.0, 0.9);
+gen.add("lsd_n_bins", int_t,    0, "Number of bins in pseudo-ordering of gradient modulus", 1024, 1, 10000)
+gen.add("lsd_line_length_threshold", double_t,    0, "Threshold of line length.", 100.0, 0.0, 1000.0)
+
+exit(gen.generate(PACKAGE, "line_segment_detector", "LineSegmentDetector"))

launch/line_segment_detector.launch

@@ -0,0 +1,34 @@
+<launch>
+  <arg name="node_name" default="line_segment_detector" />
+
+  <arg name="image" default="image" 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="lsd_refine_type" default="2" doc="Line Segment Detector Modes. 0: No refinement applied, 1: Standard refinement is applied, 2, Advanced refinement is applied."/>
+  <arg name="lsd_scale" default="0.8" doc="The scale of the image that will be used to find the lines. Range (0..1]" />
+  <arg name="lsd_sigma_scale" default="0.6" doc="Sigma for Gaussian filter. It is computed as sigma = _sigma_scale/_scale" />
+  <arg name="lsd_quant" default="2.0" doc="Bound to the quantization error on the gradient norm" />
+  <arg name="lsd_angle_threshold" default="22.5" doc="Gradient angle tolerance in degrees" />
+  <arg name="lsd_log_eps" default="0" doc="Detection threshold: -log10(NFA) > log_eps. Used only when advancent refinement is chosen" />
+  <arg name="lsd_density_threshold" default="0.3" doc="Minimal density of aligned region points in the enclosing rectangle" />
+  <arg name="lsd_n_bins" default="1024" doc="Number of bins in pseudo-ordering of gradient modulus" />
+  <arg name="lsd_line_length_threshold" default="0.0" doc="Threshold of line length" />
+
+  <!-- line_segment_detector.cpp  -->
+  <node name="$(arg node_name)" pkg="opencv_apps" type="line_segment_detector" >
+    <remap from="image" to="$(arg image)" />
+    <param name="use_camera_info" value="$(arg use_camera_info)" />
+    <param name="debug_view" value="$(arg debug_view)" />
+    <param name="lsd_refine_type" value="$(arg lsd_refine_type)" />
+    <param name="lsd_scale" value="$(arg lsd_scale)" />
+    <param name="lsd_sigma_scale" value="$(arg lsd_sigma_scale)" />
+    <param name="lsd_quant" value="$(arg lsd_quant)" />
+    <param name="lsd_angle_threshold" value="$(arg lsd_angle_threshold)" />
+    <param name="lsd_log_eps" value="$(arg lsd_log_eps)" />
+    <param name="lsd_density_threshold" value="$(arg lsd_density_threshold)" />
+    <param name="lsd_n_bins" value="$(arg lsd_n_bins)" />
+    <param name="lsd_line_length_threshold" value="$(arg lsd_line_length_threshold)" />
+  </node>
+</launch>

nodelet_plugins.xml

@@ -52,6 +52,10 @@
     <description>Nodelet to demonstrates dense optical flow algorithm by Gunnar Farneback</description>
   </class>
 
+  <class name="line_segment_detector/line_segment_detector" type="line_segment_detector::LineSegmentDetectorNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet to detector line segments</description>
+  </class>
+
   <class name="lk_flow/lk_flow" type="lk_flow::LKFlowNodelet" base_class_type="nodelet::Nodelet">
     <description>Nodelet to calculate Lukas-Kanade optical flow</description>
   </class>

src/nodelet/line_segment_detector_nodelet.cpp

@@ -0,0 +1,258 @@
+// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
+/*********************************************************************
+* 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 Kei Okada 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.
+*********************************************************************/
+
+/**
+ * @file https://github.com/opencv/opencv/blob/master/samples/cpp/lsd_lines.cpp
+ * @brief Sample code showing how to detect line segments using the LineSegmentDetector
+ * @author OpenCV team
+ */
+
+#include <ros/ros.h>
+#include "opencv_apps/nodelet.h"
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <dynamic_reconfigure/server.h>
+#include "opencv_apps/Line.h"
+#include "opencv_apps/LineArrayStamped.h"
+#include "opencv_apps/LineSegmentDetectorConfig.h"
+
+
+namespace line_segment_detector {
+class LineSegmentDetectorNodelet : 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<image_transport::ImageTransport> it_;
+
+  typedef line_segment_detector::LineSegmentDetectorConfig Config;
+  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+  Config config_;
+  boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+
+  cv::Ptr<cv::LineSegmentDetector> lsd_;
+
+  bool debug_view_;
+
+  int lsd_refine_;
+  double lsd_scale_;
+  double lsd_sigma_scale_;
+  double lsd_quant_;
+  double lsd_angle_threshold_;
+  double lsd_log_eps_;
+  double lsd_density_threshold_;
+  int lsd_n_bins_;
+  double lsd_line_length_threshold_;
+
+  std::string window_name_;
+  static bool need_config_update_;
+
+  boost::mutex mutex_;
+
+  void updateLSD() {
+    lsd_ = cv::createLineSegmentDetector(lsd_refine_, lsd_scale_,
+                                         lsd_sigma_scale_, lsd_quant_,
+                                         lsd_angle_threshold_, lsd_log_eps_,
+                                         lsd_density_threshold_, lsd_n_bins_);
+  }
+
+  void reconfigureCallback(Config &new_config, uint32_t level)
+  {
+    boost::mutex::scoped_lock lock (mutex_);
+    config_ = new_config;
+
+    lsd_refine_ = config_.lsd_refine_type;
+    lsd_scale_ = config_.lsd_scale;
+    lsd_sigma_scale_ = config_.lsd_sigma_scale;
+    lsd_angle_threshold_ = config_.lsd_angle_threshold;
+    lsd_log_eps_ = config_.lsd_log_eps;
+    lsd_density_threshold_ = config_.lsd_density_threshold;
+    lsd_n_bins_ = config_.lsd_n_bins;
+    lsd_line_length_threshold_ = config_.lsd_line_length_threshold;
+
+    updateLSD();
+  }
+
+  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)
+  {
+    boost::mutex::scoped_lock lock (mutex_);
+    // Work on the image.
+    try
+    {
+      // Convert the image into something opencv can handle.
+      cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
+
+      // Do the work
+      cv::Mat src_gray;
+      /// Convert it to gray
+      if ( frame.channels() > 1 ) {
+        cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
+      } else {
+        src_gray = frame;
+      }
+
+      /// Create window
+      if( debug_view_) {
+        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
+      }
+
+      cv::Mat line_image;
+
+      std::vector<cv::Vec4i> lines;
+      std::vector<float> widths;
+      std::vector<double> precs;
+      std::vector<double> nfas;
+      lsd_->detect(src_gray, lines, widths, precs, nfas);
+      line_image = cv::Mat::zeros(frame.rows, frame.cols, CV_8UC1);
+
+      // Messages
+      opencv_apps::LineArrayStamped lines_msg;
+      lines_msg.header = msg->header;
+
+      // draw lines
+      for(size_t i = 0; i < lines.size(); ++i ) {
+          int x1 = lines[i][0];
+          int y1 = lines[i][1];
+          int x2 = lines[i][2];
+          int y2 = lines[i][3];
+          double line_length = sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
+
+          if (line_length >= lsd_line_length_threshold_) {
+              cv::line(line_image, cv::Point(x1, y1), cv::Point(x2, y2), 255);
+          }
+
+          opencv_apps::Line line_msg;
+          line_msg.pt1.x = x1;
+          line_msg.pt1.y = y1;
+          line_msg.pt2.x = x2;
+          line_msg.pt2.y = y2;
+          lines_msg.lines.push_back(line_msg);
+      }
+
+      if( debug_view_) {
+        cv::imshow( window_name_, line_image );
+        int c = cv::waitKey(1);
+      }
+
+      // Publish the image.
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, line_image).toImageMsg();
+      img_pub_.publish(out_img);
+      // Publish the detected lines.
+      msg_pub_.publish(lines_msg);
+    }
+    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);
+    }
+
+  }
+
+  void subscribe()
+  {
+    NODELET_DEBUG("Subscribing to image topic.");
+    if (config_.use_camera_info)
+      cam_sub_ = it_->subscribeCamera("image", 3, &LineSegmentDetectorNodelet::imageCallbackWithInfo, this);
+    else
+      img_sub_ = it_->subscribe("image", 3, &LineSegmentDetectorNodelet::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<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
+
+    pnh_->param("debug_view", debug_view_, false);
+
+    if (debug_view_) {
+      always_subscribe_ = true;
+    }
+
+    window_name_ = "Line Segment Detector Demo";
+
+    reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
+    dynamic_reconfigure::Server<Config>::CallbackType f =
+      boost::bind(&LineSegmentDetectorNodelet::reconfigureCallback, this, _1, _2);
+    reconfigure_server_->setCallback(f);
+
+    updateLSD();
+
+    img_pub_ = advertiseImage(*pnh_, "image", 1);
+    msg_pub_ = advertise<opencv_apps::LineArrayStamped>(*pnh_, "lines", 1);
+
+    onInitPostProcess();
+  }
+};
+bool LineSegmentDetectorNodelet::need_config_update_ = false;
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(line_segment_detector::LineSegmentDetectorNodelet, nodelet::Nodelet);