├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── adaptive_clustering.launch └── adaptive_clustering.rviz ├── msg └── ClusterArray.msg ├── package.xml └── src └── adaptive_clustering.cpp /.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 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 # 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 | A lightweight and accurate point cloud clustering method. 8 | 9 | [![YouTube Video](https://img.youtube.com/vi/rmPn7mWssto/0.jpg)](https://www.youtube.com/watch?v=rmPn7mWssto) 10 | 11 | ## How to build ## 12 | ``` 13 | $ cd ~/catkin_ws/src/ 14 | $ git clone https://github.com/yzrobot/adaptive_clustering.git 15 | $ cd ~/catkin_ws 16 | $ catkin_make 17 | ``` 18 | 19 | ## Citation ## 20 | If you are considering using this code, please reference the following: 21 | ``` 22 | @article{yz19auro, 23 | author = {Zhi Yan and Tom Duckett and Nicola Bellotto}, 24 | title = {Online learning for 3D LiDAR-based human detection: Experimental analysis of point cloud clustering and classification methods}, 25 | journal = {Autonomous Robots}, 26 | year = {2019} 27 | } 28 | ``` 29 | -------------------------------------------------------------------------------- /launch/adaptive_clustering.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /msg/ClusterArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | sensor_msgs/PointCloud2[] clusters -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | // PCL 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | ros::Publisher cluster_array_pub_; 31 | ros::Publisher cloud_filtered_pub_; 32 | ros::Publisher pose_array_pub_; 33 | ros::Publisher marker_array_pub_; 34 | 35 | std::string sensor_model_; 36 | std::string frame_id_; 37 | bool print_fps_; 38 | float z_axis_min_; 39 | float z_axis_max_; 40 | int cluster_size_min_; 41 | int cluster_size_max_; 42 | 43 | const int region_max_ = 7; // Change this value to match how far you want to detect. 44 | int regions_[100]; 45 | uint32_t cluster_array_seq_ = 0; 46 | uint32_t pose_array_seq_ = 0; 47 | 48 | Eigen::Vector4f min_, max_; 49 | 50 | int frames; clock_t start_time; bool reset = true;//fps 51 | void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& ros_pc2_in) { 52 | if(print_fps_)if(reset){frames=0;start_time=clock();reset=false;}//fps 53 | 54 | /*** Convert ROS message to PCL ***/ 55 | pcl::PointCloud::Ptr pcl_pc_in(new pcl::PointCloud); 56 | pcl::fromROSMsg(*ros_pc2_in, *pcl_pc_in); 57 | 58 | /*** Remove ground and ceiling ***/ 59 | pcl::IndicesPtr pc_indices(new std::vector); 60 | pcl::PassThrough pt; 61 | pt.setInputCloud(pcl_pc_in); 62 | pt.setFilterFieldName("z"); 63 | pt.setFilterLimits(z_axis_min_, z_axis_max_); 64 | pt.filter(*pc_indices); 65 | 66 | /*** Divide the point cloud into nested circular regions ***/ 67 | boost::array, region_max_> indices_array; 68 | for(int i = 0; i < pc_indices->size(); i++) { 69 | float range = 0.0; 70 | for(int j = 0; j < region_max_; j++) { 71 | float d2 = pcl_pc_in->points[(*pc_indices)[i]].x * pcl_pc_in->points[(*pc_indices)[i]].x + 72 | pcl_pc_in->points[(*pc_indices)[i]].y * pcl_pc_in->points[(*pc_indices)[i]].y + 73 | pcl_pc_in->points[(*pc_indices)[i]].z * pcl_pc_in->points[(*pc_indices)[i]].z; 74 | if(d2 > range * range && d2 <= (range+regions_[j]) * (range+regions_[j])) { 75 | indices_array[j].push_back((*pc_indices)[i]); 76 | break; 77 | } 78 | range += regions_[j]; 79 | } 80 | } 81 | 82 | /*** Euclidean clustering ***/ 83 | float tolerance = 0.0; 84 | std::vector::Ptr, Eigen::aligned_allocator::Ptr > > clusters; 85 | 86 | for(int i = 0; i < region_max_; i++) { 87 | tolerance += 0.1; 88 | if(indices_array[i].size() > cluster_size_min_) { 89 | boost::shared_ptr > indices_array_ptr(new std::vector(indices_array[i])); 90 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); 91 | tree->setInputCloud(pcl_pc_in, indices_array_ptr); 92 | 93 | std::vector cluster_indices; 94 | pcl::EuclideanClusterExtraction ec; 95 | ec.setClusterTolerance(tolerance); 96 | ec.setMinClusterSize(cluster_size_min_); 97 | ec.setMaxClusterSize(cluster_size_max_); 98 | ec.setSearchMethod(tree); 99 | ec.setInputCloud(pcl_pc_in); 100 | ec.setIndices(indices_array_ptr); 101 | ec.extract(cluster_indices); 102 | 103 | for(std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++) { 104 | pcl::PointCloud::Ptr cluster(new pcl::PointCloud); 105 | for(std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { 106 | cluster->points.push_back(pcl_pc_in->points[*pit]); 107 | } 108 | cluster->width = cluster->size(); 109 | cluster->height = 1; 110 | cluster->is_dense = true; 111 | clusters.push_back(cluster); 112 | } 113 | } 114 | } 115 | 116 | /*** Output ***/ 117 | if(cloud_filtered_pub_.getNumSubscribers() > 0) { 118 | pcl::PointCloud::Ptr pcl_pc_out(new pcl::PointCloud); 119 | sensor_msgs::PointCloud2 ros_pc2_out; 120 | pcl::copyPointCloud(*pcl_pc_in, *pc_indices, *pcl_pc_out); 121 | pcl::toROSMsg(*pcl_pc_out, ros_pc2_out); 122 | cloud_filtered_pub_.publish(ros_pc2_out); 123 | } 124 | 125 | adaptive_clustering::ClusterArray cluster_array; 126 | geometry_msgs::PoseArray pose_array; 127 | visualization_msgs::MarkerArray marker_array; 128 | 129 | for(int i = 0; i < clusters.size(); i++) { 130 | if(cluster_array_pub_.getNumSubscribers() > 0) { 131 | sensor_msgs::PointCloud2 ros_pc2_out; 132 | pcl::toROSMsg(*clusters[i], ros_pc2_out); 133 | cluster_array.clusters.push_back(ros_pc2_out); 134 | } 135 | 136 | if(pose_array_pub_.getNumSubscribers() > 0) { 137 | Eigen::Vector4f centroid; 138 | pcl::compute3DCentroid(*clusters[i], centroid); 139 | 140 | geometry_msgs::Pose pose; 141 | pose.position.x = centroid[0]; 142 | pose.position.y = centroid[1]; 143 | pose.position.z = centroid[2]; 144 | pose.orientation.w = 1; 145 | pose_array.poses.push_back(pose); 146 | } 147 | 148 | if(marker_array_pub_.getNumSubscribers() > 0) { 149 | Eigen::Vector4f min, max; 150 | pcl::getMinMax3D(*clusters[i], min, max); 151 | 152 | visualization_msgs::Marker marker; 153 | marker.header.stamp = ros::Time::now(); 154 | marker.header.frame_id = frame_id_; 155 | marker.ns = "adaptive_clustering"; 156 | marker.id = i; 157 | marker.type = visualization_msgs::Marker::LINE_LIST; 158 | 159 | geometry_msgs::Point p[24]; 160 | p[0].x = max[0]; p[0].y = max[1]; p[0].z = max[2]; 161 | p[1].x = min[0]; p[1].y = max[1]; p[1].z = max[2]; 162 | p[2].x = max[0]; p[2].y = max[1]; p[2].z = max[2]; 163 | p[3].x = max[0]; p[3].y = min[1]; p[3].z = max[2]; 164 | p[4].x = max[0]; p[4].y = max[1]; p[4].z = max[2]; 165 | p[5].x = max[0]; p[5].y = max[1]; p[5].z = min[2]; 166 | p[6].x = min[0]; p[6].y = min[1]; p[6].z = min[2]; 167 | p[7].x = max[0]; p[7].y = min[1]; p[7].z = min[2]; 168 | p[8].x = min[0]; p[8].y = min[1]; p[8].z = min[2]; 169 | p[9].x = min[0]; p[9].y = max[1]; p[9].z = min[2]; 170 | p[10].x = min[0]; p[10].y = min[1]; p[10].z = min[2]; 171 | p[11].x = min[0]; p[11].y = min[1]; p[11].z = max[2]; 172 | p[12].x = min[0]; p[12].y = max[1]; p[12].z = max[2]; 173 | p[13].x = min[0]; p[13].y = max[1]; p[13].z = min[2]; 174 | p[14].x = min[0]; p[14].y = max[1]; p[14].z = max[2]; 175 | p[15].x = min[0]; p[15].y = min[1]; p[15].z = max[2]; 176 | p[16].x = max[0]; p[16].y = min[1]; p[16].z = max[2]; 177 | p[17].x = max[0]; p[17].y = min[1]; p[17].z = min[2]; 178 | p[18].x = max[0]; p[18].y = min[1]; p[18].z = max[2]; 179 | p[19].x = min[0]; p[19].y = min[1]; p[19].z = max[2]; 180 | p[20].x = max[0]; p[20].y = max[1]; p[20].z = min[2]; 181 | p[21].x = min[0]; p[21].y = max[1]; p[21].z = min[2]; 182 | p[22].x = max[0]; p[22].y = max[1]; p[22].z = min[2]; 183 | p[23].x = max[0]; p[23].y = min[1]; p[23].z = min[2]; 184 | for(int i = 0; i < 24; i++) { 185 | marker.points.push_back(p[i]); 186 | } 187 | 188 | marker.scale.x = 0.02; 189 | marker.color.a = 1.0; 190 | marker.color.r = 0.0; 191 | marker.color.g = 1.0; 192 | marker.color.b = 0.5; 193 | marker.lifetime = ros::Duration(0.1); 194 | marker_array.markers.push_back(marker); 195 | } 196 | } 197 | 198 | if(cluster_array.clusters.size()) { 199 | cluster_array.header.seq = ++cluster_array_seq_; 200 | cluster_array.header.stamp = ros::Time::now(); 201 | cluster_array.header.frame_id = frame_id_; 202 | cluster_array_pub_.publish(cluster_array); 203 | } 204 | 205 | if(pose_array.poses.size()) { 206 | pose_array.header.seq = ++pose_array_seq_; 207 | pose_array.header.stamp = ros::Time::now(); 208 | pose_array.header.frame_id = frame_id_; 209 | pose_array_pub_.publish(pose_array); 210 | } 211 | 212 | if(marker_array.markers.size()) { 213 | marker_array_pub_.publish(marker_array); 214 | } 215 | 216 | if(print_fps_)if(++frames>10){std::cerr<<"[adaptive_clustering] fps = "<("velodyne_points", 1, pointCloudCallback); 225 | 226 | /*** Publishers ***/ 227 | ros::NodeHandle private_nh("~"); 228 | cluster_array_pub_ = private_nh.advertise("clusters", 100); 229 | cloud_filtered_pub_ = private_nh.advertise("cloud_filtered", 100); 230 | pose_array_pub_ = private_nh.advertise("poses", 100); 231 | marker_array_pub_ = private_nh.advertise("markers", 100); 232 | 233 | /*** Parameters ***/ 234 | private_nh.param("sensor_model", sensor_model_, "HDL-32E"); // VLP-16, HDL-32E, HDL-64E 235 | private_nh.param("frame_id", frame_id_, "velodyne"); 236 | private_nh.param("print_fps", print_fps_, false); 237 | private_nh.param("z_axis_min", z_axis_min_, -0.5); 238 | private_nh.param("z_axis_max", z_axis_max_, 5.0); 239 | private_nh.param("cluster_size_min", cluster_size_min_, 5); 240 | private_nh.param("cluster_size_max", cluster_size_max_, 700000); 241 | 242 | // Divide the point cloud into nested circular regions centred at the sensor. 243 | // For more details, see our IROS-17 paper "Online learning for human classification in 3D LiDAR-based tracking" 244 | if(sensor_model_ == "VLP-16") { 245 | regions_[0] = 2; regions_[1] = 3; regions_[2] = 3; regions_[3] = 3; regions_[4] = 3; 246 | regions_[5] = 3; regions_[6] = 3; regions_[7] = 2; regions_[8] = 3; regions_[9] = 3; 247 | regions_[10]= 3; regions_[11]= 3; regions_[12]= 3; regions_[13]= 3; 248 | } else if (sensor_model_ == "HDL-32E") { 249 | regions_[0] = 4; regions_[1] = 5; regions_[2] = 4; regions_[3] = 5; regions_[4] = 4; 250 | regions_[5] = 5; regions_[6] = 5; regions_[7] = 4; regions_[8] = 5; regions_[9] = 4; 251 | regions_[10]= 5; regions_[11]= 5; regions_[12]= 4; regions_[13]= 5; 252 | } else if (sensor_model_ == "HDL-64E") { 253 | regions_[0] = 14; regions_[1] = 14; regions_[2] = 14; regions_[3] = 15; regions_[4] = 14; 254 | } 255 | 256 | ros::spin(); 257 | return 0; 258 | } 259 | --------------------------------------------------------------------------------