diff --git a/CMakeLists.txt b/CMakeLists.txt
index f801c2b9..988cac55 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,6 +27,7 @@ generate_dynamic_reconfigure_options(
#
cfg/CamShift.cfg
cfg/FBackFlow.cfg
+ cfg/LineSegmentDetector.cfg
cfg/LKFlow.cfg
cfg/PeopleDetect.cfg
cfg/PhaseCorr.cfg
@@ -88,6 +89,9 @@ catkin_package(CATKIN_DEPENDS std_msgs
include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
## Declare a cpp library
+if(OpenCV_VERSION VERSION_GREATER "2.9.9")
+ 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()
@@ -259,7 +263,9 @@ 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_GREATER "2.9.9")
+ 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
@@ -339,7 +345,11 @@ if(CATKIN_ENABLE_TESTING)
message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}")
else()
file(GLOB LAUNCH_FILES launch/*.launch)
+ if(NOT (TARGET line_segment_detector_exe))
+ list(REMOVE_ITEM LAUNCH_FILES launch/line_segment_detector.launch)
+ endif()
foreach(LAUNCH_FILE ${LAUNCH_FILES})
+ message(STATUS "${LAUNCH_FILE}")
roslaunch_add_file_check(${LAUNCH_FILE})
endforeach()
endif()
diff --git a/cfg/LineSegmentDetector.cfg b/cfg/LineSegmentDetector.cfg
new file mode 100755
index 00000000..847875ab
--- /dev/null
+++ b/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"))
diff --git a/launch/line_segment_detector.launch b/launch/line_segment_detector.launch
new file mode 100644
index 00000000..0d08d2de
--- /dev/null
+++ b/launch/line_segment_detector.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml
index 24cdf862..7645e119 100644
--- a/nodelet_plugins.xml
+++ b/nodelet_plugins.xml
@@ -55,6 +55,10 @@
Nodelet to demonstrates dense optical flow algorithm by Gunnar Farneback
+
+ Nodelet to detector line segments
+
+
Nodelet to calculate Lukas-Kanade optical flow
diff --git a/src/nodelet/line_segment_detector_nodelet.cpp b/src/nodelet/line_segment_detector_nodelet.cpp
new file mode 100644
index 00000000..6cbbec0e
--- /dev/null
+++ b/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
+#include "opencv_apps/nodelet.h"
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#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 it_;
+
+ typedef line_segment_detector::LineSegmentDetectorConfig Config;
+ typedef dynamic_reconfigure::Server ReconfigureServer;
+ Config config_;
+ boost::shared_ptr reconfigure_server_;
+
+ cv::Ptr 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 lines;
+ std::vector widths;
+ std::vector precs;
+ std::vector 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(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 >(*pnh_);
+ dynamic_reconfigure::Server::CallbackType f =
+ boost::bind(&LineSegmentDetectorNodelet::reconfigureCallback, this, _1, _2);
+ reconfigure_server_->setCallback(f);
+
+ updateLSD();
+
+ img_pub_ = advertiseImage(*pnh_, "image", 1);
+ msg_pub_ = advertise(*pnh_, "lines", 1);
+
+ onInitPostProcess();
+ }
+};
+bool LineSegmentDetectorNodelet::need_config_update_ = false;
+}
+
+#include
+PLUGINLIB_EXPORT_CLASS(line_segment_detector::LineSegmentDetectorNodelet, nodelet::Nodelet);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a22a9998..843977ca 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -36,6 +36,9 @@ add_rostest(test-goodfeature_track.test ARGS gui:=false)
add_rostest(test-camshift.test ARGS gui:=false)
add_rostest(test-fback_flow.test ARGS gui:=false)
add_rostest(test-lk_flow.test ARGS gui:=false)
+if(OpenCV_VERSION VERSION_GREATER "2.9.9")
+ add_rostest(test-line_segment_detector.test ARGS gui:=false)
+endif()
add_rostest(test-people_detect.test ARGS gui:=false)
add_rostest(test-phase_corr.test ARGS gui:=false)
add_rostest(test-segment_objects.test ARGS gui:=false)
@@ -52,9 +55,11 @@ if(roslaunch_VERSION VERSION_LESS "1.11.1")
message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}")
else()
file(GLOB LAUNCH_FILES *.test)
+ if(NOT (TARGET line_segment_detector_exe))
+ list(REMOVE_ITEM LAUNCH_FILES test-line_segment_detector.test)
+ endif()
foreach(LAUNCH_FILE ${LAUNCH_FILES})
+ message(STATUS "${LAUNCH_FILE}")
roslaunch_add_file_check(${LAUNCH_FILE})
endforeach()
endif()
-
-
diff --git a/test/test-line_segment_detector.test b/test/test-line_segment_detector.test
new file mode 100644
index 00000000..dab7007d
--- /dev/null
+++ b/test/test-line_segment_detector.test
@@ -0,0 +1,35 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+