├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── images └── line_extraction.gif ├── include └── laser_line_extraction │ ├── line.h │ ├── line_extraction.h │ ├── line_extraction_ros.h │ └── utilities.h ├── launch ├── debug.launch └── example.launch ├── msg ├── LineSegment.msg └── LineSegmentList.msg ├── package.xml └── src ├── line.cpp ├── line_extraction.cpp ├── line_extraction_node.cpp └── line_extraction_ros.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.swp 3 | *.swo 4 | *~ 5 | bin/* 6 | build/* 7 | data/* 8 | msg_gen/* 9 | src/__pycache__/* 10 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_line_extraction) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cmake_modules 6 | geometry_msgs 7 | message_generation 8 | roscpp 9 | rospy 10 | sensor_msgs 11 | visualization_msgs 12 | ) 13 | 14 | find_package(Eigen3 REQUIRED) 15 | 16 | add_message_files( 17 | FILES 18 | LineSegment.msg 19 | LineSegmentList.msg 20 | ) 21 | 22 | generate_messages( 23 | DEPENDENCIES 24 | sensor_msgs 25 | ) 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | LIBRARIES line line_extraction line_extraction_ros 30 | CATKIN_DEPENDS geometry_msgs message_runtime roscpp sensor_msgs visualization_msgs 31 | ) 32 | 33 | add_library(line src/line.cpp) 34 | target_link_libraries(line ${catkin_LIBRARIES}) 35 | 36 | add_library(line_extraction src/line_extraction.cpp) 37 | target_link_libraries(line_extraction ${catkin_LIBRARIES}) 38 | 39 | add_library(line_extraction_ros src/line_extraction_ros.cpp) 40 | target_link_libraries(line_extraction_ros line line_extraction ${catkin_LIBRARIES}) 41 | add_dependencies(line_extraction_ros laser_line_extraction_generate_messages_cpp) 42 | 43 | add_executable(line_extraction_node src/line_extraction_node.cpp) 44 | target_link_libraries(line_extraction_node line_extraction_ros ${catkin_LIBRARIES}) 45 | 46 | include_directories(include ${catkin_INCLUDE_DIRS}) 47 | include_directories(include ${EIGEN3_INCLUDE_DIRS}) 48 | 49 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_laser_line_extraction.cpp) 50 | 51 | install(TARGETS line_extraction_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 52 | install(TARGETS line_extraction_ros line_extraction line ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 53 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 54 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Marc Gallant 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Laser Line Extraction 2 | Laser Line Extraction is a [Robot Operating System (ROS)](http://www.ros.org) package that extracts line segments form [LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html) messages. Created by [Marc Gallant](http://marcgallant.ca), originally for use in the [Mining Systems Laboratory](http://msl.engineering.queensu.ca). Here is what the Laser Line Extraction package looks like in action: 3 | 4 | ![Laser line extraction](images/line_extraction.gif) 5 | 6 | In the above image, the white dots are points in a LaserScan message, and the red lines are what is extracted by Laser Line Extraction. This data was collected by driving a robot through Beamish-Munro Hall at Queen's University. A SICK LMS111 laser scanner was mounted to the robot. The extraction algorithm is very configurable; the above image used the parameters configured in the `example.launch` launch file. 7 | 8 | After applying some filters to remove outlying points, Laser Line Extraction implements a split-and-merge algorithm to determine which points belong to lines. Next, it implements the weighted line fitting algorithm by Pfister *et al.* [1] to find the best fit lines and their respective covariance matrices. 9 | 10 | ## Usage 11 | I recommend making a copy of `example.launch` in the launch directory and configuring the parameters until you reach a desirable outcome. The parameters in `example.launch` are a good starting point. Then simply use roslaunch, e.g., 12 | 13 | ``` 14 | roslaunch laser_line_extraction example.launch 15 | ``` 16 | 17 | ## Messages 18 | Laser Line Extraction has two messages types: 19 | 20 | #### LineSegment.msg 21 | ``` 22 | float32 radius 23 | float32 angle 24 | float32[4] covariance 25 | float32[2] start 26 | float32[2] end 27 | ``` 28 | `radius` (m) and `angle` (rad) are the polar parameterization of the line segment. `covariance` is the 2x2 covariance matrix of `radius` and `angle` (listed in row-major order). Finally `start` and `end` are the (x, y) coordinates of the start and end of the line segment. 29 | 30 | #### LineSegmentList.msg 31 | ``` 32 | Header header 33 | LineSegment[] line_segments 34 | ``` 35 | An array of LineSegment.msg with a header. 36 | 37 | ## Topics 38 | 39 | Laser Line Extraction subscribes to a single topic and publishes one or two topics. 40 | 41 | ### Subscribed topics 42 | - `/scan` ([sensor_msgs/LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html)) 43 | - The name of this topic can be configured (see Parameters). 44 | 45 | ### Published topics 46 | - `/line_segments` (laser\_line\_extraction/LineSegmentList) 47 | - A list of line segments extracted from a laser scan. 48 | - `/line_markers` ([visualization_msgs/Marker](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)) 49 | - (optional) Markers so that the extracted lines can be visualized in rviz (see above image). Can be toggled (see Parameters). 50 | 51 | ## Parameters 52 | The parameters are listed in alphabetical order. 53 | 54 | - `bearing_std_dev` (default: 0.001) 55 | - The standard deviation of bearing uncertainty in the laser scans (rad). 56 | - `frame_id` (default: "laser") 57 | - The frame in which the line segments are published. 58 | - `least_sq_angle_thresh` (default: 0.0001) 59 | - Change in angle (rad) threshold to stop iterating least squares (`least_sq_radius_thresh` must also be met). 60 | - `least_sq_radius_thresh` (default: 0.0001) 61 | - Change in radius (m) threshold to stop iterating least squares (`least_sq_angle_thresh` must also be met). 62 | - `max_line_gap` (default: 0.4) 63 | - The maximum distance between two points in the same line (m). 64 | - `min_line_length` (default: 0.5) 65 | - Lines shorter than this are not published (m). 66 | - `min_line_points` (default: 9) 67 | - Lines with fewer points than this are not published. 68 | - `min_range` (default: 0.4) 69 | - Points closer than this are ignored (m). 70 | - `max_range` (default: 10000.0) 71 | - Points farther than this are ignored (m). 72 | - `min_split_dist` (default: 0.05) 73 | - When performing "split" step of split and merge, a split between two points results when the two points are at least this far apart (m). 74 | - `outlier_dist` (default: 0.05) 75 | - Points who are at least this distance from all their neighbours are considered outliers (m). 76 | - `publish_markers` (default: false) 77 | - Whether or not markers are published. 78 | - `range_std_dev` (default: 0.02) 79 | - The standard deviation of range uncertainty in the laser scans (m). 80 | - `scan_topic` (default: "scan") 81 | - The LaserScan topic. 82 | 83 | ## References 84 | [1] S. T. Pfister, S. I. Roumeliotis, and J. W. Burdick, "Weighted line fitting algorithms for mobile robot map building and efficient data representation" in Proc. IEEE Intl. Conf. on Robotics and Automation (ICRA), Taipei, Taiwan, 14-19 Sept., 2003. 85 | -------------------------------------------------------------------------------- /images/line_extraction.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kam3k/laser_line_extraction/34de3e9d7560c04bec29e97f07339407c0bca6a6/images/line_extraction.gif -------------------------------------------------------------------------------- /include/laser_line_extraction/line.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_LINE_H 2 | #define LINE_EXTRACTION_LINE_H 3 | 4 | #include 5 | #include 6 | #include "laser_line_extraction/utilities.h" 7 | 8 | namespace line_extraction 9 | { 10 | 11 | class Line 12 | { 13 | 14 | public: 15 | // Constructor / destructor 16 | Line(const CachedData&, const RangeData&, const Params&, std::vector); 17 | Line(double angle, double radius, const boost::array &covariance, 18 | const boost::array &start, const boost::array &end, 19 | const std::vector &indices); 20 | ~Line(); 21 | // Get methods for the line parameters 22 | double getAngle() const; 23 | const boost::array& getCovariance() const; 24 | const boost::array& getEnd() const; 25 | const std::vector& getIndices() const; 26 | double getRadius() const; 27 | const boost::array& getStart() const; 28 | // Methods for line fitting 29 | double distToPoint(unsigned int); 30 | void endpointFit(); 31 | void leastSqFit(); 32 | double length() const; 33 | unsigned int numPoints() const; 34 | void projectEndpoints(); 35 | 36 | private: 37 | std::vector indices_; 38 | // Data structures 39 | CachedData c_data_; 40 | RangeData r_data_; 41 | Params params_; 42 | PointParams p_params_; 43 | // Point variances used for least squares 44 | std::vector point_scalar_vars_; 45 | std::vector > point_covs_; 46 | double p_rr_; 47 | // Line parameters 48 | double angle_; 49 | double radius_; 50 | boost::array start_; 51 | boost::array end_; 52 | boost::array covariance_; 53 | // Methods 54 | void angleFromEndpoints(); 55 | void angleFromLeastSq(); 56 | double angleIncrement(); 57 | void calcCovariance(); 58 | void calcPointCovariances(); 59 | void calcPointParameters(); 60 | void calcPointScalarCovariances(); 61 | void radiusFromEndpoints(); 62 | void radiusFromLeastSq(); 63 | }; // class Line 64 | 65 | } // namespace line_extraction 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /include/laser_line_extraction/line_extraction.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_H 2 | #define LINE_EXTRACTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "laser_line_extraction/utilities.h" 9 | #include "laser_line_extraction/line.h" 10 | 11 | namespace line_extraction 12 | { 13 | 14 | class LineExtraction 15 | { 16 | 17 | public: 18 | // Constructor / destructor 19 | LineExtraction(); 20 | ~LineExtraction(); 21 | // Run 22 | void extractLines(std::vector&); 23 | // Data setting 24 | void setCachedData(const std::vector&, const std::vector&, 25 | const std::vector&, const std::vector&); 26 | void setRangeData(const std::vector&); 27 | // Parameter setting 28 | void setBearingVariance(double); 29 | void setRangeVariance(double); 30 | void setLeastSqAngleThresh(double); 31 | void setLeastSqRadiusThresh(double); 32 | void setMaxLineGap(double); 33 | void setMinLineLength(double); 34 | void setMinLinePoints(unsigned int); 35 | void setMinRange(double); 36 | void setMaxRange(double); 37 | void setMinSplitDist(double); 38 | void setOutlierDist(double); 39 | 40 | private: 41 | // Data structures 42 | CachedData c_data_; 43 | RangeData r_data_; 44 | Params params_; 45 | // Indices after filtering 46 | std::vector filtered_indices_; 47 | // Line data 48 | std::vector lines_; 49 | // Methods 50 | double chiSquared(const Eigen::Vector2d&, const Eigen::Matrix2d&, 51 | const Eigen::Matrix2d&); 52 | double distBetweenPoints(unsigned int index_1, unsigned int index_2); 53 | void filterCloseAndFarPoints(); 54 | void filterOutlierPoints(); 55 | void filterLines(); 56 | void mergeLines(); 57 | void split(const std::vector&); 58 | }; 59 | 60 | } // namespace line_extraction 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /include/laser_line_extraction/line_extraction_ros.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_ROS_H 2 | #define LINE_EXTRACTION_ROS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "laser_line_extraction/LineSegment.h" 11 | #include "laser_line_extraction/LineSegmentList.h" 12 | #include "laser_line_extraction/line_extraction.h" 13 | #include "laser_line_extraction/line.h" 14 | 15 | namespace line_extraction 16 | { 17 | 18 | class LineExtractionROS 19 | { 20 | 21 | public: 22 | // Constructor / destructor 23 | LineExtractionROS(ros::NodeHandle&, ros::NodeHandle&); 24 | ~LineExtractionROS(); 25 | // Running 26 | void run(); 27 | 28 | private: 29 | // ROS 30 | ros::NodeHandle nh_; 31 | ros::NodeHandle nh_local_; 32 | ros::Subscriber scan_subscriber_; 33 | ros::Publisher line_publisher_; 34 | ros::Publisher marker_publisher_; 35 | // Parameters 36 | std::string frame_id_; 37 | std::string scan_topic_; 38 | bool pub_markers_; 39 | // Line extraction 40 | LineExtraction line_extraction_; 41 | bool data_cached_; // true after first scan used to cache data 42 | // Members 43 | void loadParameters(); 44 | void populateLineSegListMsg(const std::vector&, laser_line_extraction::LineSegmentList&); 45 | void populateMarkerMsg(const std::vector&, visualization_msgs::Marker&); 46 | void cacheData(const sensor_msgs::LaserScan::ConstPtr&); 47 | void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr&); 48 | }; 49 | 50 | } // namespace line_extraction 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /include/laser_line_extraction/utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_EXTRACTION_UTILITIES_H 2 | #define LINE_EXTRACTION_UTILITIES_H 3 | 4 | #include 5 | #include 6 | 7 | namespace line_extraction 8 | { 9 | 10 | struct CachedData 11 | { 12 | std::vector indices; 13 | std::vector bearings; 14 | std::vector cos_bearings; 15 | std::vector sin_bearings; 16 | }; 17 | 18 | struct RangeData 19 | { 20 | std::vector ranges; 21 | std::vector xs; 22 | std::vector ys; 23 | }; 24 | 25 | struct Params 26 | { 27 | double bearing_var; 28 | double range_var; 29 | double least_sq_angle_thresh; 30 | double least_sq_radius_thresh; 31 | double max_line_gap; 32 | double min_line_length; 33 | double min_range; 34 | double max_range; 35 | double min_split_dist; 36 | double outlier_dist; 37 | unsigned int min_line_points; 38 | }; 39 | 40 | struct PointParams 41 | { 42 | std::vector a; 43 | std::vector ap; 44 | std::vector app; 45 | std::vector b; 46 | std::vector bp; 47 | std::vector bpp; 48 | std::vector c; 49 | std::vector s; 50 | }; 51 | 52 | // Inlining this function will be faster 53 | // and also get rid of multiple definitions 54 | // error 55 | inline double pi_to_pi(double angle) 56 | { 57 | angle = fmod(angle, 2 * M_PI); 58 | if (angle >= M_PI) 59 | angle -= 2 * M_PI; 60 | return angle; 61 | } 62 | 63 | } // namespace line_extraction 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /launch/debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /msg/LineSegment.msg: -------------------------------------------------------------------------------- 1 | float32 radius 2 | float32 angle 3 | float32[4] covariance 4 | float32[2] start 5 | float32[2] end 6 | -------------------------------------------------------------------------------- /msg/LineSegmentList.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | LineSegment[] line_segments 3 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_line_extraction 4 | 0.1.0 5 | 6 | A ROS package to extract line segments from LaserScan messages. 7 | 8 | Marc Gallant 9 | BSD 10 | catkin 11 | cmake_modules 12 | eigen 13 | geometry_msgs 14 | message_generation 15 | roscpp 16 | rospy 17 | sensor_msgs 18 | visualization_msgs 19 | geometry_msgs 20 | message_runtime 21 | roscpp 22 | rospy 23 | sensor_msgs 24 | visualization_msgs 25 | 26 | -------------------------------------------------------------------------------- /src/line.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line.h" 2 | 3 | namespace line_extraction 4 | { 5 | 6 | /////////////////////////////////////////////////////////////////////////////// 7 | // Constructor / destructor 8 | /////////////////////////////////////////////////////////////////////////////// 9 | Line::Line(const CachedData &c_data, const RangeData &r_data, const Params ¶ms, 10 | std::vector indices): 11 | c_data_(c_data), 12 | r_data_(r_data), 13 | params_(params), 14 | indices_(indices) 15 | { 16 | } 17 | 18 | Line::Line(double angle, double radius, const boost::array &covariance, 19 | const boost::array &start, const boost::array &end, 20 | const std::vector &indices): 21 | angle_(angle), 22 | radius_(radius), 23 | covariance_(covariance), 24 | start_(start), 25 | end_(end), 26 | indices_(indices) 27 | { 28 | } 29 | 30 | Line::~Line() 31 | { 32 | } 33 | 34 | /////////////////////////////////////////////////////////////////////////////// 35 | // Get methods for line parameters 36 | /////////////////////////////////////////////////////////////////////////////// 37 | double Line::getAngle() const 38 | { 39 | return angle_; 40 | } 41 | 42 | const boost::array& Line::getCovariance() const 43 | { 44 | return covariance_; 45 | } 46 | 47 | const boost::array& Line::getEnd() const 48 | { 49 | return end_; 50 | } 51 | 52 | const std::vector& Line::getIndices() const 53 | { 54 | return indices_; 55 | } 56 | 57 | double Line::getRadius() const 58 | { 59 | return radius_; 60 | } 61 | 62 | const boost::array& Line::getStart() const 63 | { 64 | return start_; 65 | } 66 | 67 | /////////////////////////////////////////////////////////////////////////////// 68 | // Utility methods 69 | /////////////////////////////////////////////////////////////////////////////// 70 | double Line::distToPoint(unsigned int index) 71 | { 72 | double p_rad = sqrt(pow(r_data_.xs[index], 2) + pow(r_data_.ys[index], 2)); 73 | double p_ang = atan2(r_data_.ys[index], r_data_.xs[index]); 74 | return fabs(p_rad * cos(p_ang - angle_) - radius_); 75 | } 76 | 77 | double Line::length() const 78 | { 79 | return sqrt(pow(start_[0] - end_[0], 2) + pow(start_[1] - end_[1], 2)); 80 | } 81 | 82 | unsigned int Line::numPoints() const 83 | { 84 | return indices_.size(); 85 | } 86 | 87 | void Line::projectEndpoints() 88 | { 89 | double s = -1.0 / tan(angle_); 90 | double b = radius_ / sin(angle_); 91 | double x = start_[0]; 92 | double y = start_[1]; 93 | start_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); 94 | start_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); 95 | x = end_[0]; 96 | y = end_[1]; 97 | end_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); 98 | end_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); 99 | } 100 | 101 | /////////////////////////////////////////////////////////////////////////////// 102 | // Methods for endpoint line fitting 103 | /////////////////////////////////////////////////////////////////////////////// 104 | void Line::endpointFit() 105 | { 106 | start_[0] = r_data_.xs[indices_[0]]; 107 | start_[1] = r_data_.ys[indices_[0]]; 108 | end_[0] = r_data_.xs[indices_.back()]; 109 | end_[1] = r_data_.ys[indices_.back()]; 110 | angleFromEndpoints(); 111 | radiusFromEndpoints(); 112 | } 113 | 114 | void Line::angleFromEndpoints() 115 | { 116 | double slope; 117 | if (fabs(end_[0] - start_[0]) > 1e-9) 118 | { 119 | slope = (end_[1] - start_[1]) / (end_[0] - start_[0]); 120 | angle_ = pi_to_pi(atan(slope) + M_PI/2); 121 | } 122 | else 123 | { 124 | angle_ = 0.0; 125 | } 126 | } 127 | 128 | void Line::radiusFromEndpoints() 129 | { 130 | radius_ = start_[0] * cos(angle_) + start_[1] * sin(angle_); 131 | if (radius_ < 0) 132 | { 133 | radius_ = -radius_; 134 | angle_ = pi_to_pi(angle_ + M_PI); 135 | } 136 | } 137 | 138 | /////////////////////////////////////////////////////////////////////////////// 139 | // Methods for least squares line fitting 140 | /////////////////////////////////////////////////////////////////////////////// 141 | void Line::leastSqFit() 142 | { 143 | calcPointCovariances(); 144 | double prev_radius = 0.0; 145 | double prev_angle = 0.0; 146 | while (fabs(radius_ - prev_radius) > params_.least_sq_radius_thresh || 147 | fabs(angle_ - prev_angle) > params_.least_sq_angle_thresh) 148 | { 149 | prev_radius = radius_; 150 | prev_angle = angle_; 151 | calcPointScalarCovariances(); 152 | radiusFromLeastSq(); 153 | angleFromLeastSq(); 154 | } 155 | calcCovariance(); 156 | projectEndpoints(); 157 | } 158 | 159 | void Line::angleFromLeastSq() 160 | { 161 | calcPointParameters(); 162 | angle_ += angleIncrement(); 163 | } 164 | 165 | double Line::angleIncrement() 166 | { 167 | const std::vector &a = p_params_.a; 168 | const std::vector &ap = p_params_.ap; 169 | const std::vector &app = p_params_.app; 170 | const std::vector &b = p_params_.b; 171 | const std::vector &bp = p_params_.bp; 172 | const std::vector &bpp = p_params_.bpp; 173 | const std::vector &c = p_params_.c; 174 | const std::vector &s = p_params_.s; 175 | 176 | double numerator = 0; 177 | double denominator = 0; 178 | for (std::size_t i = 0; i < a.size(); ++i) 179 | { 180 | numerator += (b[i] * ap[i] - a[i] * bp[i]) / pow(b[i], 2); 181 | denominator += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - 182 | 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); 183 | } 184 | return -(numerator/denominator); 185 | } 186 | 187 | void Line::calcCovariance() 188 | { 189 | covariance_[0] = p_rr_; 190 | 191 | const std::vector &a = p_params_.a; 192 | const std::vector &ap = p_params_.ap; 193 | const std::vector &app = p_params_.app; 194 | const std::vector &b = p_params_.b; 195 | const std::vector &bp = p_params_.bp; 196 | const std::vector &bpp = p_params_.bpp; 197 | const std::vector &c = p_params_.c; 198 | const std::vector &s = p_params_.s; 199 | 200 | double G = 0; 201 | double A = 0; 202 | double B = 0; 203 | double r, phi; 204 | for (std::size_t i = 0; i < a.size(); ++i) 205 | { 206 | r = r_data_.ranges[indices_[i]]; // range 207 | phi = c_data_.bearings[indices_[i]]; // bearing 208 | G += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); 209 | A += 2 * r * sin(angle_ - phi) / b[i]; 210 | B += 4 * pow(r, 2) * pow(sin(angle_ - phi), 2) / b[i]; 211 | } 212 | covariance_[1] = p_rr_ * A / G; 213 | covariance_[2] = covariance_[1]; 214 | covariance_[3] = pow(1.0 / G, 2) * B; 215 | } 216 | 217 | void Line::calcPointCovariances() 218 | { 219 | point_covs_.clear(); 220 | double r, phi, var_r, var_phi; 221 | for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) 222 | { 223 | r = r_data_.ranges[*cit]; // range 224 | phi = c_data_.bearings[*cit]; // bearing 225 | var_r = params_.range_var; // range variance 226 | var_phi = params_.bearing_var; // bearing variance 227 | boost::array Q; 228 | Q[0] = pow(r, 2) * var_phi * pow(sin(phi), 2) + var_r * pow(cos(phi), 2); 229 | Q[1] = -pow(r, 2) * var_phi * sin(2 * phi) / 2.0 + var_r * sin(2 * phi) / 2.0; 230 | Q[2] = Q[1]; 231 | Q[3] = pow(r, 2) * var_phi * pow(cos(phi), 2) + var_r * pow(sin(phi), 2); 232 | point_covs_.push_back(Q); 233 | } 234 | } 235 | 236 | void Line::calcPointParameters() 237 | { 238 | p_params_.a.clear(); 239 | p_params_.ap.clear(); 240 | p_params_.app.clear(); 241 | p_params_.b.clear(); 242 | p_params_.bp.clear(); 243 | p_params_.bpp.clear(); 244 | p_params_.c.clear(); 245 | p_params_.s.clear(); 246 | 247 | double r, phi, var_r, var_phi; 248 | double a, ap, app, b, bp, bpp, c, s; 249 | for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) 250 | { 251 | r = r_data_.ranges[*cit]; // range 252 | phi = c_data_.bearings[*cit]; // bearing 253 | var_r = params_.range_var; // range variance 254 | var_phi = params_.bearing_var; // bearing variance 255 | c = cos(angle_ - phi); 256 | s = sin(angle_ - phi); 257 | a = pow(r * c - radius_, 2); 258 | ap = -2 * r * s * (r * c - radius_); 259 | app = 2 * pow(r, 2) * pow(s, 2) - 2 * r * c * (r * c - radius_); 260 | b = var_r * pow(c, 2) + var_phi * pow(r, 2) * pow(s, 2); 261 | bp = 2 * (pow(r, 2) * var_phi - var_r) * c * s; 262 | bpp = 2 * (pow(r, 2) * var_phi - var_r) * (pow(c, 2) - pow(s, 2)); 263 | p_params_.a.push_back(a); 264 | p_params_.ap.push_back(ap); 265 | p_params_.app.push_back(app); 266 | p_params_.b.push_back(b); 267 | p_params_.bp.push_back(bp); 268 | p_params_.bpp.push_back(bpp); 269 | p_params_.c.push_back(c); 270 | p_params_.s.push_back(s); 271 | } 272 | } 273 | 274 | void Line::calcPointScalarCovariances() 275 | { 276 | point_scalar_vars_.clear(); 277 | double P; 278 | double inverse_P_sum = 0; 279 | for (std::vector >::const_iterator cit = point_covs_.begin(); 280 | cit != point_covs_.end(); ++cit) 281 | { 282 | P = (*cit)[0] * pow(cos(angle_), 2) + 2 * (*cit)[1] * sin(angle_) * cos(angle_) + 283 | (*cit)[3] * pow(sin(angle_), 2); 284 | inverse_P_sum += 1.0 / P; 285 | point_scalar_vars_.push_back(P); 286 | } 287 | p_rr_ = 1.0 / inverse_P_sum; 288 | } 289 | 290 | void Line::radiusFromLeastSq() 291 | { 292 | radius_ = 0; 293 | double r, phi; 294 | for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) 295 | { 296 | r = r_data_.ranges[*cit]; // range 297 | phi = c_data_.bearings[*cit]; // bearing 298 | radius_ += r * cos(angle_ - phi) / point_scalar_vars_[cit - indices_.begin()]; // cit to index 299 | } 300 | 301 | radius_ *= p_rr_; 302 | } 303 | 304 | } // namespace line_extraction 305 | -------------------------------------------------------------------------------- /src/line_extraction.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line_extraction.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace line_extraction 7 | { 8 | 9 | /////////////////////////////////////////////////////////////////////////////// 10 | // Constructor / destructor 11 | /////////////////////////////////////////////////////////////////////////////// 12 | LineExtraction::LineExtraction() 13 | { 14 | } 15 | 16 | LineExtraction::~LineExtraction() 17 | { 18 | } 19 | 20 | /////////////////////////////////////////////////////////////////////////////// 21 | // Main run function 22 | /////////////////////////////////////////////////////////////////////////////// 23 | void LineExtraction::extractLines(std::vector& lines) 24 | { 25 | // Resets 26 | filtered_indices_ = c_data_.indices; 27 | lines_.clear(); 28 | 29 | // Filter indices 30 | filterCloseAndFarPoints(); 31 | filterOutlierPoints(); 32 | 33 | // Return no lines if not enough points left 34 | if (filtered_indices_.size() <= std::max(params_.min_line_points, static_cast(3))) 35 | { 36 | return; 37 | } 38 | 39 | // Split indices into lines and filter out short and sparse lines 40 | split(filtered_indices_); 41 | filterLines(); 42 | 43 | // Fit each line using least squares and merge colinear lines 44 | for (std::vector::iterator it = lines_.begin(); it != lines_.end(); ++it) 45 | { 46 | it->leastSqFit(); 47 | } 48 | 49 | // If there is more than one line, check if lines should be merged based on the merging criteria 50 | if (lines_.size() > 1) 51 | { 52 | mergeLines(); 53 | } 54 | 55 | lines = lines_; 56 | } 57 | 58 | /////////////////////////////////////////////////////////////////////////////// 59 | // Data setting 60 | /////////////////////////////////////////////////////////////////////////////// 61 | void LineExtraction::setCachedData(const std::vector& bearings, 62 | const std::vector& cos_bearings, 63 | const std::vector& sin_bearings, 64 | const std::vector& indices) 65 | { 66 | c_data_.bearings = bearings; 67 | c_data_.cos_bearings = cos_bearings; 68 | c_data_.sin_bearings = sin_bearings; 69 | c_data_.indices = indices; 70 | } 71 | 72 | void LineExtraction::setRangeData(const std::vector& ranges) 73 | { 74 | r_data_.ranges = ranges; 75 | r_data_.xs.clear(); 76 | r_data_.ys.clear(); 77 | for (std::vector::const_iterator cit = c_data_.indices.begin(); 78 | cit != c_data_.indices.end(); ++cit) 79 | { 80 | r_data_.xs.push_back(c_data_.cos_bearings[*cit] * ranges[*cit]); 81 | r_data_.ys.push_back(c_data_.sin_bearings[*cit] * ranges[*cit]); 82 | } 83 | } 84 | 85 | /////////////////////////////////////////////////////////////////////////////// 86 | // Parameter setting 87 | /////////////////////////////////////////////////////////////////////////////// 88 | void LineExtraction::setBearingVariance(double value) 89 | { 90 | params_.bearing_var = value; 91 | } 92 | 93 | void LineExtraction::setRangeVariance(double value) 94 | { 95 | params_.range_var = value; 96 | } 97 | 98 | void LineExtraction::setLeastSqAngleThresh(double value) 99 | { 100 | params_.least_sq_angle_thresh = value; 101 | } 102 | 103 | void LineExtraction::setLeastSqRadiusThresh(double value) 104 | { 105 | params_.least_sq_radius_thresh = value; 106 | } 107 | 108 | void LineExtraction::setMaxLineGap(double value) 109 | { 110 | params_.max_line_gap = value; 111 | } 112 | 113 | void LineExtraction::setMinLineLength(double value) 114 | { 115 | params_.min_line_length = value; 116 | } 117 | 118 | void LineExtraction::setMinLinePoints(unsigned int value) 119 | { 120 | params_.min_line_points = value; 121 | } 122 | 123 | void LineExtraction::setMinRange(double value) 124 | { 125 | params_.min_range = value; 126 | } 127 | 128 | void LineExtraction::setMaxRange(double value) 129 | { 130 | params_.max_range = value; 131 | } 132 | 133 | void LineExtraction::setMinSplitDist(double value) 134 | { 135 | params_.min_split_dist = value; 136 | } 137 | 138 | void LineExtraction::setOutlierDist(double value) 139 | { 140 | params_.outlier_dist = value; 141 | } 142 | 143 | /////////////////////////////////////////////////////////////////////////////// 144 | // Utility methods 145 | /////////////////////////////////////////////////////////////////////////////// 146 | double LineExtraction::chiSquared(const Eigen::Vector2d &dL, const Eigen::Matrix2d &P_1, 147 | const Eigen::Matrix2d &P_2) 148 | { 149 | return dL.transpose() * (P_1 + P_2).inverse() * dL; 150 | } 151 | 152 | double LineExtraction::distBetweenPoints(unsigned int index_1, unsigned int index_2) 153 | { 154 | return sqrt(pow(r_data_.xs[index_1] - r_data_.xs[index_2], 2) + 155 | pow(r_data_.ys[index_1] - r_data_.ys[index_2], 2)); 156 | } 157 | 158 | /////////////////////////////////////////////////////////////////////////////// 159 | // Filtering points 160 | /////////////////////////////////////////////////////////////////////////////// 161 | void LineExtraction::filterCloseAndFarPoints() 162 | { 163 | std::vector output; 164 | for (std::vector::const_iterator cit = filtered_indices_.begin(); 165 | cit != filtered_indices_.end(); ++cit) 166 | { 167 | const double& range = r_data_.ranges[*cit]; 168 | if (range >= params_.min_range && range <= params_.max_range) 169 | { 170 | output.push_back(*cit); 171 | } 172 | } 173 | filtered_indices_ = output; 174 | } 175 | 176 | void LineExtraction::filterOutlierPoints() 177 | { 178 | if (filtered_indices_.size() < 3) 179 | { 180 | return; 181 | } 182 | 183 | std::vector output; 184 | unsigned int p_i, p_j, p_k; 185 | for (std::size_t i = 0; i < filtered_indices_.size(); ++i) 186 | { 187 | 188 | // Get two closest neighbours 189 | 190 | p_i = filtered_indices_[i]; 191 | if (i == 0) // first point 192 | { 193 | p_j = filtered_indices_[i + 1]; 194 | p_k = filtered_indices_[i + 2]; 195 | } 196 | else if (i == filtered_indices_.size() - 1) // last point 197 | { 198 | p_j = filtered_indices_[i - 1]; 199 | p_k = filtered_indices_[i - 2]; 200 | } 201 | else // middle points 202 | { 203 | p_j = filtered_indices_[i - 1]; 204 | p_k = filtered_indices_[i + 1]; 205 | } 206 | 207 | // Check if point is an outlier 208 | 209 | if (fabs(r_data_.ranges[p_i] - r_data_.ranges[p_j]) > params_.outlier_dist && 210 | fabs(r_data_.ranges[p_i] - r_data_.ranges[p_k]) > params_.outlier_dist) 211 | { 212 | // Check if it is close to line connecting its neighbours 213 | std::vector line_indices; 214 | line_indices.push_back(p_j); 215 | line_indices.push_back(p_k); 216 | Line line(c_data_, r_data_, params_, line_indices); 217 | line.endpointFit(); 218 | if (line.distToPoint(p_i) > params_.min_split_dist) 219 | { 220 | continue; // point is an outlier 221 | } 222 | } 223 | 224 | output.push_back(p_i); 225 | } 226 | 227 | filtered_indices_ = output; 228 | } 229 | 230 | /////////////////////////////////////////////////////////////////////////////// 231 | // Filtering and merging lines 232 | /////////////////////////////////////////////////////////////////////////////// 233 | void LineExtraction::filterLines() 234 | { 235 | std::vector output; 236 | for (std::vector::const_iterator cit = lines_.begin(); cit != lines_.end(); ++cit) 237 | { 238 | if (cit->length() >= params_.min_line_length && cit->numPoints() >= params_.min_line_points) 239 | { 240 | output.push_back(*cit); 241 | } 242 | } 243 | lines_ = output; 244 | } 245 | 246 | void LineExtraction::mergeLines() 247 | { 248 | std::vector merged_lines; 249 | 250 | for (std::size_t i = 1; i < lines_.size(); ++i) 251 | { 252 | // Get L, P_1, P_2 of consecutive lines 253 | Eigen::Vector2d L_1(lines_[i-1].getRadius(), lines_[i-1].getAngle()); 254 | Eigen::Vector2d L_2(lines_[i].getRadius(), lines_[i].getAngle()); 255 | Eigen::Matrix2d P_1; 256 | P_1 << lines_[i-1].getCovariance()[0], lines_[i-1].getCovariance()[1], 257 | lines_[i-1].getCovariance()[2], lines_[i-1].getCovariance()[3]; 258 | Eigen::Matrix2d P_2; 259 | P_2 << lines_[i].getCovariance()[0], lines_[i].getCovariance()[1], 260 | lines_[i].getCovariance()[2], lines_[i].getCovariance()[3]; 261 | 262 | // Merge lines if chi-squared distance is less than 3 263 | if (chiSquared(L_1 - L_2, P_1, P_2) < 3) 264 | { 265 | // Get merged angle, radius, and covariance 266 | Eigen::Matrix2d P_m = (P_1.inverse() + P_2.inverse()).inverse(); 267 | Eigen::Vector2d L_m = P_m * (P_1.inverse() * L_1 + P_2.inverse() * L_2); 268 | // Populate new line with these merged parameters 269 | boost::array cov; 270 | cov[0] = P_m(0,0); 271 | cov[1] = P_m(0,1); 272 | cov[2] = P_m(1,0); 273 | cov[3] = P_m(1,1); 274 | std::vector indices; 275 | const std::vector &ind_1 = lines_[i-1].getIndices(); 276 | const std::vector &ind_2 = lines_[i].getIndices(); 277 | indices.resize(ind_1.size() + ind_2.size()); 278 | indices.insert(indices.end(), ind_1.begin(), ind_1.end()); 279 | indices.insert(indices.end(), ind_2.begin(), ind_2.end()); 280 | Line merged_line(L_m[1], L_m[0], cov, lines_[i-1].getStart(), lines_[i].getEnd(), indices); 281 | // Project the new endpoints 282 | merged_line.projectEndpoints(); 283 | lines_[i] = merged_line; 284 | } 285 | else 286 | { 287 | merged_lines.push_back(lines_[i-1]); 288 | } 289 | 290 | if (i == lines_.size() - 1) 291 | { 292 | merged_lines.push_back(lines_[i]); 293 | } 294 | } 295 | lines_ = merged_lines; 296 | } 297 | 298 | /////////////////////////////////////////////////////////////////////////////// 299 | // Splitting points into lines 300 | /////////////////////////////////////////////////////////////////////////////// 301 | void LineExtraction::split(const std::vector& indices) 302 | { 303 | // Don't split if only a single point (only occurs when orphaned by gap) 304 | if (indices.size() <= 1) 305 | { 306 | return; 307 | } 308 | 309 | Line line(c_data_, r_data_, params_, indices); 310 | line.endpointFit(); 311 | double dist_max = 0; 312 | double gap_max = 0; 313 | double dist, gap; 314 | int i_max, i_gap; 315 | 316 | // Find the farthest point and largest gap 317 | for (std::size_t i = 1; i < indices.size() - 1; ++i) 318 | { 319 | dist = line.distToPoint(indices[i]); 320 | if (dist > dist_max) 321 | { 322 | dist_max = dist; 323 | i_max = i; 324 | } 325 | gap = distBetweenPoints(indices[i], indices[i+1]); 326 | if (gap > gap_max) 327 | { 328 | gap_max = gap; 329 | i_gap = i; 330 | } 331 | } 332 | 333 | // Check for gaps at endpoints 334 | double gap_start = distBetweenPoints(indices[0], indices[1]); 335 | if (gap_start > gap_max) 336 | { 337 | gap_max = gap_start; 338 | i_gap = 1; 339 | } 340 | double gap_end = distBetweenPoints(indices.rbegin()[1], indices.rbegin()[0]); 341 | if (gap_end > gap_max) 342 | { 343 | gap_max = gap_end; 344 | i_gap = indices.size() - 1; 345 | } 346 | 347 | // Check if line meets requirements or should be split 348 | if (dist_max < params_.min_split_dist && gap_max < params_.max_line_gap) 349 | { 350 | lines_.push_back(line); 351 | } 352 | else 353 | { 354 | int i_split = dist_max >= params_.min_split_dist ? i_max : i_gap; 355 | std::vector first_split(&indices[0], &indices[i_split - 1]); 356 | std::vector second_split(&indices[i_split], &indices.back()); 357 | split(first_split); 358 | split(second_split); 359 | } 360 | 361 | } 362 | 363 | } // namespace line_extraction 364 | -------------------------------------------------------------------------------- /src/line_extraction_node.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line_extraction_ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | 7 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) 8 | { 9 | ros::console::notifyLoggerLevelsChanged(); 10 | } 11 | 12 | ROS_DEBUG("Starting line_extraction_node."); 13 | 14 | ros::init(argc, argv, "line_extraction_node"); 15 | ros::NodeHandle nh; 16 | ros::NodeHandle nh_local("~"); 17 | line_extraction::LineExtractionROS line_extractor(nh, nh_local); 18 | 19 | double frequency; 20 | nh_local.param("frequency", frequency, 25); 21 | ROS_DEBUG("Frequency set to %0.1f Hz", frequency); 22 | ros::Rate rate(frequency); 23 | 24 | while (ros::ok()) 25 | { 26 | line_extractor.run(); 27 | ros::spinOnce(); 28 | rate.sleep(); 29 | } 30 | return 0; 31 | } 32 | 33 | -------------------------------------------------------------------------------- /src/line_extraction_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_line_extraction/line_extraction_ros.h" 2 | #include 3 | #include 4 | 5 | 6 | namespace line_extraction 7 | { 8 | 9 | /////////////////////////////////////////////////////////////////////////////// 10 | // Constructor / destructor 11 | /////////////////////////////////////////////////////////////////////////////// 12 | LineExtractionROS::LineExtractionROS(ros::NodeHandle& nh, ros::NodeHandle& nh_local): 13 | nh_(nh), 14 | nh_local_(nh_local), 15 | data_cached_(false) 16 | { 17 | loadParameters(); 18 | line_publisher_ = nh_.advertise("line_segments", 1); 19 | scan_subscriber_ = nh_.subscribe(scan_topic_, 1, &LineExtractionROS::laserScanCallback, this); 20 | if (pub_markers_) 21 | { 22 | marker_publisher_ = nh_.advertise("line_markers", 1); 23 | } 24 | } 25 | 26 | LineExtractionROS::~LineExtractionROS() 27 | { 28 | } 29 | 30 | /////////////////////////////////////////////////////////////////////////////// 31 | // Run 32 | /////////////////////////////////////////////////////////////////////////////// 33 | void LineExtractionROS::run() 34 | { 35 | // Extract the lines 36 | std::vector lines; 37 | line_extraction_.extractLines(lines); 38 | 39 | // Populate message 40 | laser_line_extraction::LineSegmentList msg; 41 | populateLineSegListMsg(lines, msg); 42 | 43 | // Publish the lines 44 | line_publisher_.publish(msg); 45 | 46 | // Also publish markers if parameter publish_markers is set to true 47 | if (pub_markers_) 48 | { 49 | visualization_msgs::Marker marker_msg; 50 | populateMarkerMsg(lines, marker_msg); 51 | marker_publisher_.publish(marker_msg); 52 | } 53 | } 54 | 55 | /////////////////////////////////////////////////////////////////////////////// 56 | // Load ROS parameters 57 | /////////////////////////////////////////////////////////////////////////////// 58 | void LineExtractionROS::loadParameters() 59 | { 60 | 61 | ROS_DEBUG("*************************************"); 62 | ROS_DEBUG("PARAMETERS:"); 63 | 64 | // Parameters used by this node 65 | 66 | std::string frame_id, scan_topic; 67 | bool pub_markers; 68 | 69 | nh_local_.param("frame_id", frame_id, "laser"); 70 | frame_id_ = frame_id; 71 | ROS_DEBUG("frame_id: %s", frame_id_.c_str()); 72 | 73 | nh_local_.param("scan_topic", scan_topic, "scan"); 74 | scan_topic_ = scan_topic; 75 | ROS_DEBUG("scan_topic: %s", scan_topic_.c_str()); 76 | 77 | nh_local_.param("publish_markers", pub_markers, false); 78 | pub_markers_ = pub_markers; 79 | ROS_DEBUG("publish_markers: %s", pub_markers ? "true" : "false"); 80 | 81 | // Parameters used by the line extraction algorithm 82 | 83 | double bearing_std_dev, range_std_dev, least_sq_angle_thresh, least_sq_radius_thresh, 84 | max_line_gap, min_line_length, min_range, max_range, min_split_dist, outlier_dist; 85 | int min_line_points; 86 | 87 | nh_local_.param("bearing_std_dev", bearing_std_dev, 1e-3); 88 | line_extraction_.setBearingVariance(bearing_std_dev * bearing_std_dev); 89 | ROS_DEBUG("bearing_std_dev: %f", bearing_std_dev); 90 | 91 | nh_local_.param("range_std_dev", range_std_dev, 0.02); 92 | line_extraction_.setRangeVariance(range_std_dev * range_std_dev); 93 | ROS_DEBUG("range_std_dev: %f", range_std_dev); 94 | 95 | nh_local_.param("least_sq_angle_thresh", least_sq_angle_thresh, 1e-4); 96 | line_extraction_.setLeastSqAngleThresh(least_sq_angle_thresh); 97 | ROS_DEBUG("least_sq_angle_thresh: %f", least_sq_angle_thresh); 98 | 99 | nh_local_.param("least_sq_radius_thresh", least_sq_radius_thresh, 1e-4); 100 | line_extraction_.setLeastSqRadiusThresh(least_sq_radius_thresh); 101 | ROS_DEBUG("least_sq_radius_thresh: %f", least_sq_radius_thresh); 102 | 103 | nh_local_.param("max_line_gap", max_line_gap, 0.4); 104 | line_extraction_.setMaxLineGap(max_line_gap); 105 | ROS_DEBUG("max_line_gap: %f", max_line_gap); 106 | 107 | nh_local_.param("min_line_length", min_line_length, 0.5); 108 | line_extraction_.setMinLineLength(min_line_length); 109 | ROS_DEBUG("min_line_length: %f", min_line_length); 110 | 111 | nh_local_.param("min_range", min_range, 0.4); 112 | line_extraction_.setMinRange(min_range); 113 | ROS_DEBUG("min_range: %f", min_range); 114 | 115 | nh_local_.param("max_range", max_range, 10000.0); 116 | line_extraction_.setMaxRange(max_range); 117 | ROS_DEBUG("max_range: %f", max_range); 118 | 119 | nh_local_.param("min_split_dist", min_split_dist, 0.05); 120 | line_extraction_.setMinSplitDist(min_split_dist); 121 | ROS_DEBUG("min_split_dist: %f", min_split_dist); 122 | 123 | nh_local_.param("outlier_dist", outlier_dist, 0.05); 124 | line_extraction_.setOutlierDist(outlier_dist); 125 | ROS_DEBUG("outlier_dist: %f", outlier_dist); 126 | 127 | nh_local_.param("min_line_points", min_line_points, 9); 128 | line_extraction_.setMinLinePoints(static_cast(min_line_points)); 129 | ROS_DEBUG("min_line_points: %d", min_line_points); 130 | 131 | ROS_DEBUG("*************************************"); 132 | } 133 | 134 | /////////////////////////////////////////////////////////////////////////////// 135 | // Populate messages 136 | /////////////////////////////////////////////////////////////////////////////// 137 | void LineExtractionROS::populateLineSegListMsg(const std::vector &lines, 138 | laser_line_extraction::LineSegmentList &line_list_msg) 139 | { 140 | for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) 141 | { 142 | laser_line_extraction::LineSegment line_msg; 143 | line_msg.angle = cit->getAngle(); 144 | line_msg.radius = cit->getRadius(); 145 | line_msg.covariance = cit->getCovariance(); 146 | line_msg.start = cit->getStart(); 147 | line_msg.end = cit->getEnd(); 148 | line_list_msg.line_segments.push_back(line_msg); 149 | } 150 | line_list_msg.header.frame_id = frame_id_; 151 | line_list_msg.header.stamp = ros::Time::now(); 152 | } 153 | 154 | void LineExtractionROS::populateMarkerMsg(const std::vector &lines, 155 | visualization_msgs::Marker &marker_msg) 156 | { 157 | marker_msg.ns = "line_extraction"; 158 | marker_msg.id = 0; 159 | marker_msg.type = visualization_msgs::Marker::LINE_LIST; 160 | marker_msg.scale.x = 0.1; 161 | marker_msg.color.r = 1.0; 162 | marker_msg.color.g = 0.0; 163 | marker_msg.color.b = 0.0; 164 | marker_msg.color.a = 1.0; 165 | for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) 166 | { 167 | geometry_msgs::Point p_start; 168 | p_start.x = cit->getStart()[0]; 169 | p_start.y = cit->getStart()[1]; 170 | p_start.z = 0; 171 | marker_msg.points.push_back(p_start); 172 | geometry_msgs::Point p_end; 173 | p_end.x = cit->getEnd()[0]; 174 | p_end.y = cit->getEnd()[1]; 175 | p_end.z = 0; 176 | marker_msg.points.push_back(p_end); 177 | } 178 | marker_msg.header.frame_id = frame_id_; 179 | marker_msg.header.stamp = ros::Time::now(); 180 | } 181 | 182 | /////////////////////////////////////////////////////////////////////////////// 183 | // Cache data on first LaserScan message received 184 | /////////////////////////////////////////////////////////////////////////////// 185 | void LineExtractionROS::cacheData(const sensor_msgs::LaserScan::ConstPtr &scan_msg) 186 | { 187 | std::vector bearings, cos_bearings, sin_bearings; 188 | std::vector indices; 189 | const std::size_t num_measurements = std::ceil( 190 | (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment); 191 | for (std::size_t i = 0; i < num_measurements; ++i) 192 | { 193 | const double b = scan_msg->angle_min + i * scan_msg->angle_increment; 194 | bearings.push_back(b); 195 | cos_bearings.push_back(cos(b)); 196 | sin_bearings.push_back(sin(b)); 197 | indices.push_back(i); 198 | } 199 | 200 | line_extraction_.setCachedData(bearings, cos_bearings, sin_bearings, indices); 201 | ROS_DEBUG("Data has been cached."); 202 | } 203 | 204 | /////////////////////////////////////////////////////////////////////////////// 205 | // Main LaserScan callback 206 | /////////////////////////////////////////////////////////////////////////////// 207 | void LineExtractionROS::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg) 208 | { 209 | if (!data_cached_) 210 | { 211 | cacheData(scan_msg); 212 | data_cached_ = true; 213 | } 214 | 215 | std::vector scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end()); 216 | line_extraction_.setRangeData(scan_ranges_doubles); 217 | } 218 | 219 | } // namespace line_extraction 220 | 221 | --------------------------------------------------------------------------------