├── .gitignore ├── LICENSE ├── README.md ├── doc └── kitti.ply ├── linefit_ground_segmentation ├── CMakeLists.txt ├── include │ └── ground_segmentation │ │ ├── bin.h │ │ ├── ground_segmentation.h │ │ ├── segment.h │ │ ├── typedefs.h │ │ └── viewer.h ├── package.xml └── src │ ├── bin.cc │ ├── ground_segmentation.cc │ ├── segment.cc │ └── viewer.cc └── linefit_ground_segmentation_ros ├── CMakeLists.txt ├── launch ├── segmentation.launch ├── segmentation_params.yaml └── test.launch ├── package.xml └── src ├── ground_segmentation_node.cc └── ground_segmentation_test_node.cc /.gitignore: -------------------------------------------------------------------------------- 1 | *.user -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Lorenz Wellhausen 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # linefit_ground_segmentation 2 | 3 | Implementation of the ground segmentation algorithm proposed in 4 | ``` 5 | @inproceedings{himmelsbach2010fast, 6 | title={Fast segmentation of 3d point clouds for ground vehicles}, 7 | author={Himmelsbach, Michael and Hundelshausen, Felix V and Wuensche, H-J}, 8 | booktitle={Intelligent Vehicles Symposium (IV), 2010 IEEE}, 9 | pages={560--565}, 10 | year={2010}, 11 | organization={IEEE} 12 | } 13 | ``` 14 | The `linefit_ground_segmentation` package contains the ground segmentation library. 15 | A ROS interface is available in `linefit_ground_segmentation_ros` 16 | 17 | The library can be compiled separately from the ROS interface if you're not using ROS. 18 | 19 | ## Installation 20 | 21 | Requires the following dependencies to be installed: 22 | 23 | - *catkin_simple* `https://github.com/catkin/catkin_simple.git` 24 | - *eigen_conversions* `sudo apt install ros-noetic-eigen-conversions` 25 | 26 | Compile using your favorite catkin build tool (e.g. `catkin build linefit_ground_segmentation_ros`) 27 | 28 | ## Launch instructions 29 | 30 | The ground segmentation ROS node can be launch by executing `roslaunch linefit_ground_segmentation_ros segmentation.launch`. 31 | Input and output topic names can be specified in the same file. 32 | 33 | Getting up and running with your own point cloud source should be as simple as: 34 | 35 | 1. Change the `input_topic` parameter in `segmentation.launch` to your topic. 36 | 2. Adjust the `sensor_height` parameter in `segmentation_params.yaml` to the height where the sensor is mounted on your robot (e.g. KITTI Velodyne: 1.8m) 37 | 38 | ## Parameter description 39 | 40 | Parameters are set in `linefit_ground_segmentation_ros/launch/segmentation_params.yaml` 41 | 42 | This algorithm works on the assumption that you known the height of the sensor above ground. 43 | Therefore, **you have to adjust the `sensor_height`** to your robot specifications, otherwise, it will not work. 44 | 45 | The default parameters should work on the KITTI dataset. 46 | 47 | ### Ground Condition 48 | - **sensor_height** Sensor height above ground. 49 | - **max_dist_to_line** maximum vertical distance of point to line to be considered ground. 50 | - **max_slope** Maximum slope of a line. 51 | - **min_slope** Minimum slope of a line. 52 | - **max_fit_error** Maximum error a point is allowed to have in a line fit. 53 | - **max_start_height** Maximum height difference between new point and estimated ground height to start a new line. 54 | - **long_threshold** Distance after which the max_height condition is applied. 55 | - **max_height** Maximum height difference between line points when they are farther apart than *long_threshold*. 56 | - **line_search_angle** How far to search in angular direction to find a line. A higher angle helps fill "holes" in the ground segmentation. 57 | - **gravity_aligned_frame** Name of a coordinate frame which has its z-axis aligned with gravity. If specified, the incoming point cloud will be rotated, but not translated into this coordinate frame. If left empty, the sensor frame will be used. 58 | 59 | ### Segmentation 60 | 61 | - **r_min** Distance at which segmentation starts. 62 | - **r_max** Distance at which segmentation ends. 63 | - **n_bins** Number of radial bins. 64 | - **n_segments** Number of angular segments. 65 | 66 | ### Other 67 | 68 | - **n_threads** Number of threads to use. 69 | - **latch** Latch output point clouds in ROS node. 70 | - **visualize** Visualize the segmentation result. **ONLY FOR DEBUGGING.** Do not set true during online operation. 71 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(linefit_ground_segmentation) 3 | 4 | find_package(catkin_simple 0.1.0 REQUIRED ) 5 | 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | 8 | add_definitions(-std=c++14) 9 | 10 | find_package(PCL REQUIRED) 11 | include_directories(${PCL_INCLUDE_DIRS}) 12 | link_directories(${PCL_LIBRARY_DIRS}) 13 | add_definitions(${PCL_DEFINITIONS}) 14 | 15 | find_package(Eigen3 REQUIRED) 16 | include_directories(${Eigen_INCLUDE_DIRS}) 17 | 18 | cs_add_library(${PROJECT_NAME} src/ground_segmentation.cc src/segment.cc src/bin.cc src/viewer.cc) 19 | 20 | #add_doxygen(NOT_AUTOMATIC) 21 | 22 | cs_install() 23 | cs_export() 24 | 25 | ############# 26 | # QTCREATOR # 27 | ############# 28 | FILE(GLOB_RECURSE LibFiles "include/*") 29 | add_custom_target(headers SOURCES ${LibFiles}) 30 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/include/ground_segmentation/bin.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_SEGMENTATION_BIN_H_ 2 | #define GROUND_SEGMENTATION_BIN_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | class Bin { 10 | public: 11 | struct MinZPoint { 12 | MinZPoint() : z(0), d(0) {} 13 | MinZPoint(const double& d, const double& z) : z(z), d(d) {} 14 | bool operator==(const MinZPoint& comp) {return z == comp.z && d == comp.d;} 15 | 16 | double z; 17 | double d; 18 | }; 19 | 20 | private: 21 | 22 | std::atomic has_point_; 23 | std::atomic min_z; 24 | std::atomic min_z_range; 25 | 26 | public: 27 | 28 | Bin(); 29 | 30 | /// \brief Fake copy constructor to allow vector > initialization. 31 | Bin(const Bin& segment); 32 | 33 | void addPoint(const pcl::PointXYZ& point); 34 | 35 | void addPoint(const double& d, const double& z); 36 | 37 | MinZPoint getMinZPoint(); 38 | 39 | inline bool hasPoint() {return has_point_;} 40 | 41 | }; 42 | 43 | #endif /* GROUND_SEGMENTATION_BIN_H_ */ 44 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/include/ground_segmentation/ground_segmentation.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_SEGMENTATION_H_ 2 | #define GROUND_SEGMENTATION_H_ 3 | 4 | #include 5 | 6 | #include "ground_segmentation/segment.h" 7 | #include "ground_segmentation/typedefs.h" 8 | #include "ground_segmentation/viewer.h" 9 | 10 | struct GroundSegmentationParams { 11 | GroundSegmentationParams() : 12 | visualize(false), 13 | r_min_square(0.3 * 0.3), 14 | r_max_square(20*20), 15 | n_bins(30), 16 | n_segments(180), 17 | max_dist_to_line(0.15), 18 | min_slope(0), 19 | max_slope(1), 20 | n_threads(4), 21 | max_error_square(0.01), 22 | long_threshold(2.0), 23 | max_long_height(0.1), 24 | max_start_height(0.2), 25 | sensor_height(0.2), 26 | line_search_angle(0.2) {} 27 | 28 | // Visualize estimated ground. 29 | bool visualize; 30 | // Minimum range of segmentation. 31 | double r_min_square; 32 | // Maximum range of segmentation. 33 | double r_max_square; 34 | // Number of radial bins. 35 | int n_bins; 36 | // Number of angular segments. 37 | int n_segments; 38 | // Maximum distance to a ground line to be classified as ground. 39 | double max_dist_to_line; 40 | // Min slope to be considered ground line. 41 | double min_slope; 42 | // Max slope to be considered ground line. 43 | double max_slope; 44 | // Max error for line fit. 45 | double max_error_square; 46 | // Distance at which points are considered far from each other. 47 | double long_threshold; 48 | // Maximum slope for 49 | double max_long_height; 50 | // Maximum heigh of starting line to be labelled ground. 51 | double max_start_height; 52 | // Height of sensor above ground. 53 | double sensor_height; 54 | // How far to search for a line in angular direction [rad]. 55 | double line_search_angle; 56 | // Number of threads. 57 | int n_threads; 58 | }; 59 | 60 | typedef pcl::PointCloud PointCloud; 61 | 62 | typedef std::pair PointLine; 63 | 64 | class GroundSegmentation { 65 | 66 | const GroundSegmentationParams params_; 67 | 68 | // Access with segments_[segment][bin]. 69 | std::vector segments_; 70 | 71 | // Bin index of every point. 72 | std::vector > bin_index_; 73 | 74 | // 2D coordinates (d, z) of every point in its respective segment. 75 | std::vector segment_coordinates_; 76 | 77 | // Visualizer. 78 | std::unique_ptr viewer_; 79 | 80 | void assignCluster(std::vector* segmentation); 81 | 82 | void assignClusterThread(const unsigned int& start_index, 83 | const unsigned int& end_index, 84 | std::vector* segmentation); 85 | 86 | void insertPoints(const PointCloud& cloud); 87 | 88 | void insertionThread(const PointCloud& cloud, 89 | const size_t start_index, 90 | const size_t end_index); 91 | 92 | void getMinZPoints(PointCloud* out_cloud); 93 | 94 | void getLines(std::list* lines); 95 | 96 | void lineFitThread(const unsigned int start_index, const unsigned int end_index, 97 | std::list *lines, std::mutex* lines_mutex); 98 | 99 | pcl::PointXYZ minZPointTo3d(const Bin::MinZPoint& min_z_point, const double& angle); 100 | 101 | void getMinZPointCloud(PointCloud* cloud); 102 | 103 | void resetSegments(); 104 | 105 | void visualizePointCloud(const PointCloud::ConstPtr& cloud, 106 | const std::string& id = "point_cloud"); 107 | 108 | void visualizeLines(const std::list& lines); 109 | 110 | void visualize(const std::list& lines, const PointCloud::ConstPtr& cloud, const PointCloud::ConstPtr& ground_cloud, const PointCloud::ConstPtr& obstacle_cloud); 111 | 112 | public: 113 | 114 | GroundSegmentation(const GroundSegmentationParams& params = GroundSegmentationParams()); 115 | 116 | void segment(const PointCloud& cloud, std::vector* segmentation); 117 | 118 | }; 119 | 120 | #endif // GROUND_SEGMENTATION_H_ 121 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/include/ground_segmentation/segment.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_SEGMENTATION_SEGMENT_H_ 2 | #define GROUND_SEGMENTATION_SEGMENT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "ground_segmentation/bin.h" 8 | 9 | class Segment { 10 | public: 11 | typedef std::pair Line; 12 | 13 | typedef std::pair LocalLine; 14 | 15 | private: 16 | // Parameters. Description in GroundSegmentation. 17 | const double min_slope_; 18 | const double max_slope_; 19 | const double max_error_; 20 | const double long_threshold_; 21 | const double max_long_height_; 22 | const double max_start_height_; 23 | const double sensor_height_; 24 | 25 | std::vector bins_; 26 | 27 | std::list lines_; 28 | 29 | LocalLine fitLocalLine(const std::list& points); 30 | 31 | double getMeanError(const std::list& points, const LocalLine& line); 32 | 33 | double getMaxError(const std::list& points, const LocalLine& line); 34 | 35 | Line localLineToLine(const LocalLine& local_line, const std::list& line_points); 36 | 37 | 38 | public: 39 | 40 | Segment(const unsigned int& n_bins, 41 | const double& min_slope, 42 | const double& max_slope, 43 | const double& max_error, 44 | const double& long_threshold, 45 | const double& max_long_height, 46 | const double& max_start_height, 47 | const double& sensor_height); 48 | 49 | double verticalDistanceToLine(const double& d, const double &z); 50 | 51 | void fitSegmentLines(); 52 | 53 | inline Bin& operator[](const size_t& index) { 54 | return bins_[index]; 55 | } 56 | 57 | inline std::vector::iterator begin() { 58 | return bins_.begin(); 59 | } 60 | 61 | inline std::vector::iterator end() { 62 | return bins_.end(); 63 | } 64 | 65 | bool getLines(std::list* lines); 66 | 67 | }; 68 | 69 | #endif /* GROUND_SEGMENTATION_SEGMENT_H_ */ 70 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/include/ground_segmentation/typedefs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | typedef pcl::PointCloud PointCloud; 8 | 9 | typedef std::pair PointLine; 10 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/include/ground_segmentation/viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "ground_segmentation/typedefs.h" 10 | 11 | 12 | class Viewer { 13 | public: 14 | Viewer(); 15 | ~Viewer(); 16 | 17 | void visualize(const std::list& lines, 18 | const PointCloud::ConstPtr& min_cloud, 19 | const PointCloud::ConstPtr& ground_cloud, 20 | const PointCloud::ConstPtr& obstacle_cloud); 21 | 22 | protected: 23 | 24 | // Visualizer. 25 | pcl::visualization::PCLVisualizer viewer_; 26 | std::thread view_thread_; 27 | std::mutex viewer_mutex_; 28 | std::atomic redraw_{true}; 29 | 30 | void visualizeLines(const std::list& lines); 31 | 32 | void visualizePointCloud(const PointCloud::ConstPtr& cloud, 33 | const std::string& id); 34 | 35 | void addEmptyPointCloud(const std::string& id); 36 | 37 | void drawThread(); 38 | 39 | }; 40 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | linefit_ground_segmentation 4 | 0.0.1 5 | linefit_ground_segmentation 6 | 7 | Lorenz Wellhausen 8 | 9 | BSD 10 | 11 | catkin 12 | catkin_simple 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/src/bin.cc: -------------------------------------------------------------------------------- 1 | #include "ground_segmentation/bin.h" 2 | 3 | #include 4 | 5 | Bin::Bin() : min_z(std::numeric_limits::max()), has_point_(false) {} 6 | 7 | Bin::Bin(const Bin& bin) : min_z(std::numeric_limits::max()), 8 | has_point_(false) {} 9 | 10 | void Bin::addPoint(const pcl::PointXYZ& point) { 11 | const double d = sqrt(point.x * point.x + point.y * point.y); 12 | addPoint(d, point.z); 13 | } 14 | 15 | void Bin::addPoint(const double& d, const double& z) { 16 | has_point_ = true; 17 | if (z < min_z) { 18 | min_z = z; 19 | min_z_range = d; 20 | } 21 | } 22 | 23 | Bin::MinZPoint Bin::getMinZPoint() { 24 | MinZPoint point; 25 | 26 | if (has_point_) { 27 | point.z = min_z; 28 | point.d = min_z_range; 29 | } 30 | 31 | return point; 32 | } 33 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/src/ground_segmentation.cc: -------------------------------------------------------------------------------- 1 | #include "ground_segmentation/ground_segmentation.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std::chrono_literals; 10 | 11 | 12 | GroundSegmentation::GroundSegmentation(const GroundSegmentationParams& params) : 13 | params_(params), 14 | segments_(params.n_segments, Segment(params.n_bins, 15 | params.min_slope, 16 | params.max_slope, 17 | params.max_error_square, 18 | params.long_threshold, 19 | params.max_long_height, 20 | params.max_start_height, 21 | params.sensor_height)) { 22 | if (params.visualize) viewer_.reset(new Viewer()); 23 | } 24 | 25 | void GroundSegmentation::segment(const PointCloud& cloud, std::vector* segmentation) { 26 | std::cout << "Segmenting cloud with " << cloud.size() << " points...\n"; 27 | std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now(); 28 | segmentation->clear(); 29 | segmentation->resize(cloud.size(), 0); 30 | bin_index_.resize(cloud.size()); 31 | segment_coordinates_.resize(cloud.size()); 32 | resetSegments(); 33 | 34 | insertPoints(cloud); 35 | std::list lines; 36 | if (params_.visualize) { 37 | getLines(&lines); 38 | } 39 | else { 40 | getLines(NULL); 41 | } 42 | assignCluster(segmentation); 43 | 44 | size_t n_ground = 0; 45 | for (const auto seg: *segmentation) { 46 | n_ground += seg; 47 | } 48 | 49 | if (params_.visualize) { 50 | // Visualize. 51 | PointCloud::Ptr obstacle_cloud = boost::make_shared(); 52 | obstacle_cloud->reserve(segmentation->size() - n_ground); 53 | // Get cloud of ground points. 54 | PointCloud::Ptr ground_cloud = boost::make_shared(); 55 | ground_cloud->reserve(n_ground); 56 | for (size_t i = 0; i < cloud.size(); ++i) { 57 | if (segmentation->at(i) == 1) ground_cloud->push_back(cloud[i]); 58 | else obstacle_cloud->push_back(cloud[i]); 59 | } 60 | PointCloud::Ptr min_cloud = boost::make_shared(); 61 | getMinZPointCloud(min_cloud.get()); 62 | viewer_->visualize(lines, min_cloud, ground_cloud, obstacle_cloud); 63 | } 64 | std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); 65 | std::chrono::duration fp_ms = end - start; 66 | std::cout << "Done! Took " << fp_ms.count() << "ms\n"; 67 | } 68 | 69 | void GroundSegmentation::getLines(std::list *lines) { 70 | std::mutex line_mutex; 71 | std::vector thread_vec(params_.n_threads); 72 | unsigned int i; 73 | for (i = 0; i < params_.n_threads; ++i) { 74 | const unsigned int start_index = params_.n_segments / params_.n_threads * i; 75 | const unsigned int end_index = params_.n_segments / params_.n_threads * (i+1); 76 | thread_vec[i] = std::thread(&GroundSegmentation::lineFitThread, this, 77 | start_index, end_index, lines, &line_mutex); 78 | } 79 | for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) { 80 | it->join(); 81 | } 82 | } 83 | 84 | void GroundSegmentation::lineFitThread(const unsigned int start_index, 85 | const unsigned int end_index, 86 | std::list *lines, std::mutex* lines_mutex) { 87 | const bool visualize = lines; 88 | const double seg_step = 2*M_PI / params_.n_segments; 89 | double angle = -M_PI + seg_step/2 + seg_step * start_index; 90 | for (unsigned int i = start_index; i < end_index; ++i) { 91 | segments_[i].fitSegmentLines(); 92 | // Convert lines to 3d if we want to. 93 | if (visualize) { 94 | std::list segment_lines; 95 | segments_[i].getLines(&segment_lines); 96 | for (auto line_iter = segment_lines.begin(); line_iter != segment_lines.end(); ++line_iter) { 97 | const pcl::PointXYZ start = minZPointTo3d(line_iter->first, angle); 98 | const pcl::PointXYZ end = minZPointTo3d(line_iter->second, angle); 99 | lines_mutex->lock(); 100 | lines->emplace_back(start, end); 101 | lines_mutex->unlock(); 102 | } 103 | 104 | angle += seg_step; 105 | } 106 | } 107 | } 108 | 109 | void GroundSegmentation::getMinZPointCloud(PointCloud* cloud) { 110 | cloud->reserve(params_.n_segments * params_.n_bins); 111 | const double seg_step = 2*M_PI / params_.n_segments; 112 | double angle = -M_PI + seg_step/2; 113 | for (auto seg_iter = segments_.begin(); seg_iter != segments_.end(); ++seg_iter) { 114 | for (auto bin_iter = seg_iter->begin(); bin_iter != seg_iter->end(); ++bin_iter) { 115 | const pcl::PointXYZ min = minZPointTo3d(bin_iter->getMinZPoint(), angle); 116 | cloud->push_back(min); 117 | } 118 | 119 | angle += seg_step; 120 | } 121 | } 122 | 123 | void GroundSegmentation::resetSegments() { 124 | segments_ = std::vector(params_.n_segments, Segment(params_.n_bins, 125 | params_.min_slope, 126 | params_.max_slope, 127 | params_.max_error_square, 128 | params_.long_threshold, 129 | params_.max_long_height, 130 | params_.max_start_height, 131 | params_.sensor_height)); 132 | } 133 | 134 | pcl::PointXYZ GroundSegmentation::minZPointTo3d(const Bin::MinZPoint &min_z_point, 135 | const double &angle) { 136 | pcl::PointXYZ point; 137 | point.x = cos(angle) * min_z_point.d; 138 | point.y = sin(angle) * min_z_point.d; 139 | point.z = min_z_point.z; 140 | return point; 141 | } 142 | 143 | void GroundSegmentation::assignCluster(std::vector* segmentation) { 144 | std::vector thread_vec(params_.n_threads); 145 | const size_t cloud_size = segmentation->size(); 146 | for (unsigned int i = 0; i < params_.n_threads; ++i) { 147 | const unsigned int start_index = cloud_size / params_.n_threads * i; 148 | const unsigned int end_index = cloud_size / params_.n_threads * (i+1); 149 | thread_vec[i] = std::thread(&GroundSegmentation::assignClusterThread, this, 150 | start_index, end_index, segmentation); 151 | } 152 | for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) { 153 | it->join(); 154 | } 155 | } 156 | 157 | void GroundSegmentation::assignClusterThread(const unsigned int &start_index, 158 | const unsigned int &end_index, 159 | std::vector *segmentation) { 160 | const double segment_step = 2*M_PI/params_.n_segments; 161 | for (unsigned int i = start_index; i < end_index; ++i) { 162 | Bin::MinZPoint point_2d = segment_coordinates_[i]; 163 | const int segment_index = bin_index_[i].first; 164 | if (segment_index >= 0) { 165 | double dist = segments_[segment_index].verticalDistanceToLine(point_2d.d, point_2d.z); 166 | // Search neighboring segments. 167 | int steps = 1; 168 | while (dist < 0 && steps * segment_step < params_.line_search_angle) { 169 | // Fix indices that are out of bounds. 170 | int index_1 = segment_index + steps; 171 | while (index_1 >= params_.n_segments) index_1 -= params_.n_segments; 172 | int index_2 = segment_index - steps; 173 | while (index_2 < 0) index_2 += params_.n_segments; 174 | // Get distance to neighboring lines. 175 | const double dist_1 = segments_[index_1].verticalDistanceToLine(point_2d.d, point_2d.z); 176 | const double dist_2 = segments_[index_2].verticalDistanceToLine(point_2d.d, point_2d.z); 177 | if (dist_1 >= 0) { 178 | dist = dist_1; 179 | } 180 | if (dist_2 >= 0) { 181 | // Select smaller distance if both segments return a valid distance. 182 | if (dist < 0 || dist_2 < dist) { 183 | dist = dist_2; 184 | } 185 | } 186 | ++steps; 187 | } 188 | if (dist < params_.max_dist_to_line && dist != -1) { 189 | segmentation->at(i) = 1; 190 | } 191 | } 192 | } 193 | } 194 | 195 | void GroundSegmentation::getMinZPoints(PointCloud* out_cloud) { 196 | const double seg_step = 2*M_PI / params_.n_segments; 197 | const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square)) 198 | / params_.n_bins; 199 | const double r_min = sqrt(params_.r_min_square); 200 | double angle = -M_PI + seg_step/2; 201 | for (auto seg_iter = segments_.begin(); seg_iter != segments_.end(); ++seg_iter) { 202 | double dist = r_min + bin_step/2; 203 | for (auto bin_iter = seg_iter->begin(); bin_iter != seg_iter->end(); ++bin_iter) { 204 | pcl::PointXYZ point; 205 | if (bin_iter->hasPoint()) { 206 | Bin::MinZPoint min_z_point(bin_iter->getMinZPoint()); 207 | point.x = cos(angle) * min_z_point.d; 208 | point.y = sin(angle) * min_z_point.d; 209 | point.z = min_z_point.z; 210 | 211 | out_cloud->push_back(point); 212 | } 213 | dist += bin_step; 214 | } 215 | angle += seg_step; 216 | } 217 | } 218 | 219 | void GroundSegmentation::insertPoints(const PointCloud& cloud) { 220 | std::vector threads(params_.n_threads); 221 | const size_t points_per_thread = cloud.size() / params_.n_threads; 222 | // Launch threads. 223 | for (unsigned int i = 0; i < params_.n_threads - 1; ++i) { 224 | const size_t start_index = i * points_per_thread; 225 | const size_t end_index = (i+1) * points_per_thread; 226 | threads[i] = std::thread(&GroundSegmentation::insertionThread, this, 227 | cloud, start_index, end_index); 228 | } 229 | // Launch last thread which might have more points than others. 230 | const size_t start_index = (params_.n_threads - 1) * points_per_thread; 231 | const size_t end_index = cloud.size(); 232 | threads[params_.n_threads - 1] = 233 | std::thread(&GroundSegmentation::insertionThread, this, cloud, start_index, end_index); 234 | // Wait for threads to finish. 235 | for (auto it = threads.begin(); it != threads.end(); ++it) { 236 | it->join(); 237 | } 238 | } 239 | 240 | void GroundSegmentation::insertionThread(const PointCloud& cloud, 241 | const size_t start_index, 242 | const size_t end_index) { 243 | const double segment_step = 2*M_PI / params_.n_segments; 244 | const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square)) 245 | / params_.n_bins; 246 | const double r_min = sqrt(params_.r_min_square); 247 | for (unsigned int i = start_index; i < end_index; ++i) { 248 | pcl::PointXYZ point(cloud[i]); 249 | const double range_square = point.x * point.x + point.y * point.y; 250 | const double range = sqrt(range_square); 251 | if (range_square < params_.r_max_square && range_square > params_.r_min_square) { 252 | const double angle = std::atan2(point.y, point.x); 253 | const unsigned int bin_index = (range - r_min) / bin_step; 254 | const unsigned int segment_index = (angle + M_PI) / segment_step; 255 | const unsigned int segment_index_clamped = segment_index == params_.n_segments ? 0 : segment_index; 256 | segments_[segment_index_clamped][bin_index].addPoint(range, point.z); 257 | bin_index_[i] = std::make_pair(segment_index_clamped, bin_index); 258 | } 259 | else { 260 | bin_index_[i] = std::make_pair(-1, -1); 261 | } 262 | segment_coordinates_[i] = Bin::MinZPoint(range, point.z); 263 | } 264 | } 265 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/src/segment.cc: -------------------------------------------------------------------------------- 1 | #include "ground_segmentation/segment.h" 2 | 3 | Segment::Segment(const unsigned int& n_bins, 4 | const double& min_slope, 5 | const double& max_slope, 6 | const double& max_error, 7 | const double& long_threshold, 8 | const double& max_long_height, 9 | const double& max_start_height, 10 | const double& sensor_height) : 11 | bins_(n_bins), 12 | min_slope_(min_slope), 13 | max_slope_(max_slope), 14 | max_error_(max_error), 15 | long_threshold_(long_threshold), 16 | max_long_height_(max_long_height), 17 | max_start_height_(max_start_height), 18 | sensor_height_(sensor_height){} 19 | 20 | void Segment::fitSegmentLines() { 21 | // Find first point. 22 | auto line_start = bins_.begin(); 23 | while (!line_start->hasPoint()) { 24 | ++line_start; 25 | // Stop if we reached last point. 26 | if (line_start == bins_.end()) return; 27 | } 28 | // Fill lines. 29 | bool is_long_line = false; 30 | double cur_ground_height = -sensor_height_; 31 | std::list current_line_points(1, line_start->getMinZPoint()); 32 | LocalLine cur_line = std::make_pair(0,0); 33 | for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter) { 34 | if (line_iter->hasPoint()) { 35 | Bin::MinZPoint cur_point = line_iter->getMinZPoint(); 36 | if (cur_point.d - current_line_points.back().d > long_threshold_) is_long_line = true; 37 | if (current_line_points.size() >= 2) { 38 | // Get expected z value to possibly reject far away points. 39 | double expected_z = std::numeric_limits::max(); 40 | if (is_long_line && current_line_points.size() > 2) { 41 | expected_z = cur_line.first * cur_point.d + cur_line.second; 42 | } 43 | current_line_points.push_back(cur_point); 44 | cur_line = fitLocalLine(current_line_points); 45 | const double error = getMaxError(current_line_points, cur_line); 46 | // Check if not a good line. 47 | if (error > max_error_ || 48 | std::fabs(cur_line.first) > max_slope_ || 49 | (current_line_points.size() > 2 && std::fabs(cur_line.first) < min_slope_) || 50 | is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) { 51 | // Add line until previous point as ground. 52 | current_line_points.pop_back(); 53 | // Don't let lines with 2 base points through. 54 | if (current_line_points.size() >= 3) { 55 | const LocalLine new_line = fitLocalLine(current_line_points); 56 | lines_.push_back(localLineToLine(new_line, current_line_points)); 57 | cur_ground_height = new_line.first * current_line_points.back().d + new_line.second; 58 | } 59 | // Start new line. 60 | is_long_line = false; 61 | current_line_points.erase(current_line_points.begin(), --current_line_points.end()); 62 | --line_iter; 63 | } 64 | // Good line, continue. 65 | else { } 66 | } 67 | else { 68 | // Not enough points. 69 | if (cur_point.d - current_line_points.back().d < long_threshold_ && 70 | std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) { 71 | // Add point if valid. 72 | current_line_points.push_back(cur_point); 73 | } 74 | else { 75 | // Start new line. 76 | current_line_points.clear(); 77 | current_line_points.push_back(cur_point); 78 | } 79 | } 80 | } 81 | } 82 | // Add last line. 83 | if (current_line_points.size() > 2) { 84 | const LocalLine new_line = fitLocalLine(current_line_points); 85 | lines_.push_back(localLineToLine(new_line, current_line_points)); 86 | } 87 | } 88 | 89 | Segment::Line Segment::localLineToLine(const LocalLine& local_line, 90 | const std::list& line_points) { 91 | Line line; 92 | const double first_d = line_points.front().d; 93 | const double second_d = line_points.back().d; 94 | const double first_z = local_line.first * first_d + local_line.second; 95 | const double second_z = local_line.first * second_d + local_line.second; 96 | line.first.z = first_z; 97 | line.first.d = first_d; 98 | line.second.z = second_z; 99 | line.second.d = second_d; 100 | return line; 101 | } 102 | 103 | double Segment::verticalDistanceToLine(const double &d, const double &z) { 104 | static const double kMargin = 0.1; 105 | double distance = -1; 106 | for (auto it = lines_.begin(); it != lines_.end(); ++it) { 107 | if (it->first.d - kMargin < d && it->second.d + kMargin > d) { 108 | const double delta_z = it->second.z - it->first.z; 109 | const double delta_d = it->second.d - it->first.d; 110 | const double expected_z = (d - it->first.d)/delta_d *delta_z + it->first.z; 111 | distance = std::fabs(z - expected_z); 112 | } 113 | } 114 | return distance; 115 | } 116 | 117 | double Segment::getMeanError(const std::list &points, const LocalLine &line) { 118 | double error_sum = 0; 119 | for (auto it = points.begin(); it != points.end(); ++it) { 120 | const double residual = (line.first * it->d + line.second) - it->z; 121 | error_sum += residual * residual; 122 | } 123 | return error_sum/points.size(); 124 | } 125 | 126 | double Segment::getMaxError(const std::list &points, const LocalLine &line) { 127 | double max_error = 0; 128 | for (auto it = points.begin(); it != points.end(); ++it) { 129 | const double residual = (line.first * it->d + line.second) - it->z; 130 | const double error = residual * residual; 131 | if (error > max_error) max_error = error; 132 | } 133 | return max_error; 134 | } 135 | 136 | Segment::LocalLine Segment::fitLocalLine(const std::list &points) { 137 | const unsigned int n_points = points.size(); 138 | Eigen::MatrixXd X(n_points, 2); 139 | Eigen::VectorXd Y(n_points); 140 | unsigned int counter = 0; 141 | for (auto iter = points.begin(); iter != points.end(); ++iter) { 142 | X(counter, 0) = iter->d; 143 | X(counter, 1) = 1; 144 | Y(counter) = iter->z; 145 | ++counter; 146 | } 147 | Eigen::VectorXd result = X.colPivHouseholderQr().solve(Y); 148 | LocalLine line_result; 149 | line_result.first = result(0); 150 | line_result.second = result(1); 151 | return line_result; 152 | } 153 | 154 | bool Segment::getLines(std::list *lines) { 155 | if (lines_.empty()) { 156 | return false; 157 | } 158 | else { 159 | *lines = lines_; 160 | return true; 161 | } 162 | } 163 | -------------------------------------------------------------------------------- /linefit_ground_segmentation/src/viewer.cc: -------------------------------------------------------------------------------- 1 | #include "ground_segmentation/viewer.h" 2 | 3 | #include 4 | 5 | using namespace std::chrono_literals; 6 | 7 | 8 | 9 | Viewer::Viewer() { 10 | std::lock_guard lock(viewer_mutex_); 11 | 12 | viewer_.setBackgroundColor (0, 0, 0); 13 | viewer_.addCoordinateSystem (1.0); 14 | viewer_.initCameraParameters (); 15 | viewer_.setCameraPosition(-2.0, 0, 2.0, 1.0, 0, 0); 16 | 17 | addEmptyPointCloud("min_cloud"); 18 | addEmptyPointCloud("ground_cloud"); 19 | addEmptyPointCloud("obstacle_cloud"); 20 | 21 | viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 22 | 0.0f, 1.0f, 0.0f, 23 | "min_cloud"); 24 | viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 25 | 2.0f, 26 | "min_cloud"); 27 | viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 28 | 1.0f, 0.0f, 0.0f, 29 | "ground_cloud"); 30 | 31 | view_thread_ = std::thread(&Viewer::drawThread, this); 32 | } 33 | 34 | 35 | 36 | Viewer::~Viewer() { 37 | redraw_ = false; 38 | 39 | { 40 | std::lock_guard lock(viewer_mutex_); 41 | viewer_.close(); 42 | } 43 | 44 | view_thread_.join(); 45 | } 46 | 47 | 48 | 49 | void Viewer::visualize(const std::list& lines, 50 | const PointCloud::ConstPtr& min_cloud, 51 | const PointCloud::ConstPtr& ground_cloud, 52 | const PointCloud::ConstPtr& obstacle_cloud) { 53 | redraw_ = false; 54 | std::lock_guard lock(viewer_mutex_); 55 | // TODO: Do not delete and add every time. 56 | viewer_.removeAllShapes(); 57 | visualizePointCloud(min_cloud, "min_cloud"); 58 | visualizePointCloud(ground_cloud, "ground_cloud"); 59 | visualizePointCloud(obstacle_cloud, "obstacle_cloud"); 60 | visualizeLines(lines); 61 | } 62 | 63 | 64 | 65 | void Viewer::visualizeLines(const std::list& lines) { 66 | size_t counter = 0; 67 | for (auto it = lines.begin(); it != lines.end(); ++it) { 68 | viewer_.addLine(it->first, it->second, std::to_string(counter++)); 69 | } 70 | } 71 | 72 | 73 | 74 | void Viewer::visualizePointCloud(const PointCloud::ConstPtr& cloud, 75 | const std::string& id) { 76 | viewer_.updatePointCloud(cloud, id); 77 | } 78 | 79 | 80 | 81 | void Viewer::addEmptyPointCloud(const std::string& id) { 82 | viewer_.addPointCloud(PointCloud().makeShared(), id); 83 | } 84 | 85 | 86 | 87 | void Viewer::drawThread() { 88 | bool stopped = false; 89 | while (!stopped) { 90 | { 91 | std::lock_guard lock(viewer_mutex_); 92 | redraw_ = true; 93 | while (redraw_) { 94 | viewer_.spinOnce(1); 95 | } 96 | } 97 | std::this_thread::sleep_for(1ms); 98 | std::lock_guard lock(viewer_mutex_); 99 | stopped = viewer_.wasStopped(); 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(linefit_ground_segmentation_ros) 3 | 4 | find_package(catkin_simple 0.1.0 REQUIRED ) 5 | 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | 8 | add_definitions(-std=c++14) 9 | 10 | find_package(PCL REQUIRED) 11 | include_directories(${PCL_INCLUDE_DIRS}) 12 | link_directories(${PCL_LIBRARY_DIRS}) 13 | add_definitions(${PCL_DEFINITIONS}) 14 | 15 | add_executable(ground_segmentation_node src/ground_segmentation_node.cc) 16 | add_executable(ground_segmentation_test_node src/ground_segmentation_test_node.cc) 17 | target_link_libraries(ground_segmentation_node ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 18 | target_link_libraries(ground_segmentation_test_node ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 19 | 20 | #add_doxygen(NOT_AUTOMATIC) 21 | 22 | cs_install() 23 | cs_export() 24 | 25 | ############# 26 | # QTCREATOR # 27 | ############# 28 | FILE(GLOB_RECURSE LibFiles "include/*") 29 | add_custom_target(headers SOURCES ${LibFiles}) 30 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/launch/segmentation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/launch/segmentation_params.yaml: -------------------------------------------------------------------------------- 1 | n_threads: 4 # number of threads to use. 2 | 3 | r_min: 0.5 # minimum point distance. 4 | r_max: 50 # maximum point distance. 5 | n_bins: 120 # number of radial bins. 6 | n_segments: 360 # number of radial segments. 7 | 8 | max_dist_to_line: 0.05 # maximum vertical distance of point to line to be considered ground. 9 | 10 | sensor_height: 1.8 # sensor height above ground. 11 | min_slope: 0.0 # minimum slope of a ground line. 12 | max_slope: 0.3 # maximum slope of a ground line. 13 | max_fit_error: 0.05 # maximum error of a point during line fit. 14 | long_threshold: 1.0 # distance between points after which they are considered far from each other. 15 | max_long_height: 0.1 # maximum height change to previous point in long line. 16 | max_start_height: 0.2 # maximum difference to estimated ground height to start a new line. 17 | line_search_angle: 0.1 # how far to search in angular direction to find a line [rad]. 18 | 19 | gravity_aligned_frame: "" # Frame which has its z axis aligned with gravity. (Sensor frame if empty.) 20 | 21 | latch: false # latch output topics or not 22 | visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING 23 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | linefit_ground_segmentation_ros 4 | 0.0.1 5 | linefit_ground_segmentation_ros 6 | 7 | Lorenz Wellhausen 8 | 9 | BSD 10 | 11 | catkin 12 | catkin_simple 13 | 14 | eigen_conversions 15 | roscpp 16 | tf2_ros 17 | 18 | linefit_ground_segmentation 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/src/ground_segmentation_node.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "ground_segmentation/ground_segmentation.h" 10 | 11 | class SegmentationNode { 12 | ros::Publisher ground_pub_; 13 | ros::Publisher obstacle_pub_; 14 | tf2_ros::Buffer tf_buffer_; 15 | tf2_ros::TransformListener tf_listener_{tf_buffer_}; 16 | GroundSegmentationParams params_; 17 | GroundSegmentation segmenter_; 18 | std::string gravity_aligned_frame_; 19 | 20 | public: 21 | SegmentationNode(ros::NodeHandle& nh, 22 | const std::string& ground_topic, 23 | const std::string& obstacle_topic, 24 | const GroundSegmentationParams& params, 25 | const bool& latch = false) : params_(params), segmenter_(params) { 26 | ground_pub_ = nh.advertise>(ground_topic, 1, latch); 27 | obstacle_pub_ = nh.advertise>(obstacle_topic, 1, latch); 28 | nh.param("gravity_aligned_frame", gravity_aligned_frame_, ""); 29 | } 30 | 31 | void scanCallback(const pcl::PointCloud& cloud) { 32 | pcl::PointCloud cloud_transformed; 33 | 34 | std::vector labels; 35 | 36 | bool is_original_pc = true; 37 | if (!gravity_aligned_frame_.empty()) { 38 | geometry_msgs::TransformStamped tf_stamped; 39 | try{ 40 | tf_stamped = tf_buffer_.lookupTransform(gravity_aligned_frame_, cloud.header.frame_id, 41 | pcl_conversions::fromPCL(cloud.header.stamp)); 42 | // Remove translation part. 43 | tf_stamped.transform.translation.x = 0; 44 | tf_stamped.transform.translation.y = 0; 45 | tf_stamped.transform.translation.z = 0; 46 | Eigen::Affine3d tf; 47 | tf::transformMsgToEigen(tf_stamped.transform, tf); 48 | pcl::transformPointCloud(cloud, cloud_transformed, tf); 49 | is_original_pc = false; 50 | } 51 | catch (tf2::TransformException &ex) { 52 | ROS_WARN_THROTTLE(1.0, "Failed to transform point cloud into gravity frame: %s",ex.what()); 53 | } 54 | } 55 | 56 | // Trick to avoid PC copy if we do not transform. 57 | const pcl::PointCloud& cloud_proc = is_original_pc ? cloud : cloud_transformed; 58 | 59 | segmenter_.segment(cloud_proc, &labels); 60 | pcl::PointCloud ground_cloud, obstacle_cloud; 61 | ground_cloud.header = cloud.header; 62 | obstacle_cloud.header = cloud.header; 63 | for (size_t i = 0; i < cloud.size(); ++i) { 64 | if (labels[i] == 1) ground_cloud.push_back(cloud[i]); 65 | else obstacle_cloud.push_back(cloud[i]); 66 | } 67 | ground_pub_.publish(ground_cloud); 68 | obstacle_pub_.publish(obstacle_cloud); 69 | } 70 | }; 71 | 72 | int main(int argc, char** argv) { 73 | ros::init(argc, argv, "ground_segmentation"); 74 | 75 | ros::NodeHandle nh("~"); 76 | 77 | // Do parameter stuff. 78 | GroundSegmentationParams params; 79 | nh.param("visualize", params.visualize, params.visualize); 80 | nh.param("n_bins", params.n_bins, params.n_bins); 81 | nh.param("n_segments", params.n_segments, params.n_segments); 82 | nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line); 83 | nh.param("max_slope", params.max_slope, params.max_slope); 84 | nh.param("min_slope", params.min_slope, params.min_slope); 85 | nh.param("long_threshold", params.long_threshold, params.long_threshold); 86 | nh.param("max_long_height", params.max_long_height, params.max_long_height); 87 | nh.param("max_start_height", params.max_start_height, params.max_start_height); 88 | nh.param("sensor_height", params.sensor_height, params.sensor_height); 89 | nh.param("line_search_angle", params.line_search_angle, params.line_search_angle); 90 | nh.param("n_threads", params.n_threads, params.n_threads); 91 | // Params that need to be squared. 92 | double r_min, r_max, max_fit_error; 93 | if (nh.getParam("r_min", r_min)) { 94 | params.r_min_square = r_min*r_min; 95 | } 96 | if (nh.getParam("r_max", r_max)) { 97 | params.r_max_square = r_max*r_max; 98 | } 99 | if (nh.getParam("max_fit_error", max_fit_error)) { 100 | params.max_error_square = max_fit_error * max_fit_error; 101 | } 102 | 103 | std::string ground_topic, obstacle_topic, input_topic; 104 | bool latch; 105 | nh.param("input_topic", input_topic, "input_cloud"); 106 | nh.param("ground_output_topic", ground_topic, "ground_cloud"); 107 | nh.param("obstacle_output_topic", obstacle_topic, "obstacle_cloud"); 108 | nh.param("latch", latch, false); 109 | 110 | // Start node. 111 | SegmentationNode node(nh, ground_topic, obstacle_topic, params, latch); 112 | ros::Subscriber cloud_sub; 113 | cloud_sub = nh.subscribe(input_topic, 1, &SegmentationNode::scanCallback, &node); 114 | ros::spin(); 115 | } 116 | -------------------------------------------------------------------------------- /linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "ground_segmentation/ground_segmentation.h" 6 | 7 | int main(int argc, char** argv) { 8 | ros::init(argc, argv, "ground_segmentation"); 9 | 10 | ros::NodeHandle nh("~"); 11 | 12 | std::string cloud_file; 13 | if (nh.getParam("point_cloud_file", cloud_file)) { 14 | std::cout << "Point cloud file is \"" << cloud_file << "\"\n"; 15 | pcl::PointCloud cloud; 16 | pcl::io::loadPLYFile(cloud_file, cloud); 17 | 18 | GroundSegmentationParams params; 19 | nh.param("visualize", params.visualize, params.visualize); 20 | nh.param("n_bins", params.n_bins, params.n_bins); 21 | nh.param("n_segments", params.n_segments, params.n_segments); 22 | nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line); 23 | nh.param("max_slope", params.max_slope, params.max_slope); 24 | nh.param("long_threshold", params.long_threshold, params.long_threshold); 25 | nh.param("max_long_height", params.max_long_height, params.max_long_height); 26 | nh.param("max_start_height", params.max_start_height, params.max_start_height); 27 | nh.param("sensor_height", params.sensor_height, params.sensor_height); 28 | nh.param("line_search_angle", params.line_search_angle, params.line_search_angle); 29 | nh.param("n_threads", params.n_threads, params.n_threads); 30 | // Params that need to be squared. 31 | double r_min, r_max, max_fit_error; 32 | if (nh.getParam("r_min", r_min)) { 33 | params.r_min_square = r_min*r_min; 34 | } 35 | if (nh.getParam("r_max", r_max)) { 36 | params.r_max_square = r_max*r_max; 37 | } 38 | if (nh.getParam("max_fit_error", max_fit_error)) { 39 | params.max_error_square = max_fit_error * max_fit_error; 40 | } 41 | 42 | GroundSegmentation segmenter(params); 43 | std::vector labels; 44 | 45 | segmenter.segment(cloud, &labels); 46 | 47 | ros::spin(); 48 | } 49 | else { 50 | std::cerr << "No point cloud file given\n"; 51 | } 52 | } 53 | --------------------------------------------------------------------------------