From d99a7588ea55281d45f87b125407aa48ae5089cf Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 16 Sep 2016 02:46:39 +0900 Subject: [PATCH] Add line segment detector(lsd) --- CMakeLists.txt | 11 +- cfg/LineSegmentDetector.cfg | 55 ++++ launch/line_segment_detector.launch | 34 +++ nodelet_plugins.xml | 4 + src/nodelet/line_segment_detector_nodelet.cpp | 258 ++++++++++++++++++ test/CMakeLists.txt | 4 + test/test-line_segment_detector.test | 35 +++ 7 files changed, 400 insertions(+), 1 deletion(-) create mode 100755 cfg/LineSegmentDetector.cfg create mode 100644 launch/line_segment_detector.launch create mode 100644 src/nodelet/line_segment_detector_nodelet.cpp create mode 100644 test/test-line_segment_detector.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 2086e71a..5e197e8b 100644 --- a/CMakeLists.txt +++ b/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 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 a6ca40ca..db4e08e9 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -52,6 +52,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 b73ce322..683507e7 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -35,6 +35,10 @@ 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_LESS "3.0") +else() + 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) 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +