├── msg └── ClusterArray.msg ├── launch ├── adaptive_clustering.launch └── adaptive_clustering.rviz ├── .gitignore ├── CMakeLists.txt ├── .travis.yml ├── package.xml ├── LICENSE ├── README.md └── src └── adaptive_clustering.cpp /msg/ClusterArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | sensor_msgs/PointCloud2[] clusters -------------------------------------------------------------------------------- /launch/adaptive_clustering.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.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(adaptive_clustering) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs visualization_msgs geometry_msgs pcl_conversions pcl_ros) 5 | 6 | find_package(PCL REQUIRED) 7 | 8 | include_directories(include ${catkin_INCLUDE_DIRS}) 9 | 10 | add_message_files(FILES ClusterArray.msg) 11 | 12 | generate_messages(DEPENDENCIES std_msgs sensor_msgs) 13 | 14 | catkin_package() 15 | 16 | add_executable(adaptive_clustering src/adaptive_clustering.cpp) 17 | target_link_libraries(adaptive_clustering ${catkin_LIBRARIES}) 18 | if(catkin_EXPORTED_TARGETS) 19 | add_dependencies(adaptive_clustering ${catkin_EXPORTED_TARGETS}) 20 | endif() 21 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: xenial 2 | sudo: required 3 | language: generic 4 | cache: apt 5 | 6 | install: 7 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 8 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 9 | - sudo apt-get update 10 | - sudo apt-get install ros-kinetic-desktop-full 11 | - source /opt/ros/kinetic/setup.bash 12 | - pip install catkin_pkg empy 13 | - mkdir -p ~/catkin_ws/src 14 | - cd ~/catkin_ws/ 15 | - catkin_make 16 | - source devel/setup.bash 17 | 18 | script: 19 | - cd ~/catkin_ws/src 20 | - git clone https://github.com/yzrobot/adaptive_clustering.git 21 | - cd ~/catkin_ws 22 | - catkin_make 23 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | adaptive_clustering 4 | 0.0.1 5 | Light-weight real-time high-accuracy point cloud clustering. 6 | Zhi Yan 7 | Li Sun 8 | BSD 9 | 10 | catkin 11 | 12 | roscpp 13 | sensor_msgs 14 | visualization_msgs 15 | geometry_msgs 16 | pcl_conversions 17 | pcl_ros 18 | 19 | roscpp 20 | sensor_msgs 21 | visualization_msgs 22 | geometry_msgs 23 | pcl_conversions 24 | pcl_ros 25 | 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Zhi Yan 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. 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. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Adaptive Clustering: A lightweight and accurate point cloud clustering method # 2 | 3 | [![Build Status](https://travis-ci.org/yzrobot/adaptive_clustering.svg?branch=master)](https://travis-ci.org/yzrobot/adaptive_clustering) 4 | [![Codacy Badge](https://api.codacy.com/project/badge/Grade/61a01a79a7ac41fd9deded9050ef6030)](https://www.codacy.com/app/yzrobot/adaptive_clustering?utm_source=github.com&utm_medium=referral&utm_content=yzrobot/adaptive_clustering&utm_campaign=Badge_Grade) 5 | [![License](https://img.shields.io/badge/License-BSD%203--Clause-green.svg)](https://opensource.org/licenses/BSD-3-Clause) 6 | 7 | [![YouTube Video](https://img.youtube.com/vi/rmPn7mWssto/0.jpg)](https://www.youtube.com/watch?v=rmPn7mWssto) 8 | 9 | ## Changelog ## 10 | 11 | * **\[Apr 14, 2022\]:** Two new branches, [gpu](https://github.com/yzrobot/adaptive_clustering/tree/gpu) and [agx](https://github.com/yzrobot/adaptive_clustering/tree/agx), have been created for GPU-based implementations: 12 | * [gpu](https://github.com/yzrobot/adaptive_clustering/tree/gpu) is based on [PCL-GPU](https://pcl.readthedocs.io/projects/tutorials/en/master/#gpu) and has been tested with an NVIDIA TITAN Xp. 13 | * [agx](https://github.com/yzrobot/adaptive_clustering/tree/agx) is based on [CUDA-PCL](https://github.com/NVIDIA-AI-IOT/cuda-pcl) and has been tested with an NVIDIA Jetson AGX Xavier. 14 | 15 | * **\[Feb 25, 2019\]:** A new branch, [devel](https://github.com/yzrobot/adaptive_clustering/tree/devel), faster (by downsampling) and better (by merging clusters split by nested regions and on the z-axis). 16 | 17 | ## How to build ## 18 | ```sh 19 | cd ~/catkin_ws/src/ 20 | git clone https://github.com/yzrobot/adaptive_clustering.git 21 | cd ~/catkin_ws 22 | catkin_make 23 | ``` 24 | 25 | ## Citation ## 26 | If you are considering using this code, please reference the following: 27 | ``` 28 | @article{yz19auro, 29 | author = {Zhi Yan and Tom Duckett and Nicola Bellotto}, 30 | title = {Online learning for 3D LiDAR-based human detection: Experimental analysis of point cloud clustering and classification methods}, 31 | journal = {Autonomous Robots}, 32 | year = {2019} 33 | } 34 | ``` 35 | -------------------------------------------------------------------------------- /launch/adaptive_clustering.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 632 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Velodyne 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: false 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: false 51 | - Alpha: 1 52 | Autocompute Intensity Bounds: true 53 | Autocompute Value Bounds: 54 | Max Value: 7.27798986 55 | Min Value: -0.939535022 56 | Value: true 57 | Axis: Z 58 | Channel Name: intensity 59 | Class: rviz/PointCloud2 60 | Color: 255; 255; 255 61 | Color Transformer: Intensity 62 | Decay Time: 0 63 | Enabled: true 64 | Invert Rainbow: false 65 | Max Color: 255; 255; 255 66 | Max Intensity: 111 67 | Min Color: 0; 0; 0 68 | Min Intensity: 0 69 | Name: Velodyne 70 | Position Transformer: XYZ 71 | Queue Size: 10 72 | Selectable: true 73 | Size (Pixels): 1 74 | Size (m): 0.0199999996 75 | Style: Points 76 | Topic: /velodyne_points 77 | Unreliable: false 78 | Use Fixed Frame: true 79 | Use rainbow: true 80 | Value: true 81 | - Alpha: 1 82 | Autocompute Intensity Bounds: true 83 | Autocompute Value Bounds: 84 | Max Value: 10 85 | Min Value: -10 86 | Value: true 87 | Axis: Z 88 | Channel Name: intensity 89 | Class: rviz/PointCloud2 90 | Color: 255; 255; 255 91 | Color Transformer: Intensity 92 | Decay Time: 0 93 | Enabled: true 94 | Invert Rainbow: false 95 | Max Color: 255; 255; 255 96 | Max Intensity: 0 97 | Min Color: 0; 0; 0 98 | Min Intensity: 0 99 | Name: Cloud_filtered 100 | Position Transformer: XYZ 101 | Queue Size: 10 102 | Selectable: true 103 | Size (Pixels): 2 104 | Size (m): 0.00999999978 105 | Style: Points 106 | Topic: /adaptive_clustering/cloud_filtered 107 | Unreliable: false 108 | Use Fixed Frame: true 109 | Use rainbow: true 110 | Value: true 111 | - Class: rviz/MarkerArray 112 | Enabled: true 113 | Marker Topic: /adaptive_clustering/markers 114 | Name: Clusters 115 | Namespaces: 116 | adaptive_clustering: true 117 | Queue Size: 100 118 | Value: true 119 | - Alpha: 1 120 | Arrow Length: 0.300000012 121 | Axes Length: 0.300000012 122 | Axes Radius: 0.00999999978 123 | Class: rviz/PoseArray 124 | Color: 0; 170; 255 125 | Enabled: false 126 | Head Length: 0.0700000003 127 | Head Radius: 0.0299999993 128 | Name: Cluster_centroid 129 | Shaft Length: 0.230000004 130 | Shaft Radius: 0.00999999978 131 | Shape: Arrow (Flat) 132 | Topic: /adaptive_clustering/poses 133 | Unreliable: false 134 | Value: false 135 | Enabled: true 136 | Global Options: 137 | Background Color: 48; 48; 48 138 | Fixed Frame: velodyne 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/FocusCamera 147 | - Class: rviz/Measure 148 | - Class: rviz/SetInitialPose 149 | Topic: /initialpose 150 | - Class: rviz/SetGoal 151 | Topic: /move_base_simple/goal 152 | - Class: rviz/PublishPoint 153 | Single click: true 154 | Topic: /clicked_point 155 | Value: true 156 | Views: 157 | Current: 158 | Class: rviz/Orbit 159 | Distance: 33.7053566 160 | Enable Stereo Rendering: 161 | Stereo Eye Separation: 0.0599999987 162 | Stereo Focal Distance: 1 163 | Swap Stereo Eyes: false 164 | Value: false 165 | Focal Point: 166 | X: 0.63240701 167 | Y: -0.142278999 168 | Z: -0.571869016 169 | Focal Shape Fixed Size: true 170 | Focal Shape Size: 0.0500000007 171 | Invert Z Axis: false 172 | Name: Current View 173 | Near Clip Distance: 0.00999999978 174 | Pitch: 0.470398456 175 | Target Frame: 176 | Value: Orbit (rviz) 177 | Yaw: 4.88358212 178 | Saved: ~ 179 | Window Geometry: 180 | Displays: 181 | collapsed: false 182 | Height: 913 183 | Hide Left Dock: false 184 | Hide Right Dock: true 185 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000307fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000307000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001d9000000160000000000000000fb0000000a0049006d00610067006500000002ac000001120000000000000000000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002b3000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051f0000003efc0100000002fb0000000800540069006d006501000000000000051f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003af0000030700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 186 | Selection: 187 | collapsed: false 188 | Time: 189 | collapsed: false 190 | Tool Properties: 191 | collapsed: false 192 | Views: 193 | collapsed: true 194 | Width: 1311 195 | X: 65 196 | Y: 24 197 | -------------------------------------------------------------------------------- /src/adaptive_clustering.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2018 Zhi Yan and Li Sun 2 | 3 | // This program is free software: you can redistribute it and/or modify it 4 | // under the terms of the GNU General Public License as published by the Free 5 | // Software Foundation, either version 3 of the License, or (at your option) 6 | // any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, but WITHOUT 9 | // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 10 | // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 11 | // more details. 12 | 13 | // You should have received a copy of the GNU General Public License along 14 | // with this program. If not, see . 15 | 16 | // ROS 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "adaptive_clustering/ClusterArray.h" 22 | 23 | // PCL 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | //#define LOG 31 | 32 | ros::Publisher cluster_array_pub_; 33 | ros::Publisher cloud_filtered_pub_; 34 | ros::Publisher pose_array_pub_; 35 | ros::Publisher marker_array_pub_; 36 | 37 | bool print_fps_; 38 | int leaf_; 39 | float z_axis_min_; 40 | float z_axis_max_; 41 | int cluster_size_min_; 42 | int cluster_size_max_; 43 | 44 | const int region_max_ = 10; // Change this value to match how far you want to detect. 45 | int regions_[100]; 46 | 47 | int frames; clock_t start_time; bool reset = true;//fps 48 | void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& ros_pc2_in) { 49 | if(print_fps_)if(reset){frames=0;start_time=clock();reset=false;}//fps 50 | 51 | /*** Convert ROS message to PCL ***/ 52 | pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); 53 | pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in); 54 | 55 | /*** Downsampling + ground & ceiling removal ***/ 56 | pcl::IndicesPtr pc_indices(new std::vector); 57 | for(int i = 0; i < pcl_pc_in->size(); ++i) { 58 | if(i % leaf_ == 0) { 59 | if(pcl_pc_in->points[i].z >= z_axis_min_ && pcl_pc_in->points[i].z <= z_axis_max_) { 60 | pc_indices->push_back(i); 61 | } 62 | } 63 | } 64 | 65 | /*** Divide the point cloud into nested circular regions ***/ 66 | boost::array, region_max_> indices_array; 67 | for(int i = 0; i < pc_indices->size(); i++) { 68 | float range = 0.0; 69 | for(int j = 0; j < region_max_; j++) { 70 | float d2 = pcl_pc_in->points[(*pc_indices)[i]].x * pcl_pc_in->points[(*pc_indices)[i]].x + 71 | pcl_pc_in->points[(*pc_indices)[i]].y * pcl_pc_in->points[(*pc_indices)[i]].y + 72 | pcl_pc_in->points[(*pc_indices)[i]].z * pcl_pc_in->points[(*pc_indices)[i]].z; 73 | if(d2 > range * range && d2 <= (range+regions_[j]) * (range+regions_[j])) { 74 | indices_array[j].push_back((*pc_indices)[i]); 75 | break; 76 | } 77 | range += regions_[j]; 78 | } 79 | } 80 | 81 | /*** Euclidean clustering ***/ 82 | float tolerance = 0.0; 83 | std::vector::Ptr, Eigen::aligned_allocator::Ptr > > clusters; 84 | 85 | for(int i = 0; i < region_max_; i++) { 86 | tolerance += 0.1; 87 | if(indices_array[i].size() > cluster_size_min_) { 88 | boost::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); 89 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); 90 | tree->setInputCloud(pcl_pc_in, indices_array_ptr); 91 | 92 | std::vector cluster_indices; 93 | pcl::EuclideanClusterExtraction ec; 94 | ec.setClusterTolerance(tolerance); 95 | ec.setMinClusterSize(cluster_size_min_); 96 | ec.setMaxClusterSize(cluster_size_max_); 97 | ec.setSearchMethod(tree); 98 | ec.setInputCloud(pcl_pc_in); 99 | ec.setIndices(indices_array_ptr); 100 | ec.extract(cluster_indices); 101 | 102 | for(std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++) { 103 | pcl::PointCloud::Ptr cluster(new pcl::PointCloud); 104 | for(std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { 105 | cluster->points.push_back(pcl_pc_in->points[*pit]); 106 | } 107 | cluster->width = cluster->size(); 108 | cluster->height = 1; 109 | cluster->is_dense = true; 110 | clusters.push_back(cluster); 111 | } 112 | } 113 | } 114 | 115 | /*** Output ***/ 116 | if(cloud_filtered_pub_.getNumSubscribers() > 0) { 117 | pcl::PointCloud::Ptr pcl_pc_out(new pcl::PointCloud); 118 | sensor_msgs::PointCloud2 ros_pc2_out; 119 | pcl::copyPointCloud(*pcl_pc_in, *pc_indices, *pcl_pc_out); 120 | pcl::toROSMsg(*pcl_pc_out, ros_pc2_out); 121 | cloud_filtered_pub_.publish(ros_pc2_out); 122 | } 123 | 124 | adaptive_clustering::ClusterArray cluster_array; 125 | geometry_msgs::PoseArray pose_array; 126 | visualization_msgs::MarkerArray marker_array; 127 | 128 | for(int i = 0; i < clusters.size(); i++) { 129 | if(cluster_array_pub_.getNumSubscribers() > 0) { 130 | sensor_msgs::PointCloud2 ros_pc2_out; 131 | pcl::toROSMsg(*clusters[i], ros_pc2_out); 132 | cluster_array.clusters.push_back(ros_pc2_out); 133 | } 134 | 135 | if(pose_array_pub_.getNumSubscribers() > 0) { 136 | Eigen::Vector4f centroid; 137 | pcl::compute3DCentroid(*clusters[i], centroid); 138 | 139 | geometry_msgs::Pose pose; 140 | pose.position.x = centroid[0]; 141 | pose.position.y = centroid[1]; 142 | pose.position.z = centroid[2]; 143 | pose.orientation.w = 1; 144 | pose_array.poses.push_back(pose); 145 | 146 | #ifdef LOG 147 | Eigen::Vector4f min, max; 148 | pcl::getMinMax3D(*clusters[i], min, max); 149 | std::cerr << ros_pc2_in->header.seq << " " 150 | << ros_pc2_in->header.stamp << " " 151 | << min[0] << " " 152 | << min[1] << " " 153 | << min[2] << " " 154 | << max[0] << " " 155 | << max[1] << " " 156 | << max[2] << " " 157 | << std::endl; 158 | #endif 159 | } 160 | 161 | if(marker_array_pub_.getNumSubscribers() > 0) { 162 | Eigen::Vector4f min, max; 163 | pcl::getMinMax3D(*clusters[i], min, max); 164 | 165 | visualization_msgs::Marker marker; 166 | marker.header = ros_pc2_in->header; 167 | marker.ns = "adaptive_clustering"; 168 | marker.id = i; 169 | marker.type = visualization_msgs::Marker::LINE_LIST; 170 | 171 | geometry_msgs::Point p[24]; 172 | p[0].x = max[0]; p[0].y = max[1]; p[0].z = max[2]; 173 | p[1].x = min[0]; p[1].y = max[1]; p[1].z = max[2]; 174 | p[2].x = max[0]; p[2].y = max[1]; p[2].z = max[2]; 175 | p[3].x = max[0]; p[3].y = min[1]; p[3].z = max[2]; 176 | p[4].x = max[0]; p[4].y = max[1]; p[4].z = max[2]; 177 | p[5].x = max[0]; p[5].y = max[1]; p[5].z = min[2]; 178 | p[6].x = min[0]; p[6].y = min[1]; p[6].z = min[2]; 179 | p[7].x = max[0]; p[7].y = min[1]; p[7].z = min[2]; 180 | p[8].x = min[0]; p[8].y = min[1]; p[8].z = min[2]; 181 | p[9].x = min[0]; p[9].y = max[1]; p[9].z = min[2]; 182 | p[10].x = min[0]; p[10].y = min[1]; p[10].z = min[2]; 183 | p[11].x = min[0]; p[11].y = min[1]; p[11].z = max[2]; 184 | p[12].x = min[0]; p[12].y = max[1]; p[12].z = max[2]; 185 | p[13].x = min[0]; p[13].y = max[1]; p[13].z = min[2]; 186 | p[14].x = min[0]; p[14].y = max[1]; p[14].z = max[2]; 187 | p[15].x = min[0]; p[15].y = min[1]; p[15].z = max[2]; 188 | p[16].x = max[0]; p[16].y = min[1]; p[16].z = max[2]; 189 | p[17].x = max[0]; p[17].y = min[1]; p[17].z = min[2]; 190 | p[18].x = max[0]; p[18].y = min[1]; p[18].z = max[2]; 191 | p[19].x = min[0]; p[19].y = min[1]; p[19].z = max[2]; 192 | p[20].x = max[0]; p[20].y = max[1]; p[20].z = min[2]; 193 | p[21].x = min[0]; p[21].y = max[1]; p[21].z = min[2]; 194 | p[22].x = max[0]; p[22].y = max[1]; p[22].z = min[2]; 195 | p[23].x = max[0]; p[23].y = min[1]; p[23].z = min[2]; 196 | for(int i = 0; i < 24; i++) { 197 | marker.points.push_back(p[i]); 198 | } 199 | 200 | marker.scale.x = 0.02; 201 | marker.color.a = 1.0; 202 | marker.color.r = 0.0; 203 | marker.color.g = 1.0; 204 | marker.color.b = 0.5; 205 | marker.lifetime = ros::Duration(0.1); 206 | marker_array.markers.push_back(marker); 207 | } 208 | } 209 | 210 | if(cluster_array.clusters.size()) { 211 | cluster_array.header = ros_pc2_in->header; 212 | cluster_array_pub_.publish(cluster_array); 213 | } 214 | 215 | if(pose_array.poses.size()) { 216 | pose_array.header = ros_pc2_in->header; 217 | pose_array_pub_.publish(pose_array); 218 | } 219 | 220 | if(marker_array.markers.size()) { 221 | marker_array_pub_.publish(marker_array); 222 | } 223 | 224 | if(print_fps_)if(++frames>10){std::cerr<<"[adaptive_clustering] fps = "<("velodyne_points", 1, pointCloudCallback); 233 | 234 | /*** Publishers ***/ 235 | ros::NodeHandle private_nh("~"); 236 | cluster_array_pub_ = private_nh.advertise("clusters", 100); 237 | cloud_filtered_pub_ = private_nh.advertise("cloud_filtered", 100); 238 | pose_array_pub_ = private_nh.advertise("poses", 100); 239 | marker_array_pub_ = private_nh.advertise("markers", 100); 240 | 241 | /*** Parameters ***/ 242 | std::string sensor_model; 243 | 244 | private_nh.param("sensor_model", sensor_model, "VLP-16"); // VLP-16, HDL-32E, HDL-64E 245 | private_nh.param("print_fps", print_fps_, false); 246 | private_nh.param("leaf", leaf_, 1); 247 | private_nh.param("z_axis_min", z_axis_min_, -0.8); 248 | private_nh.param("z_axis_max", z_axis_max_, 2.0); 249 | private_nh.param("cluster_size_min", cluster_size_min_, 3); 250 | private_nh.param("cluster_size_max", cluster_size_max_, 2200000); 251 | 252 | // Divide the point cloud into nested circular regions centred at the sensor. 253 | // For more details, see our IROS-17 paper "Online learning for human classification in 3D LiDAR-based tracking" 254 | if(sensor_model.compare("VLP-16") == 0) { 255 | regions_[0] = 2; regions_[1] = 3; regions_[2] = 3; regions_[3] = 3; regions_[4] = 3; 256 | regions_[5] = 3; regions_[6] = 3; regions_[7] = 2; regions_[8] = 3; regions_[9] = 3; 257 | regions_[10]= 3; regions_[11]= 3; regions_[12]= 3; regions_[13]= 3; 258 | } else if (sensor_model.compare("HDL-32E") == 0) { 259 | regions_[0] = 4; regions_[1] = 5; regions_[2] = 4; regions_[3] = 5; regions_[4] = 4; 260 | regions_[5] = 5; regions_[6] = 5; regions_[7] = 4; regions_[8] = 5; regions_[9] = 4; 261 | regions_[10]= 5; regions_[11]= 5; regions_[12]= 4; regions_[13]= 5; 262 | } else if (sensor_model.compare("HDL-64E") == 0) { 263 | regions_[0] = 14; regions_[1] = 14; regions_[2] = 14; regions_[3] = 15; regions_[4] = 14; 264 | } else { 265 | ROS_FATAL("Unknown sensor model!"); 266 | } 267 | 268 | ros::spin(); 269 | 270 | return 0; 271 | } 272 | --------------------------------------------------------------------------------