├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── doc ├── Bogoslavskyi, Stachniss - 2017 - Efficient Online Segmentation for Sparse 3D Laser Scans.pdf ├── Fast-Segmentation-of-3D-Point-Clouds-A-Paradigm-on-LiDAR-Data-for-Autonomous-Vehicle-Applications.pdf ├── cluster_3d.gif ├── cluster_algo.png ├── clusters.gif ├── depth_image.gif └── ground.gif ├── include ├── dataIO.h ├── depth_cluster.cpp ├── depth_cluster.h └── pointinfo.h ├── launch └── depth_cluster_ros.launch ├── package.xml ├── rviz └── simple_rviz.rviz └── src └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-debug 2 | .idea 3 | 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(depth_cluster_ros) 3 | ## Compile as C++11, supported in ROS Kinetic and newer 4 | add_compile_options(-std=c++11) 5 | 6 | ########### 7 | ## Build ## 8 | ########### 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | std_msgs 13 | tf 14 | ) 15 | catkin_package( 16 | # INCLUDE_DIRS include 17 | # LIBRARIES simple_slam 18 | # CATKIN_DEPENDS roscpp rospy std_msgs 19 | # DEPENDS system_lib 20 | ) 21 | 22 | find_package(PCL REQUIRED) 23 | set(Eigen_INCLUDE_DIRS "usr/include/eigen3") 24 | include_directories( 25 | ${PCL_INCLUDE_DIRS} 26 | ${Eigen_INCLUDE_DIRS} 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | ## Specify additional locations of header files 31 | ## Your package locations should be listed before other locations 32 | include_directories( 33 | include 34 | ) 35 | 36 | ## Declare a C++ library 37 | # add_library(pose_curve 38 | # include/pose_curve.cpp 39 | # ) 40 | 41 | add_library(depth_cluster include/depth_cluster.cpp) 42 | target_link_libraries(depth_cluster ${PCL_LIBRARIES}) 43 | 44 | add_executable(depth_cluster_node src/main.cpp) 45 | target_link_libraries(depth_cluster_node depth_cluster ${catkin_LIBRARIES}) 46 | 47 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Vincent Cheung 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 | # depth_clustre_ros 2 | 3 | > Created by Alex Su 08/05/2020 4 | 5 | This is a point cloud clustering segmentation algorithm, including the removal of ground point clouds and the segmentation of point clouds. 6 | 7 | 8 | 9 | clusters 10 | 11 | ground 12 | 13 | ## Requirement 14 | 15 | - [PCL](https://github.com/PointCloudLibrary/pcl) 16 | - [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) 17 | 18 | ## Compile 19 | 20 | You can use the following commands to download and compile the package. 21 | 22 | ``` 23 | cd ~/catkin_ws/src 24 | https://github.com/supengufo/depth_clustre_ros 25 | cd .. 26 | caktin_make 27 | ``` 28 | 29 | ## Run 30 | 31 | run the launch file: 32 | 33 | ``` 34 | cd ~/catkin_ws 35 | source devel/setup.bash(or setup.zsh) 36 | roslaunch depth_cluster_ros depth_cluster_ros.launch 37 | ``` 38 | 39 | 40 | 41 | ## Principles 42 | 43 | ### Ground Remove 44 | 45 | ![](http://39.107.30.202:8080/s/Z6by7XmHddM6ZC4/preview) 46 | 47 | ### Cloud Segmentation 48 | 49 | ground 50 | 51 | 52 | 53 | First, convert all the 3D points coordinates into 2D depth image; 54 | 55 | ground 56 | 57 | Then,use Breadth-First-Search(BFS) to extraction point cloud clusters. The algorithm flow chart is as follows: 58 | 59 | ground 60 | 61 | 62 | 63 | ## Acknowledgements 64 | 65 | The main idea of point cloud segmentation is based on depth_cluster, in which the filtering threshold condition and neighborhood search are modified; 66 | 67 | The segmentation of ground point clouds is based on Zermas' paper, although principal component analysis is already a very common method to extract ground. 68 | 69 | Thank them for their work! 70 | 71 | 72 | 73 | All these two papers can be found in the `doc` folder. 74 | 75 | ``` 76 | @Article{bogoslavskyi17pfg, 77 | title = {Efficient Online Segmentation for Sparse 3D Laser Scans}, 78 | author = {I. Bogoslavskyi and C. Stachniss}, 79 | journal = {PFG -- Journal of Photogrammetry, Remote Sensing and Geoinformation Science}, 80 | year = {2017}, 81 | pages = {1--12}, 82 | url = {https://link.springer.com/article/10.1007%2Fs41064-016-0003-y}, 83 | } 84 | ``` 85 | 86 | ``` 87 | @inproceedings{Zermas2017Fast, 88 | title={Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications}, 89 | author={Zermas, Dimitris and Izzat, Izzat and Papanikolopoulos, Nikolaos}, 90 | booktitle={IEEE International Conference on Robotics and Automation}, 91 | year={2017}, 92 | } 93 | ``` 94 | 95 | -------------------------------------------------------------------------------- /doc/Bogoslavskyi, Stachniss - 2017 - Efficient Online Segmentation for Sparse 3D Laser Scans.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/Bogoslavskyi, Stachniss - 2017 - Efficient Online Segmentation for Sparse 3D Laser Scans.pdf -------------------------------------------------------------------------------- /doc/Fast-Segmentation-of-3D-Point-Clouds-A-Paradigm-on-LiDAR-Data-for-Autonomous-Vehicle-Applications.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/Fast-Segmentation-of-3D-Point-Clouds-A-Paradigm-on-LiDAR-Data-for-Autonomous-Vehicle-Applications.pdf -------------------------------------------------------------------------------- /doc/cluster_3d.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/cluster_3d.gif -------------------------------------------------------------------------------- /doc/cluster_algo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/cluster_algo.png -------------------------------------------------------------------------------- /doc/clusters.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/clusters.gif -------------------------------------------------------------------------------- /doc/depth_image.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/depth_image.gif -------------------------------------------------------------------------------- /doc/ground.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supengufo/depth_clustering_ros/b18f16e6c5177233c018af848c4ec57bdf06001c/doc/ground.gif -------------------------------------------------------------------------------- /include/dataIO.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by alex on 2020/4/19. 3 | // 4 | 5 | #ifndef PCL_TEST_DATAIO_H 6 | #define PCL_TEST_DATAIO_H 7 | //PCL 8 | #include 9 | #include 10 | //std 11 | #include 12 | using namespace std; 13 | typedef pcl::PointXYZI PointT; 14 | typedef pcl::PointCloud PointCloudT; 15 | struct Point { 16 | float x, y; 17 | int label_; 18 | Point(){} 19 | Point(float x_in, float y_in) { 20 | x = x_in; 21 | y = y_in; 22 | label_=0; 23 | } 24 | }; 25 | 26 | class DataIO { 27 | public: 28 | vector file_names = {"dresser", 29 | "mantel", 30 | "sink", 31 | "piano", 32 | "tent", 33 | "range_hood", 34 | "bookshelf", 35 | "chair", 36 | "night_stand", 37 | "lamp", 38 | "bed", 39 | "toilet", 40 | "sofa", 41 | "guitar", 42 | "vase", 43 | "keyboard", 44 | "stool", 45 | "cup", 46 | "xbox", 47 | "stairs", 48 | "plant", 49 | "bench", 50 | "wardrobe", 51 | "door", 52 | "bowl", 53 | "monitor", 54 | "desk", 55 | "radio", 56 | "table", 57 | "bottle", 58 | "person", 59 | "airplane", 60 | "curtain", 61 | "car", 62 | "bathtub", 63 | "tv_stand", 64 | "cone", 65 | "flower_pot", 66 | "laptop" 67 | }; 68 | 69 | void getPLYData(vector> &all_pointsCloud) { 70 | PointCloudT::Ptr cloud_in(new PointCloudT); // Original point cloud 71 | for (auto &name:file_names) { 72 | string file_path = "/home/alex/code/code_tools/pcl_test/ply_data/" + name + "/train/" + name + "_0001.ply"; 73 | // cout << file_path << endl; 74 | if (pcl::io::loadPLYFile(file_path, *cloud_in) < 0) { 75 | PCL_ERROR ("Error loading cloud %s.\n", &file_path); 76 | return; 77 | } 78 | all_pointsCloud.push_back(*cloud_in); 79 | } 80 | return; 81 | } 82 | // 83 | PointCloudT::Ptr readBinFile(string &input_dir){ 84 | fstream input(input_dir, ios::in | ios::binary); 85 | if(!input.good()){ 86 | cerr << "Could not read file: " << input_dir << endl; 87 | exit(EXIT_FAILURE); 88 | } 89 | input.seekg(0, ios::beg); 90 | PointCloudT::Ptr pointCloud (new PointCloudT); 91 | int i; 92 | for (i=0; input.good() && !input.eof(); i++) { 93 | PointT point; 94 | input.read((char *) &point.x, 3*sizeof(float)); 95 | input.read((char *) &point.intensity, sizeof(float)); 96 | point.intensity*=255; 97 | pointCloud->push_back(point); 98 | } 99 | input.close(); 100 | 101 | cout << "Read bin file from [" << input_dir << "]: "<< i << " points"<< endl; 102 | return pointCloud; 103 | // if (pcl::io::savePCDFileBinary(outfile, *pointCloud) == -1) 104 | // { 105 | // PCL_ERROR("Couldn't write file\n"); 106 | // return false; 107 | // } 108 | } 109 | 110 | //read data from txt 111 | vector> readTXT(const string &root_path,const vector &txt_file_names){ 112 | //1.read all file names path 113 | vector paths; 114 | for (const auto &path:txt_file_names) { 115 | paths.push_back(root_path + path); 116 | } 117 | // read data 118 | vector> res; 119 | for(const auto &file:paths){ 120 | ifstream inFile(file); 121 | string strShow; 122 | vector point_vec; 123 | if (inFile) { 124 | string strLine; 125 | while (getline(inFile, strLine)) // line中不包括每行的换行符 126 | { 127 | auto front_length = strLine.find(','); 128 | int back_length = strLine.length() - front_length; 129 | auto x = strLine.substr(0, front_length); 130 | auto y = strLine.substr(front_length + 1, back_length); 131 | point_vec.emplace_back(atof(x.c_str()),atof(y.c_str())); 132 | } 133 | } 134 | res.push_back(point_vec); 135 | } 136 | return res; 137 | } 138 | }; 139 | 140 | #endif //PCL_TEST_DATAIO_H 141 | -------------------------------------------------------------------------------- /include/depth_cluster.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by alex on 2020/4/12. 3 | // 4 | #include "depth_cluster.h" 5 | using namespace std; 6 | using namespace pcl; 7 | using namespace Eigen; 8 | 9 | DepthCluster::DepthCluster(float vertcal_resolution, float horizontal_resolution, int lidar_lines, int cluster_size) 10 | : vertcal_resolution_(vertcal_resolution), horizontal_resolution_(horizontal_resolution), lidar_lines_(lidar_lines), cluster_size_(cluster_size) { 11 | initParams(); 12 | } 13 | 14 | void DepthCluster::initParams() { 15 | //ground move algorithm parameters 16 | sorted_Pointcloud_.reset(new pcl::PointCloud()); 17 | 18 | //cluster algorithm parameters init 19 | image_cols_ = int(360.0/horizontal_resolution_); 20 | image_rows_ = lidar_lines_; 21 | // vertical_angle_range_ = 30; 22 | vertical_angle_min_ = -15; 23 | vertical_angle_max_ = 15; 24 | } 25 | 26 | void DepthCluster::setInputCloud(pcl::PointCloud::Ptr &msg) { 27 | cout << "---------------- Algorithm Start ----------------" << endl; 28 | vector> label_image(image_rows_, vector(image_cols_, -1)); 29 | //move ground point cloud 30 | exactGroundPoints(msg, label_image); 31 | /* maybe can use OpenMP */ 32 | vector depth_vec; 33 | auto depth_image = generateDepthImage(msg); 34 | labelComponents(depth_image, label_image, msg); 35 | } 36 | 37 | vector DepthCluster::exactGroundPoints(pcl::PointCloud::Ptr &msg, vector> &label_image) { 38 | clock_t time_start = clock(); 39 | pcl::PointCloud::Ptr cloud_copy(new pcl::PointCloud);//shared ptr, dont need to be deleted by self 40 | pcl::copyPointCloud(*msg, *cloud_copy); // 41 | vector ground_indices; 42 | pcl::PointCloud::Ptr initial_ground_points(new pcl::PointCloud);// 43 | extractInitialGroundPoint(cloud_copy, initial_ground_points); 44 | Eigen::Vector4f ground_params = estimatePlaneParams(initial_ground_points); 45 | ground_indices = exactGroundCloudIndices(msg, label_image, ground_params); 46 | clock_t time_end = clock(); 47 | auto time_used = 1000.0*double(time_end - time_start)/(double) CLOCKS_PER_SEC; 48 | cout << "Label Ground used time: " << time_used << " ms" << endl; 49 | return ground_indices; 50 | } 51 | 52 | vector DepthCluster::exactGroundCloudIndices(pcl::PointCloud::Ptr &msg, vector> &label_image, Eigen::Vector4f &ground_params) { 53 | //1. 54 | int clouds_size = msg->points.size(); 55 | MatrixXf points(clouds_size, 3); 56 | int j = 0; 57 | for (int i = 0; i < clouds_size; ++i) { 58 | points.row(j++) << msg->points[i].x, msg->points[i].y, msg->points[i].z; 59 | } 60 | MatrixXf normal = ground_params.head<3>(); 61 | VectorXf d_matrix = points*normal;// 62 | vector ground_points_indeices; 63 | for (int i = 0; i < j; ++i) { 64 | if (d_matrix[i] < -(0.2 - ground_params(3, 0))) { 65 | int row_index, col_index; 66 | if (!calculateCoordinate(msg->points[i], row_index, col_index)) { 67 | continue; 68 | } 69 | label_image[row_index][col_index] = 0; //0 means ground 70 | ground_points_indeices.push_back(i); 71 | } 72 | } 73 | ground_points_indices_ = ground_points_indeices; 74 | return ground_points_indeices; 75 | } 76 | 77 | void DepthCluster::extractInitialGroundPoint(pcl::PointCloud::Ptr &cloud, pcl::PointCloud::Ptr &initial_ground_points) { 78 | sort(cloud->points.begin(), cloud->points.end(), [](const pcl::PointXYZI &a, const pcl::PointXYZI &b) { return a.z < b.z; }); 79 | auto it = cloud->points.begin(); 80 | 81 | for (auto &point : cloud->points) { 82 | if (point.z < -1.5*sensor_height_) { 83 | it++; 84 | } 85 | else { 86 | break; 87 | } 88 | } 89 | 90 | cloud->points.erase(cloud->points.begin(), it); //把低于安装高度1.5倍的点全都删掉 91 | int initial_ground_points_counts = 50; 92 | if (initial_ground_points_counts > cloud->points.size()) { 93 | cout << "Too few points" << endl; 94 | return; 95 | } 96 | 97 | float ground_height = 0; 98 | for (int i = 0; i < initial_ground_points_counts; ++i) { 99 | ground_height += cloud->points[i].z; 100 | } 101 | 102 | ground_height /= initial_ground_points_counts; 103 | initial_ground_points->clear(); 104 | for (auto point: cloud->points) { 105 | if (point.z < ground_height + 0.2) { 106 | initial_ground_points->points.push_back(point); 107 | } 108 | } 109 | } 110 | 111 | Eigen::Vector4f DepthCluster::estimatePlaneParams(pcl::PointCloud::Ptr &initial_ground_points) { 112 | Eigen::Matrix3f cov; 113 | Eigen::Vector4f pc_mean; 114 | pcl::computeMeanAndCovarianceMatrix(*initial_ground_points, cov, pc_mean); 115 | JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); 116 | MatrixXf normal = (svd.matrixU().col(2));//第三列为地面平面的法向量。 3*1 117 | Eigen::Vector3f seeds_mean = pc_mean.head<3>();//前三个数 也就是xyz的均值 取地面点的均值 118 | float d = -(normal.transpose()*seeds_mean)(0, 0);//d!=0 119 | Eigen::Vector4f plane_parameters; 120 | plane_parameters(3, 0) = d; 121 | plane_parameters.block<3, 1>(0, 0) = normal.block<3, 1>(0, 0);// 122 | return plane_parameters; 123 | } 124 | 125 | vector> DepthCluster::generateDepthImage(pcl::PointCloud::Ptr cloud_fused_ptr) { 126 | vector> depth_image(image_rows_, vector(image_cols_, PointInfo(-1, -1))); 127 | clock_t time_start = clock(); 128 | auto cloud_size = cloud_fused_ptr->points.size(); 129 | 130 | for (int i = 0; i < cloud_size; i++) { 131 | const pcl::PointXYZI &point = cloud_fused_ptr->points[i]; 132 | if (isnan(point.x) || isnan(point.y) || isnan(point.z)) { 133 | continue; 134 | } 135 | int row_index, col_index; 136 | if (!calculateCoordinate(point, row_index, col_index)) { 137 | continue; 138 | } 139 | float depth = sqrt(point.x*point.x + point.y*point.y + point.z*point.z);//[2,104] 140 | if (depth < 0.15 || depth > 110) { //delete too long or too short points 141 | continue; 142 | } 143 | 144 | depth_image[row_index][col_index].depth_ = depth; 145 | depth_image[row_index][col_index].index_ = i; 146 | } 147 | 148 | clock_t time_end = clock(); 149 | auto time_used = 1000.0*double(time_end - time_start)/(double) CLOCKS_PER_SEC; 150 | cout << "Depth Image used time: " << time_used << " ms" << endl; 151 | return depth_image; 152 | } 153 | 154 | void DepthCluster::labelComponents(const vector> &depth_image, vector> &label_image, const pcl::PointCloud::Ptr &cloud_msg) { 155 | // 四邻域 156 | // vector> neighbor = {{1, 0}, 157 | // {-1, 0}, 158 | // {0, 1}, 159 | // {0, -1}}; 160 | // 八邻域 161 | // vector> neighbor = {{1, 0}, 162 | // {1, 1}, 163 | // {1, -1}, 164 | // {-1, 0}, 165 | // {-1, -1}, 166 | // {-1, 1}, 167 | // {0, 1}, 168 | // {0, -1}}; 169 | 170 | vector> neighbor = {{1, 0}, 171 | {1, 1}, 172 | {1, -1}, 173 | {-1, 0}, 174 | {-1, -1}, 175 | {-1, 1}, 176 | {0, 1}, 177 | {0, -1}}; 178 | 179 | int label_val = 1; 180 | int rows = image_rows_; 181 | int cols = image_cols_; 182 | const double eps = 1.0e-6;// 183 | 184 | vector> clusters_indices_vec; 185 | clock_t time_start = clock(); 186 | for (int row = 0; row < rows; row++) { 187 | for (int col = 0; col < cols; col++) { 188 | if (label_image[row][col] == -1 && depth_image[row][col].depth_ > 0) { //has been labeled or ground points 189 | queue> q; 190 | q.push(make_pair(row, col)); 191 | label_image[row][col] = label_val; 192 | 193 | vector cluster_indices_vec; 194 | cluster_indices_vec.push_back(depth_image[row][col].index_); 195 | 196 | while (!q.empty()) { 197 | auto target_point = q.front(); 198 | q.pop(); 199 | for (const auto &neigh:neighbor) { 200 | int x = target_point.first + neigh.first; 201 | int y = target_point.second + neigh.second; 202 | pair neigh_point = make_pair(x, y); 203 | 204 | if (warpPoint(neigh_point) && judgmentCondition(depth_image, target_point, neigh_point) && 205 | label_image[neigh_point.first][neigh_point.second] == -1 && depth_image[neigh_point.first][neigh_point.second].depth_ > 0) { // valid point 206 | q.push(neigh_point); 207 | label_image[neigh_point.first][neigh_point.second] = label_val; 208 | cluster_indices_vec.push_back(depth_image[neigh_point.first][neigh_point.second].index_); 209 | } 210 | } 211 | } 212 | label_val++; 213 | if (cluster_indices_vec.size() > cluster_size_) { 214 | clusters_indices_vec.push_back(cluster_indices_vec); 215 | } 216 | } 217 | } 218 | } 219 | 220 | clock_t time_end = clock(); 221 | auto time_used = 1000.0*double(time_end - time_start)/(double) CLOCKS_PER_SEC; 222 | cout << "Cluster Algorithm used time: " << time_used << " ms" << endl; 223 | 224 | auto totle_clusters_size = clusters_indices_vec.size(); 225 | clusters_indices_vec_ = clusters_indices_vec; 226 | // cout << "Total cluster nums: " << totle_clusters_size << endl; 227 | 228 | } 229 | 230 | bool DepthCluster::judgmentCondition(const vector> &depth_image, const pair &target_point, const pair &neigh_point) { 231 | float distance_sum = fabs(depth_image[target_point.first][target_point.second].depth_ - depth_image[neigh_point.first][neigh_point.second].depth_); 232 | return distance_sum < 0.4; 233 | } 234 | 235 | vector> DepthCluster::getClustersIndex() { 236 | return clusters_indices_vec_; 237 | } 238 | 239 | vector DepthCluster::getGroundCloudIndices() { 240 | return ground_points_indices_; 241 | } 242 | vector DepthCluster::getMergedClustersIndex() { 243 | vector res_resturn; 244 | for(auto i:clusters_indices_vec_){ 245 | res_resturn.insert(res_resturn.end(),i.begin(),i.end()); 246 | } 247 | return res_resturn; 248 | } 249 | bool DepthCluster::warpPoint(pair &pt) { 250 | if (pt.first < 0 || pt.first >= image_rows_) 251 | return false; 252 | if (pt.second < 0) 253 | pt.second += image_cols_; 254 | if (pt.second >= image_cols_) 255 | pt.second -= image_cols_; 256 | return true; 257 | } 258 | 259 | void DepthCluster::paramsReset() { 260 | sorted_Pointcloud_->clear(); 261 | } 262 | 263 | bool DepthCluster::calculateCoordinate(const pcl::PointXYZI &point, int &row, int &col) { 264 | //1. calculate angle 265 | float vertical_angle = atan2(point.z, sqrt(point.x*point.x + point.y*point.y))*180/M_PI;//范围是[-15,15] 266 | float horizon_angle = atan2(point.x, point.y)*180/M_PI;//[-180,180] 267 | //2. calculate current points row col coordinate 268 | int row_index = int((vertical_angle - vertical_angle_min_)/vertcal_resolution_);//0-15 269 | 270 | if (row_index < 0) { 271 | row_index = 0; 272 | } 273 | else if (row_index > vertical_angle_max_) { 274 | row_index = vertical_angle_max_; 275 | } 276 | 277 | int col_index = -round((horizon_angle)/horizontal_resolution_) + image_cols_/2.0; 278 | if (col_index >= image_cols_) 279 | col_index -= image_cols_; 280 | if (col_index < 0 || col_index >= image_cols_) { 281 | return false; 282 | } 283 | row = row_index; 284 | col = col_index; 285 | return true; 286 | } 287 | 288 | -------------------------------------------------------------------------------- /include/depth_cluster.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by alex on 2020/4/12. 3 | // 4 | 5 | #ifndef SRC_DEPTH_CLUSTER_H 6 | #define SRC_DEPTH_CLUSTER_H 7 | //std 8 | #include 9 | #include 10 | #include 11 | #include 12 | //ROS 13 | //PCL 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | //Eigen 21 | #include 22 | //using Eigen::MatrixXf; 23 | //using Eigen::JacobiSVD; 24 | //using Eigen::VectorXf; 25 | 26 | //self 27 | #include "pointinfo.h" 28 | 29 | using namespace std; 30 | using namespace pcl; 31 | 32 | 33 | class DepthCluster { 34 | public: 35 | /** 36 | * 37 | * @param vertcal_resolution sensor vertical resolution 38 | * @param horizontal_resolution sensor horizontal resolution 39 | * @param lidar_lines lidar lines,usually 16,32,64,128 40 | * @param cluster_size Point clouds larger than this number are considered as a cluster 41 | */ 42 | DepthCluster(float vertcal_resolution, float horizontal_resolution, int lidar_lines, int cluster_size); 43 | /** 44 | * init some parameters,depend on different sensors 45 | */ 46 | void initParams(); 47 | /** 48 | * 49 | * @param msg input pointcloud msg 50 | */ 51 | void setInputCloud(pcl::PointCloud::Ptr &msg); 52 | /** 53 | * 54 | * @param msg 55 | * @param label_image 56 | * @return 57 | */ 58 | vector exactGroundPoints(pcl::PointCloud::Ptr &msg, vector> &label_image); 59 | 60 | vector exactGroundCloudIndices(pcl::PointCloud::Ptr &msg, vector> &label_image, Eigen::Vector4f &ground_params); 61 | 62 | Eigen::Vector4f estimatePlaneParams(pcl::PointCloud::Ptr &initial_ground_points); 63 | void extractInitialGroundPoint(pcl::PointCloud::Ptr &cloud, pcl::PointCloud::Ptr &initial_ground_points); 64 | /** 65 | * 66 | * @param depth_image 67 | * @param label_image 68 | * @param cloud_msg 69 | */ 70 | void labelComponents(const vector> &depth_image, vector> &label_image, const pcl::PointCloud::Ptr &cloud_msg); 71 | /** 72 | * convert 3D points seach to 2D,instead of using kd-tree and more faster 73 | * @param cloud_fused_ptr input cloud ptr 74 | * @return 2D vector image stored every point depth and index 75 | */ 76 | vector> generateDepthImage(pcl::PointCloud::Ptr cloud_fused_ptr); 77 | /** 78 | * 79 | * @param depth_image 80 | * @param target_point 81 | * @param neigh_point 82 | * @return if the neighbour point belong to the same cluster 83 | */ 84 | bool judgmentCondition(const vector> &depth_image, const pair &target_point, const pair &neigh_point); 85 | bool calculateCoordinate(const pcl::PointXYZI &point, int &row, int &col); 86 | /** 87 | * 88 | * @param pt 89 | * @return make sure the searched point belong to the search region 90 | */ 91 | bool warpPoint(pair &pt); 92 | 93 | /** 94 | * 95 | * @return 2D clusters index in pointcloud 96 | */ 97 | vector> getClustersIndex(); 98 | 99 | vector getMergedClustersIndex(); 100 | /** 101 | * 102 | * @return all the ground points indices 103 | */ 104 | vector getGroundCloudIndices(); 105 | 106 | /** 107 | * reset some parameters 108 | */ 109 | void paramsReset(); 110 | private: 111 | /** 112 | * ground move parameters 113 | */ 114 | pcl::PointCloud::Ptr sorted_Pointcloud_; 115 | vector ground_points_indices_; 116 | float sensor_height_; 117 | /** 118 | * cluster parameters 119 | */ 120 | //lidar params need to initialize in class construction function 121 | float vertcal_resolution_, horizontal_resolution_; 122 | int lidar_lines_; 123 | int cluster_size_; 124 | //needed to be initialize in initial function 125 | int image_rows_, image_cols_; 126 | int vertical_angle_min_; 127 | int vertical_angle_max_; 128 | 129 | vector> clusters_indices_vec_; 130 | }; 131 | 132 | 133 | #endif //SRC_DEPTH_CLUSTER_H 134 | -------------------------------------------------------------------------------- /include/pointinfo.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by alex on 2020/5/5. 3 | // 4 | 5 | #ifndef SRC_POINTINFO_H 6 | #define SRC_POINTINFO_H 7 | struct PointInfo { 8 | public: 9 | int index_; 10 | float depth_; 11 | PointInfo(int index,float depth) : index_(index), depth_(depth){}; 12 | }; 13 | #endif //SRC_POINTINFO_H 14 | -------------------------------------------------------------------------------- /launch/depth_cluster_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | depth_cluster_ros 4 | 0.0.0 5 | The simple_slam package 6 | 7 | 8 | 9 | 10 | alex 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /rviz/simple_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | Splitter Ratio: 0.5 11 | Tree Height: 825 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Ground 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Axes Length: 1 56 | Axes Radius: 0.100000001 57 | Class: rviz/Pose 58 | Color: 255; 25; 0 59 | Enabled: false 60 | Head Length: 0.300000012 61 | Head Radius: 0.100000001 62 | Name: Pose 63 | Shaft Length: 10 64 | Shaft Radius: 0.400000006 65 | Shape: Axes 66 | Topic: /imu_pose 67 | Unreliable: false 68 | Value: false 69 | - Class: rviz/Axes 70 | Enabled: false 71 | Length: 1 72 | Name: Axes 73 | Radius: 0.100000001 74 | Reference Frame: velodyne 75 | Value: false 76 | - Class: rviz/Axes 77 | Enabled: false 78 | Length: 0.5 79 | Name: Axes 80 | Radius: 0.100000001 81 | Reference Frame: odom 82 | Value: false 83 | - Alpha: 1 84 | Autocompute Intensity Bounds: true 85 | Autocompute Value Bounds: 86 | Max Value: 10 87 | Min Value: -10 88 | Value: true 89 | Axis: Z 90 | Channel Name: intensity 91 | Class: rviz/PointCloud2 92 | Color: 255; 255; 255 93 | Color Transformer: Intensity 94 | Decay Time: 0 95 | Enabled: false 96 | Invert Rainbow: false 97 | Max Color: 255; 255; 255 98 | Max Intensity: 254 99 | Min Color: 0; 0; 0 100 | Min Intensity: 25 101 | Name: local_map 102 | Position Transformer: XYZ 103 | Queue Size: 10 104 | Selectable: true 105 | Size (Pixels): 2 106 | Size (m): 0.100000001 107 | Style: Points 108 | Topic: /local_map 109 | Unreliable: false 110 | Use Fixed Frame: true 111 | Use rainbow: true 112 | Value: false 113 | - Angle Tolerance: 0.100000001 114 | Class: rviz/Odometry 115 | Covariance: 116 | Orientation: 117 | Alpha: 0.5 118 | Color: 255; 255; 127 119 | Color Style: Unique 120 | Frame: Local 121 | Offset: 1 122 | Scale: 1 123 | Value: true 124 | Position: 125 | Alpha: 0.300000012 126 | Color: 204; 51; 204 127 | Scale: 1 128 | Value: true 129 | Value: true 130 | Enabled: false 131 | Keep: 100 132 | Name: Odometry 133 | Position Tolerance: 0.100000001 134 | Shape: 135 | Alpha: 1 136 | Axes Length: 1 137 | Axes Radius: 0.100000001 138 | Color: 255; 25; 0 139 | Head Length: 0.300000012 140 | Head Radius: 0.100000001 141 | Shaft Length: 1 142 | Shaft Radius: 0.0500000007 143 | Value: Arrow 144 | Topic: /lidar_odom 145 | Unreliable: false 146 | Value: false 147 | - Alpha: 1 148 | Autocompute Intensity Bounds: true 149 | Autocompute Value Bounds: 150 | Max Value: 10 151 | Min Value: -10 152 | Value: true 153 | Axis: Z 154 | Channel Name: intensity 155 | Class: rviz/PointCloud2 156 | Color: 255; 255; 255 157 | Color Transformer: Intensity 158 | Decay Time: 0 159 | Enabled: true 160 | Invert Rainbow: false 161 | Max Color: 255; 255; 255 162 | Max Intensity: 100 163 | Min Color: 0; 0; 0 164 | Min Intensity: 100 165 | Name: Ground 166 | Position Transformer: XYZ 167 | Queue Size: 10 168 | Selectable: true 169 | Size (Pixels): 2 170 | Size (m): 0.00999999978 171 | Style: Points 172 | Topic: /ground_result 173 | Unreliable: false 174 | Use Fixed Frame: true 175 | Use rainbow: true 176 | Value: true 177 | - Alpha: 1 178 | Autocompute Intensity Bounds: true 179 | Autocompute Value Bounds: 180 | Max Value: 10 181 | Min Value: -10 182 | Value: true 183 | Axis: Z 184 | Channel Name: intensity 185 | Class: rviz/PointCloud2 186 | Color: 255; 255; 255 187 | Color Transformer: Intensity 188 | Decay Time: 0 189 | Enabled: false 190 | Invert Rainbow: false 191 | Max Color: 255; 255; 255 192 | Max Intensity: 209 193 | Min Color: 0; 0; 0 194 | Min Intensity: 0 195 | Name: NoGround 196 | Position Transformer: XYZ 197 | Queue Size: 10 198 | Selectable: true 199 | Size (Pixels): 2 200 | Size (m): 0.00999999978 201 | Style: Points 202 | Topic: /points_no_ground 203 | Unreliable: false 204 | Use Fixed Frame: true 205 | Use rainbow: true 206 | Value: false 207 | - Alpha: 1 208 | Autocompute Intensity Bounds: true 209 | Autocompute Value Bounds: 210 | Max Value: 10 211 | Min Value: -10 212 | Value: true 213 | Axis: Z 214 | Channel Name: intensity 215 | Class: rviz/PointCloud2 216 | Color: 255; 255; 255 217 | Color Transformer: Intensity 218 | Decay Time: 0 219 | Enabled: true 220 | Invert Rainbow: false 221 | Max Color: 255; 255; 255 222 | Max Intensity: 254 223 | Min Color: 0; 0; 0 224 | Min Intensity: 2 225 | Name: depthcluster_result 226 | Position Transformer: XYZ 227 | Queue Size: 10 228 | Selectable: true 229 | Size (Pixels): 3 230 | Size (m): 0.00999999978 231 | Style: Points 232 | Topic: /cluster_result 233 | Unreliable: false 234 | Use Fixed Frame: true 235 | Use rainbow: true 236 | Value: true 237 | - Alpha: 1 238 | Autocompute Intensity Bounds: true 239 | Autocompute Value Bounds: 240 | Max Value: 10 241 | Min Value: -10 242 | Value: true 243 | Axis: Z 244 | Channel Name: intensity 245 | Class: rviz/PointCloud2 246 | Color: 255; 255; 255 247 | Color Transformer: Intensity 248 | Decay Time: 0 249 | Enabled: false 250 | Invert Rainbow: false 251 | Max Color: 255; 255; 255 252 | Max Intensity: 227 253 | Min Color: 0; 0; 0 254 | Min Intensity: 0 255 | Name: PointCloud2 256 | Position Transformer: XYZ 257 | Queue Size: 10 258 | Selectable: true 259 | Size (Pixels): 3 260 | Size (m): 0.00999999978 261 | Style: Flat Squares 262 | Topic: /points_ground 263 | Unreliable: false 264 | Use Fixed Frame: true 265 | Use rainbow: true 266 | Value: false 267 | Enabled: true 268 | Global Options: 269 | Background Color: 255; 255; 255 270 | Default Light: true 271 | Fixed Frame: velodyne 272 | Frame Rate: 30 273 | Name: root 274 | Tools: 275 | - Class: rviz/Interact 276 | Hide Inactive Objects: true 277 | - Class: rviz/MoveCamera 278 | - Class: rviz/Select 279 | - Class: rviz/FocusCamera 280 | - Class: rviz/Measure 281 | - Class: rviz/SetInitialPose 282 | Topic: /initialpose 283 | - Class: rviz/SetGoal 284 | Topic: /move_base_simple/goal 285 | - Class: rviz/PublishPoint 286 | Single click: true 287 | Topic: /clicked_point 288 | Value: true 289 | Views: 290 | Current: 291 | Class: rviz/ThirdPersonFollower 292 | Distance: 40.6829605 293 | Enable Stereo Rendering: 294 | Stereo Eye Separation: 0.0599999987 295 | Stereo Focal Distance: 1 296 | Swap Stereo Eyes: false 297 | Value: false 298 | Focal Point: 299 | X: -12.6666069 300 | Y: 2.2622571 301 | Z: -1.31581291e-05 302 | Focal Shape Fixed Size: true 303 | Focal Shape Size: 0.0500000007 304 | Invert Z Axis: false 305 | Name: Current View 306 | Near Clip Distance: 0.00999999978 307 | Pitch: 1.00020301 308 | Target Frame: 309 | Value: ThirdPersonFollower (rviz) 310 | Yaw: 3.05538797 311 | Saved: ~ 312 | Window Geometry: 313 | Displays: 314 | collapsed: false 315 | Height: 1028 316 | Hide Left Dock: false 317 | Hide Right Dock: false 318 | QMainWindow State: 000000ff00000000fd0000000400000000000001aa0000037afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001f4000001ae0000000000000000000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000047a0000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 319 | Selection: 320 | collapsed: false 321 | Time: 322 | collapsed: false 323 | Tool Properties: 324 | collapsed: false 325 | Views: 326 | collapsed: false 327 | Width: 1855 328 | X: 65 329 | Y: 24 330 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by alex on 2020/4/12. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | DepthCluster depthCluster(2, 0.2, 16, 20); 10 | ros::Publisher point_pub; 11 | ros::Publisher ground_pub; 12 | 13 | void callback(const sensor_msgs::PointCloud2ConstPtr &cloud_ptr) { 14 | pcl::PointCloud::Ptr laserCloudIn; 15 | laserCloudIn.reset(new pcl::PointCloud()); 16 | pcl::fromROSMsg(*cloud_ptr, *laserCloudIn); 17 | 18 | depthCluster.setInputCloud(laserCloudIn); 19 | vector> clustersIndex = depthCluster.getClustersIndex(); 20 | for (auto cluster_vec:clustersIndex) { 21 | int intensity = rand()%255; 22 | for (int i = 0; i < cluster_vec.size(); ++i) { 23 | laserCloudIn->points[cluster_vec[i]].intensity = intensity; 24 | } 25 | } 26 | 27 | auto ground_index = depthCluster.getGroundCloudIndices(); 28 | int intensity = 100; 29 | for (int j : ground_index) { 30 | laserCloudIn->points[j].intensity = intensity; 31 | } 32 | 33 | pcl::PointCloud::Ptr ground_points(new pcl::PointCloud()); 34 | pcl::copyPointCloud(*laserCloudIn, ground_index, *ground_points); 35 | 36 | sensor_msgs::PointCloud2 laserCloudTemp; 37 | pcl::toROSMsg(*ground_points, laserCloudTemp); 38 | laserCloudTemp.header.stamp = cloud_ptr->header.stamp; 39 | laserCloudTemp.header.frame_id = cloud_ptr->header.frame_id; 40 | ground_pub.publish(laserCloudTemp); 41 | 42 | auto cluster_indices = depthCluster.getMergedClustersIndex(); 43 | pcl::PointCloud::Ptr cluster_points(new pcl::PointCloud()); 44 | pcl::copyPointCloud(*laserCloudIn, cluster_indices, *cluster_points); 45 | pcl::toROSMsg(*cluster_points, laserCloudTemp); 46 | laserCloudTemp.header.stamp = cloud_ptr->header.stamp; 47 | laserCloudTemp.header.frame_id = cloud_ptr->header.frame_id; 48 | point_pub.publish(laserCloudTemp); 49 | 50 | }; 51 | int main(int argc, char **argv) { 52 | ros::init(argc, argv, "depth_cluster"); 53 | ros::NodeHandle nh; 54 | ros::NodeHandle nh_local("~"); 55 | ros::Subscriber point_sub = nh.subscribe("velodyne_points", 1, callback); 56 | point_pub = nh.advertise("cluster_result", 1); 57 | ground_pub = nh.advertise("ground_result", 1); 58 | ros::spin(); 59 | } --------------------------------------------------------------------------------