├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cloud_segmentation_https.rosinstall ├── cloud_segmentation_ssh.rosinstall ├── config ├── cofusion_car.yaml ├── cofusion_room.yaml └── default.yaml ├── include └── cloud_segmentation │ ├── cloud_segmentation.h │ └── viewer_utils.h ├── launch └── cloud_segmentation.launch ├── package.xml └── src ├── cloud_segmentation.cc └── node.cc /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cloud_segmentation) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | cs_add_library(${PROJECT_NAME} 8 | src/cloud_segmentation.cc 9 | ) 10 | 11 | cs_add_executable(cloud_segmentation_node 12 | src/node.cc 13 | ) 14 | target_link_libraries(cloud_segmentation_node ${PROJECT_NAME}) 15 | 16 | cs_install() 17 | cs_export() 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Margarita Grinvald, Autonomous Systems Lab, ETH Zurich 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS package for point cloud segmentation 2 | 3 | This package implements geometric segmentation of depth clouds based on region growing. First, the incoming clouds are filtered, normals are estimated for each 3D point, and then [PCL region growing segmentation](https://pcl.readthedocs.io/projects/tutorials/en/latest/region_growing_segmentation.html) is applied. 4 | 5 | The [rgbd_segmentation](https://github.com/ethz-asl/rgbd_segmentation) package combines the geometric segmentation implemented here with instance-aware semantic segmentation via [Mask R-CNN](https://github.com/ethz-asl/mask_rcnn_ros) to obtain combined geometric-semantic segmentation results. 6 | 7 | This package was developed as part of the [**TSDF++** framework](https://github.com/ethz-asl/tsdf-plusplus) for multiple dynamic object tracking and reconstruction. 8 | 9 | ## Citing 10 | 11 | When using this code in your research, please cite the following publication: 12 | 13 | Margarita Grinvald, Federico Tombari, Roland Siegwart, and Juan Nieto, **TSDF++: A Multi-Object Formulation for Dynamic Object Tracking and Reconstruction**, in _2021 IEEE International Conference on Robotics and Automation (ICRA)_, 2021. [[Video](https://youtu.be/dSJmoeVasI0)] 14 | 15 | ```bibtex 16 | @INPROCEEDINGS{grinvald2021tsdf, 17 | author={Grinvald, Margarita and Tombari, Federico and Siegwart, Roland and Nieto, Juan}, 18 | booktitle={2021 IEEE International Conference on Robotics and Automation (ICRA)}, 19 | title={{TSDF++: A Multi-Object Formulation for Dynamic Object Tracking and Reconstruction}}, 20 | year={2021}, 21 | volume={}, 22 | number={}, 23 | pages={14192-14198}, 24 | doi={10.1109/ICRA48506.2021.9560923}} 25 | ``` 26 | 27 | 28 | ## Installation 29 | 30 | The installation has been tested on Ubuntu 16.04 and Ubutnu 20.04. 31 | 32 | ### Requirements 33 | - ROS 34 | - C++14 for [PCL 1.10](https://github.com/PointCloudLibrary/pcl) 35 | 36 | ### Install dependencies 37 | Install ROS following the instructions at the [ROS installation page](http://wiki.ros.org/ROS/Installation). The full install (`ros-kinetic-desktop-full`, `ros-melodic-desktop-full`) are recommended. 38 | 39 | Make sure to source your ROS _setup.bash_ script by following the instructions on the ROS installation page. 40 | 41 | ### Installation on Ubuntu 42 | In your terminal, define the installed ROS version and name of the catkin workspace to use: 43 | ```bash 44 | export ROS_VERSION=kinetic # (Ubuntu 16.04: kinetic, Ubuntu 18.04: melodic) 45 | export CATKIN_WS=~/catkin_ws 46 | ``` 47 | 48 | If you don't have a [catkin](http://wiki.ros.org/catkin) workspace yet, create a new one: 49 | ```bash 50 | mkdir -p $CATKIN_WS/src && cd $CATKIN_WS 51 | catkin init 52 | catkin config --extend /opt/ros/$ROS_VERSION --merge-devel 53 | catkin config --cmake-args -DCMAKE_CXX_STANDARD=14 -DCMAKE_BUILD_TYPE=Release 54 | wstool init src 55 | ``` 56 | 57 | Clone the `cloud_segmentation` repository over HTTPS (no Github account required) and automatically fetch dependencies: 58 | ```bash 59 | cd $CATKIN_WS/src 60 | git clone https://github.com/ethz-asl/cloud_segmentation.git 61 | wstool merge -t . cloud_segmentation/cloud_segmentation_https.rosinstall 62 | wstool update 63 | ``` 64 | 65 | Alternatively, clone over SSH (Github account required): 66 | ```bash 67 | cd $CATKIN_WS/src 68 | git clone git@github.com:ethz-asl/cloud_segmentation.git 69 | wstool merge -t . cloud_segmentation/cloud_segmentation_ssh.rosinstall 70 | wstool update 71 | ``` 72 | 73 | Build and source the fetched packages: 74 | ```bash 75 | catkin build 76 | source ../devel/setup.bash # (bash shell: ../devel/setup.bash, zsh shell: ../devel/setup.zsh) 77 | ``` 78 | 79 | ## License 80 | The code is available under the [MIT license](https://github.com/ethz-asl/cloud_segmentation/blob/master/LICENSE). 81 | -------------------------------------------------------------------------------- /cloud_segmentation_https.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: https://github.com/ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: glog_catkin 9 | uri: https://github.com/ethz-asl/glog_catkin.git 10 | - git: 11 | local-name: pcl_catkin 12 | uri: https://github.com/ethz-asl/pcl_catkin.git 13 | -------------------------------------------------------------------------------- /cloud_segmentation_ssh.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: git@github.com:catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: git@github.com:ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: glog_catkin 9 | uri: git@github.com:ethz-asl/glog_catkin.git 10 | - git: 11 | local-name: pcl_catkin 12 | uri: git@github.com:ethz-asl/pcl_catkin.git 13 | -------------------------------------------------------------------------------- /config/cofusion_car.yaml: -------------------------------------------------------------------------------- 1 | filtering: 2 | enable: true 3 | leaf_size: 0.01 # meters 4 | max_distance: 4 # meters 5 | 6 | normal_estimation: 7 | max_depth_change_factor: 0.02 8 | normal_smoothing_size: 10.0 9 | 10 | region_growing: 11 | min_cluster_size: 50 12 | number_of_neighbours: 15 13 | smoothness_threshold_deg: 7.0 14 | curvature_threshold: 1 15 | 16 | viewer: 17 | enable: true 18 | write_frames_to_file: false 19 | -------------------------------------------------------------------------------- /config/cofusion_room.yaml: -------------------------------------------------------------------------------- 1 | filtering: 2 | enable: true 3 | leaf_size: 0.01 # meters 4 | max_distance: 4 # meters 5 | 6 | normal_estimation: 7 | max_depth_change_factor: 0.02 8 | normal_smoothing_size: 10.0 9 | 10 | region_growing: 11 | min_cluster_size: 100 12 | number_of_neighbours: 15 13 | smoothness_threshold_deg: 7 14 | curvature_threshold: 1 15 | 16 | viewer: 17 | enable: true 18 | write_frames_to_file: false 19 | -------------------------------------------------------------------------------- /config/default.yaml: -------------------------------------------------------------------------------- 1 | filtering: 2 | enable: true 3 | leaf_size: 0.01 # meters 4 | min_distance: 0.0 # meters 5 | max_distance: 3.0 # meters 6 | 7 | normal_estimation: 8 | use_integral_image: false 9 | max_depth_change_factor: 0.01 10 | normal_smoothing_size: 7.0 11 | 12 | region_growing: 13 | min_cluster_size: 100 14 | number_of_neighbours: 30 15 | smoothness_threshold_deg: 3.0 16 | curvature_threshold: 1.0 17 | 18 | viewer: 19 | enable: true 20 | camera_intrinsics: [360, 0, 320, 0, 360, 240, 0, 0, 1] 21 | write_frames_to_file: false 22 | 23 | debug: 24 | verbose_log: false 25 | -------------------------------------------------------------------------------- /include/cloud_segmentation/cloud_segmentation.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020- Margarita Grinvald, Autonomous Systems Lab, ETH Zurich 2 | // Licensed under the MIT License (see LICENSE for details) 3 | 4 | #ifndef CLOUD_SEGMENTATION_CLOUD_SEGMENTATION_H_ 5 | #define CLOUD_SEGMENTATION_CLOUD_SEGMENTATION_H_ 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | struct PointXYZRGBLNormal { 19 | PCL_ADD_POINT4D; 20 | PCL_ADD_NORMAL4D; 21 | PCL_ADD_RGB; 22 | uint32_t label; 23 | 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | } EIGEN_ALIGN16; 26 | 27 | POINT_CLOUD_REGISTER_POINT_STRUCT( 28 | PointXYZRGBLNormal, 29 | (float, x, x)(float, y, y)(float, z, z)(float, normal_x, normal_x)( 30 | float, normal_y, normal_y)(float, normal_z, 31 | normal_z)(float, rgb, rgb)(uint32_t, label, 32 | label)) 33 | 34 | class CloudSegmentation { 35 | public: 36 | struct Config { 37 | // Filtering. 38 | bool enable_filtering = false; 39 | float leaf_size = 0.02f; 40 | float min_distance = 0.0f; 41 | float max_distance = 3.0f; 42 | 43 | // Normal estimation. 44 | bool use_integral_image = false; 45 | float max_depth_change_factor = 0.01f; 46 | float normal_smoothing_size = 5.0f; 47 | 48 | // Region growing. 49 | int min_cluster_size = 30; 50 | int number_of_neighbours = 20; 51 | float smoothness_threshold_deg = 30.0f; 52 | float curvature_threshold = 0.5f; 53 | }; 54 | 55 | CloudSegmentation(const ros::NodeHandle& nh, 56 | const ros::NodeHandle& nh_private); 57 | 58 | Config getConfigFromParam(const ros::NodeHandle& nh_private); 59 | 60 | void cloudCallback(const sensor_msgs::PointCloud2::Ptr& pointcloud); 61 | 62 | void visualize(const Eigen::Matrix3f& camera_intrinsics, 63 | pcl::PointCloud::Ptr* colored_cloud); 64 | 65 | private: 66 | void filterPointcloud(); 67 | 68 | void normalEstimation(); 69 | 70 | // Normal estimation based on integral images. This method is faster, however 71 | // it can only be used when the cloud has not been filtered and hence is still 72 | // organized. 73 | void integralImageNormalEstimation(); 74 | 75 | void kdTreeNormalEstimation(); 76 | 77 | void regionGrowing(); 78 | 79 | void getSegmentedCloud(); 80 | 81 | pcl::PointCloud::Ptr getColoredCloud(); 82 | 83 | ros::NodeHandle nh_; 84 | ros::NodeHandle nh_private_; 85 | 86 | Config config_; 87 | 88 | // Subscriber. 89 | ros::Subscriber pointcloud_sub_; 90 | 91 | // Publishers. 92 | ros::Publisher segmented_cloud_pub_; 93 | 94 | // KDTree. 95 | pcl::search::KdTree::Ptr tree_; 96 | 97 | // Normals. 98 | pcl::PointCloud::Ptr normals_; 99 | 100 | // Region growing. 101 | pcl::RegionGrowing region_growing_; 102 | 103 | // Visualization. 104 | bool visualize_; 105 | bool write_frames_to_file_; 106 | 107 | std::thread vizualizer_thread_; 108 | 109 | pcl::PointCloud::Ptr pcl_pointcloud_; 110 | pcl::PointCloud::Ptr pcl_pointcloud_normals_; 111 | pcl::PointCloud::Ptr segmented_cloud_; 112 | 113 | pcl::PointCloud::Ptr colored_cloud_; 114 | bool updated_cloud_; 115 | 116 | std::vector clusters_; 117 | }; 118 | 119 | #endif // CLOUD_SEGMENTATION_CLOUD_SEGMENTATION_H_ 120 | -------------------------------------------------------------------------------- /include/cloud_segmentation/viewer_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | */ 38 | 39 | #ifndef CLOUD_SEGMENTATION_VIEWER_UTILS_H_ 40 | #define CLOUD_SEGMENTATION_VIEWER_UTILS_H_ 41 | 42 | inline void computeCameraParams(const Eigen::Matrix3f& intrinsics, 43 | const Eigen::Matrix4f& extrinsics, 44 | pcl::visualization::Camera* camera) { 45 | // Position = extrinsic translation. 46 | Eigen::Vector3f pos_vec = extrinsics.block<3, 1>(0, 3); 47 | 48 | // Rotate the view vector. 49 | Eigen::Matrix3f rotation = extrinsics.block<3, 3>(0, 0); 50 | Eigen::Vector3f y_axis(0.f, 1.f, 0.f); 51 | Eigen::Vector3f up_vec(rotation * y_axis); 52 | 53 | // Compute the new focal point. 54 | Eigen::Vector3f z_axis(0.f, 0.f, 1.f); 55 | Eigen::Vector3f focal_vec = pos_vec + rotation * z_axis; 56 | 57 | // Get the width and height of the image - assume the 58 | // calibrated centers are at the center of the image. 59 | Eigen::Vector2i window_size; 60 | window_size[0] = 2 * static_cast(intrinsics(0, 2)); 61 | window_size[1] = 2 * static_cast(intrinsics(1, 2)); 62 | 63 | // Compute the vertical field of view based 64 | // on the focal length and image height. 65 | double fovy = 2 * std::atan(window_size[1] / (2. * intrinsics(1, 1))); 66 | 67 | camera->pos[0] = pos_vec[0]; 68 | camera->pos[1] = pos_vec[1]; 69 | camera->pos[2] = pos_vec[2]; 70 | camera->focal[0] = focal_vec[0]; 71 | camera->focal[1] = focal_vec[1]; 72 | camera->focal[2] = focal_vec[2]; 73 | 74 | // The camera coordinate frame in the PCL visualizer 75 | // are rotated around the z axis by 180 degrees. 76 | camera->view[0] = -up_vec[0]; 77 | camera->view[1] = -up_vec[1]; 78 | camera->view[2] = -up_vec[2]; 79 | 80 | camera->fovy = fovy; 81 | camera->clip[0] = 0.01; 82 | camera->clip[1] = 1000.01; 83 | camera->window_size[0] = window_size[0]; 84 | camera->window_size[1] = window_size[1]; 85 | } 86 | 87 | #endif // CLOUD_SEGMENTATION_VIEWER_UTILS_H_ 88 | -------------------------------------------------------------------------------- /launch/cloud_segmentation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cloud_segmentation 4 | 1.0.0 5 | ROS node for region growing segmentation of point clouds. 6 | 7 | Margarita Grinvald 8 | 9 | Margarita Grinvald 10 | 11 | MIT 12 | 13 | catkin 14 | catkin_simple 15 | 16 | eigen_catkin 17 | glog_catkin 18 | pcl_catkin 19 | roscpp 20 | 21 | -------------------------------------------------------------------------------- /src/cloud_segmentation.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020- Margarita Grinvald, Autonomous Systems Lab, ETH Zurich 2 | // Licensed under the MIT License (see LICENSE for details) 3 | 4 | #include "cloud_segmentation/cloud_segmentation.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "cloud_segmentation/viewer_utils.h" 18 | 19 | CloudSegmentation::CloudSegmentation(const ros::NodeHandle& nh, 20 | const ros::NodeHandle& nh_private) 21 | : nh_(nh), 22 | nh_private_(nh_private), 23 | config_(getConfigFromParam(nh_private)), 24 | tree_(new pcl::search::KdTree()), 25 | normals_(new pcl::PointCloud), 26 | pcl_pointcloud_(new pcl::PointCloud), 27 | segmented_cloud_(new pcl::PointCloud), 28 | colored_cloud_(new pcl::PointCloud), 29 | visualize_(false) { 30 | pointcloud_sub_ = nh_.subscribe("/camera/depth_registered/points", 1, 31 | &CloudSegmentation::cloudCallback, this); 32 | 33 | segmented_cloud_pub_ = 34 | nh_private_.advertise("segmented_cloud", 1000); 35 | 36 | // Region growing config. 37 | region_growing_.setSearchMethod(tree_); 38 | region_growing_.setMinClusterSize(config_.min_cluster_size); 39 | region_growing_.setMaxClusterSize(10000000); 40 | region_growing_.setNumberOfNeighbours(config_.number_of_neighbours); 41 | float smoothness_threshold_rad = 42 | config_.smoothness_threshold_deg / 180.0 * M_PI; 43 | region_growing_.setSmoothnessThreshold(smoothness_threshold_rad); 44 | region_growing_.setCurvatureThreshold(config_.curvature_threshold); 45 | 46 | nh_private_.param("viewer/enable", visualize_, visualize_); 47 | 48 | if (visualize_) { 49 | std::vector camera_intrinsics_vec; 50 | nh_private.param>( 51 | "camera_intrinsics", camera_intrinsics_vec, camera_intrinsics_vec); 52 | 53 | Eigen::Matrix3f camera_intrinsics = 54 | Eigen::Map>( 55 | camera_intrinsics_vec.data()); 56 | 57 | vizualizer_thread_ = std::thread(&CloudSegmentation::visualize, this, 58 | camera_intrinsics, &colored_cloud_); 59 | 60 | nh_private_.param("viewer/write_frames_to_file", 61 | write_frames_to_file_, write_frames_to_file_); 62 | } 63 | } 64 | 65 | CloudSegmentation::Config CloudSegmentation::getConfigFromParam( 66 | const ros::NodeHandle& nh_private) { 67 | Config config; 68 | 69 | nh_private.param("filtering/enable", config.enable_filtering, 70 | config.enable_filtering); 71 | nh_private.param("filtering/leaf_size", config.leaf_size, config.leaf_size); 72 | nh_private.param("filtering/min_distance", config.min_distance, 73 | config.min_distance); 74 | nh_private.param("filtering/max_distance", config.max_distance, 75 | config.max_distance); 76 | 77 | nh_private.param("normal_estimation/max_depth_change_factor", 78 | config.max_depth_change_factor, 79 | config.max_depth_change_factor); 80 | nh_private.param("normal_estimation/normal_smoothing_size", 81 | config.normal_smoothing_size, config.normal_smoothing_size); 82 | 83 | nh_private.param("region_growing/min_cluster_size", config.min_cluster_size, 84 | config.min_cluster_size); 85 | nh_private.param("region_growing/number_of_neighbours", 86 | config.number_of_neighbours, config.number_of_neighbours); 87 | nh_private.param("region_growing/smoothness_threshold_deg", 88 | config.smoothness_threshold_deg, 89 | config.smoothness_threshold_deg); 90 | nh_private.param("region_growing/curvature_threshold", 91 | config.curvature_threshold, config.curvature_threshold); 92 | 93 | bool verbose_log = false; 94 | nh_private.param("debug/verbose_log", verbose_log, verbose_log); 95 | 96 | if (verbose_log) { 97 | FLAGS_stderrthreshold = 0; 98 | } 99 | 100 | return config; 101 | } 102 | 103 | void CloudSegmentation::visualize( 104 | const Eigen::Matrix3f& camera_intrinsics, 105 | pcl::PointCloud::Ptr* colored_cloud) { 106 | pcl::visualization::PCLVisualizer viewer("Cloud segmentation viewer"); 107 | 108 | viewer.setBackgroundColor(0.15, 0.15, 0.15); 109 | 110 | pcl::visualization::Camera camera; 111 | Eigen::Matrix4f camera_extrinsics = Eigen::Matrix4f::Identity(); 112 | computeCameraParams(camera_intrinsics, camera_extrinsics, &camera); 113 | viewer.setCameraParameters(camera); 114 | 115 | int frame = 1; 116 | while (!viewer.wasStopped()) { 117 | viewer.setCameraParameters(camera); 118 | 119 | if (updated_cloud_) { 120 | if (!viewer.updatePointCloud(*colored_cloud, "segmented_cloud")) { 121 | viewer.addPointCloud(*colored_cloud, "segmented_cloud"); 122 | viewer.setPointCloudRenderingProperties( 123 | pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, 124 | "segmented_cloud"); 125 | } 126 | if (write_frames_to_file_) { 127 | std::stringstream ss; 128 | ss << std::setw(4) << std::setfill('0') << static_cast(frame); 129 | viewer.saveScreenshot("cloud_segmentation/frame_" + ss.str() + ".png"); 130 | } 131 | 132 | ++frame; 133 | updated_cloud_ = false; 134 | } 135 | 136 | viewer.spinOnce(10); 137 | } 138 | } 139 | 140 | void CloudSegmentation::cloudCallback( 141 | const sensor_msgs::PointCloud2::Ptr& pointcloud_msg) { 142 | pcl::PCLPointCloud2::Ptr pointcloud2(new pcl::PCLPointCloud2()); 143 | 144 | pcl_conversions::moveToPCL(*pointcloud_msg, *pointcloud2); 145 | 146 | pcl::StopWatch watch_all; 147 | 148 | pcl_pointcloud_.reset(new pcl::PointCloud); 149 | pcl::fromPCLPointCloud2(*pointcloud2, *pcl_pointcloud_); 150 | 151 | if (config_.enable_filtering) { 152 | // Downsample the input pointcloud. 153 | pcl_pointcloud_normals_.reset(new pcl::PointCloud); 154 | filterPointcloud(); 155 | } 156 | 157 | // Estimate pointcloud normals. 158 | tree_.reset(new pcl::search::KdTree); 159 | kdTreeNormalEstimation(); 160 | 161 | // Region growing-based clustering. 162 | tree_.reset(new pcl::search::KdTree); 163 | regionGrowing(); 164 | 165 | LOG(INFO) << "Cloud segmentation completed in " << std::fixed 166 | << watch_all.getTimeSeconds() << " seconds." << std::endl 167 | << std::endl; 168 | 169 | // Extract the clusters into a labeled pointcloud. 170 | getSegmentedCloud(); 171 | 172 | if (segmented_cloud_->size()) { 173 | pcl::toPCLPointCloud2(*segmented_cloud_, *pointcloud2); 174 | } 175 | 176 | sensor_msgs::PointCloud2 segmented_cloud_msg; 177 | pcl_conversions::moveFromPCL(*pointcloud2, segmented_cloud_msg); 178 | 179 | segmented_cloud_msg.header.stamp = pointcloud_msg->header.stamp; 180 | segmented_cloud_msg.header.frame_id = pointcloud_msg->header.frame_id; 181 | segmented_cloud_pub_.publish(segmented_cloud_msg); 182 | 183 | if (visualize_) { 184 | colored_cloud_ = getColoredCloud(); 185 | updated_cloud_ = true; 186 | } 187 | } 188 | 189 | void CloudSegmentation::filterPointcloud() { 190 | pcl::VoxelGrid voxel_grid; 191 | voxel_grid.setInputCloud(pcl_pointcloud_); 192 | voxel_grid.setLeafSize(config_.leaf_size, config_.leaf_size, 193 | config_.leaf_size); 194 | 195 | voxel_grid.setFilterFieldName("z"); 196 | voxel_grid.setFilterLimits(config_.min_distance, config_.max_distance); 197 | 198 | voxel_grid.filter(*pcl_pointcloud_); 199 | } 200 | 201 | void CloudSegmentation::kdTreeNormalEstimation() { 202 | pcl::NormalEstimationOMP normal_estimator; 203 | 204 | normal_estimator.setSearchMethod(tree_); 205 | normal_estimator.setInputCloud(pcl_pointcloud_); 206 | normal_estimator.setKSearch(50); 207 | normal_estimator.compute(*normals_); 208 | } 209 | 210 | void CloudSegmentation::regionGrowing() { 211 | region_growing_.setSearchMethod(tree_); 212 | region_growing_.setInputCloud(pcl_pointcloud_); 213 | region_growing_.setInputNormals(normals_); 214 | 215 | region_growing_.extract(clusters_); 216 | } 217 | 218 | void CloudSegmentation::getSegmentedCloud() { 219 | segmented_cloud_->clear(); 220 | 221 | if (!clusters_.empty()) { 222 | segmented_cloud_->is_dense = pcl_pointcloud_->is_dense; 223 | 224 | for (size_t segment_idx = 0u; segment_idx < clusters_.size(); 225 | ++segment_idx) { 226 | for (const auto& index : clusters_[segment_idx].indices) { 227 | PointXYZRGBLNormal point; 228 | 229 | point.x = *(pcl_pointcloud_->points[index].data); 230 | point.y = *(pcl_pointcloud_->points[index].data + 1); 231 | point.z = *(pcl_pointcloud_->points[index].data + 2); 232 | point.normal_x = normals_->points[index].normal_x; 233 | point.normal_y = normals_->points[index].normal_y; 234 | point.normal_z = normals_->points[index].normal_z; 235 | point.r = pcl_pointcloud_->points[index].r; 236 | point.g = pcl_pointcloud_->points[index].g; 237 | point.b = pcl_pointcloud_->points[index].b; 238 | point.label = segment_idx; 239 | 240 | segmented_cloud_->points.push_back(point); 241 | } 242 | } 243 | } 244 | } 245 | 246 | pcl::PointCloud::Ptr CloudSegmentation::getColoredCloud() { 247 | pcl::PointCloud::Ptr colored_cloud; 248 | 249 | if (!clusters_.empty()) { 250 | colored_cloud = (new pcl::PointCloud)->makeShared(); 251 | 252 | srand(static_cast(time(nullptr))); 253 | std::vector colors; 254 | for (std::size_t i_segment = 0; i_segment < clusters_.size(); i_segment++) { 255 | colors.push_back(static_cast(rand() % 256)); 256 | colors.push_back(static_cast(rand() % 256)); 257 | colors.push_back(static_cast(rand() % 256)); 258 | } 259 | 260 | colored_cloud->width = pcl_pointcloud_->width; 261 | colored_cloud->height = pcl_pointcloud_->height; 262 | colored_cloud->is_dense = pcl_pointcloud_->is_dense; 263 | 264 | int next_color = 0; 265 | for (const auto& i_segment : clusters_) { 266 | for (const auto& index : i_segment.indices) { 267 | pcl::PointXYZRGB point; 268 | point.x = *(pcl_pointcloud_->points[index].data); 269 | point.y = *(pcl_pointcloud_->points[index].data + 1); 270 | point.z = *(pcl_pointcloud_->points[index].data + 2); 271 | point.r = colors[3 * next_color]; 272 | point.g = colors[3 * next_color + 1]; 273 | point.b = colors[3 * next_color + 2]; 274 | colored_cloud->points.push_back(point); 275 | } 276 | next_color++; 277 | } 278 | } 279 | 280 | return (colored_cloud); 281 | } 282 | -------------------------------------------------------------------------------- /src/node.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "cloud_segmentation/cloud_segmentation.h" 5 | 6 | int main(int argc, char** argv) { 7 | std::cout << std::endl 8 | << "Point cloud segmentation ROS node - Copyright (c) 2020- " 9 | "Margarita Grinvald, Autonomous " 10 | "Systems Lab, ETH Zurich." 11 | << std::endl 12 | << std::endl; 13 | 14 | ros::init(argc, argv, "cloud_segmentation_node"); 15 | google::InitGoogleLogging(argv[0]); 16 | 17 | ros::NodeHandle nh; 18 | ros::NodeHandle nh_private("~"); 19 | 20 | CloudSegmentation cloud_segmentation(nh, nh_private); 21 | 22 | ros::spin(); 23 | 24 | return 0; 25 | } 26 | --------------------------------------------------------------------------------