├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── examples └── example.bag ├── nodes ├── clustering │ └── scanlinerun.cpp └── ground_filter │ └── groundplanfit.cpp ├── package.xml ├── pic ├── 1.png └── 2.png └── readme.md /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(points_preprocessor_usi) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | pcl_ros 8 | pcl_conversions 9 | velodyne_pointcloud 10 | ) 11 | 12 | catkin_package(CATKIN_DEPENDS 13 | sensor_msgs 14 | velodyne_pointcloud 15 | ) 16 | 17 | ########### 18 | ## Build ## 19 | ########### 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | find_package(Eigen3 REQUIRED) 26 | 27 | #SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") 28 | SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") 29 | 30 | link_directories(${PCL_LIBRARY_DIRS}) 31 | 32 | #Ground Plane Fitter 33 | find_package(PCL 1.8 REQUIRED) 34 | add_definitions(${PCL_DEFINITIONS}) 35 | 36 | add_executable(groundplanfit 37 | nodes/ground_filter/groundplanfit.cpp 38 | ) 39 | 40 | target_include_directories(groundplanfit PRIVATE 41 | ${PCL_INCLUDE_DIRS} 42 | ) 43 | 44 | target_link_libraries(groundplanfit 45 | ${catkin_LIBRARIES} 46 | ${PCL_LIBRARIES} 47 | ) 48 | 49 | #Scan Line Run 50 | add_executable(scanlinerun 51 | nodes/clustering/scanlinerun.cpp 52 | ) 53 | 54 | target_include_directories(scanlinerun PRIVATE 55 | ${PCL_INCLUDE_DIRS} 56 | ) 57 | 58 | target_link_libraries(scanlinerun 59 | ${catkin_LIBRARIES} 60 | ${PCL_LIBRARIES} 61 | ) 62 | 63 | 64 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /examples/example.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VincentCheungM/Run_based_segmentation/50f4f1bd5f4141d95968c9adb59cb55c10f1db71/examples/example.bag -------------------------------------------------------------------------------- /nodes/clustering/scanlinerun.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | @file scanlinerun.cpp 3 | @brief ROS Node for scan line run 4 | 5 | This is a ROS node to perform scan line run clustring. 6 | Implementation accoriding to 7 | 8 | In this case, it's assumed that the x,y axis points at sea-level, 9 | and z-axis points up. The sort of height is based on the Z-axis value. 10 | 11 | @author Vincent Cheung(VincentCheungm) 12 | @bug . 13 | */ 14 | #define IO 15 | #include 16 | #include 17 | // For disable PCL complile lib, to use PointXYZIR, and customized pointcloud 18 | #define PCL_NO_PRECOMPILE 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | #ifdef MARKER 30 | #include 31 | #include 32 | #endif 33 | 34 | #include 35 | #include 36 | 37 | #ifdef IO 38 | #include 39 | #include 40 | #endif 41 | 42 | using namespace std; 43 | 44 | //Customed Point Struct for holding clustered points 45 | namespace scan_line_run 46 | { 47 | /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */ 48 | struct PointXYZIRL 49 | { 50 | PCL_ADD_POINT4D; // quad-word XYZ 51 | float intensity; ///< laser intensity reading 52 | uint16_t ring; ///< laser ring number 53 | uint16_t label; ///< point label 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 55 | } EIGEN_ALIGN16; 56 | 57 | }; // namespace scan_line_run 58 | 59 | #define SLRPointXYZIRL scan_line_run::PointXYZIRL 60 | #define VPoint velodyne_pointcloud::PointXYZIR 61 | #define RUN pcl::PointCloud 62 | // Register custom point struct according to PCL 63 | POINT_CLOUD_REGISTER_POINT_STRUCT(scan_line_run::PointXYZIRL, 64 | (float, x, x) 65 | (float, y, y) 66 | (float, z, z) 67 | (float, intensity, intensity) 68 | (uint16_t, ring, ring) 69 | (uint16_t, label, label)) 70 | 71 | #define dist(a,b) sqrt(((a).x-(b).x)*((a).x-(b).x)+((a).y-(b).y)*((a).y-(b).y)) 72 | 73 | /* 74 | @brief Scan Line Run ROS Node. 75 | @param Velodyne Pointcloud Non Ground topic. 76 | @param Sensor Model. 77 | @param Threshold between points belong to the same run 78 | @param Threshold between runs 79 | 80 | @subscirbe:/all_points 81 | @publish:/slr 82 | */ 83 | class ScanLineRun{ 84 | public: 85 | ScanLineRun(); 86 | private: 87 | ros::NodeHandle node_handle_; 88 | ros::Subscriber points_node_sub_; 89 | ros::Publisher cluster_points_pub_; 90 | ros::Publisher ring_points_pub_; 91 | 92 | std::string point_topic_; 93 | 94 | int sensor_model_;// also means number of sensor scan line. 95 | double th_run_;// thresold of distance of points belong to the same run. 96 | double th_merge_;// threshold of distance of runs to be merged. 97 | 98 | // For organization of points. 99 | std::vector > laser_frame_; 100 | std::vector laser_row_; 101 | 102 | std::vector > runs_;// For holding all runs. 103 | uint16_t max_label_;// max run labels, for disinguish different runs. 104 | std::vector > ng_idx_;// non ground point index. 105 | 106 | // Call back funtion. 107 | void velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); 108 | // For finding runs on a scanline. 109 | void find_runs_(int scanline); 110 | // For update points cluster label after merge action. 111 | void update_labels_(int scanline); 112 | // For merge `current` run to `target` run. 113 | void merge_runs_(uint16_t cur_label, uint16_t target_label); 114 | 115 | /// @deprecated methods for smart index 116 | // Smart idx according to paper, but not useful in my case. 117 | int smart_idx_(int local_idx, int n_i, int n_j, bool inverse); 118 | 119 | #ifdef MARKER 120 | // For display markers only, however, currently the orientation is bad. 121 | ros::Publisher marker_array_pub_; 122 | #endif 123 | // Dummy object to occupy idx 0. 124 | std::forward_list dummy_; 125 | 126 | 127 | #ifdef INTEREST_ONLY 128 | // For showing interest and index point 129 | int interest_line_; 130 | int interest_idx_; 131 | #endif 132 | }; 133 | 134 | /* 135 | @brief Constructor of SLR Node. 136 | @return void 137 | */ 138 | ScanLineRun::ScanLineRun():node_handle_("~"){ 139 | // Init ROS related 140 | ROS_INFO("Inititalizing Scan Line Run Cluster..."); 141 | node_handle_.param("point_topic", point_topic_, "/all_points"); 142 | ROS_INFO("point_topic: %s", point_topic_.c_str()); 143 | 144 | node_handle_.param("sensor_model", sensor_model_, 32); 145 | ROS_INFO("Sensor Model: %d", sensor_model_); 146 | 147 | // Init Ptrs with vectors 148 | for(int i=0;i dummy_vec; 150 | ng_idx_.push_back(dummy_vec); 151 | } 152 | 153 | // Init LiDAR frames with vectors and points 154 | SLRPointXYZIRL p_dummy; 155 | p_dummy.intensity = -1;// Means unoccupy by any points 156 | laser_row_ = std::vector(2251, p_dummy); 157 | laser_frame_ = std::vector >(32, laser_row_); 158 | 159 | // Init runs, idx 0 for interest point, and idx for ground points 160 | max_label_ = 1; 161 | runs_.push_back(dummy_); 162 | runs_.push_back(dummy_); 163 | 164 | node_handle_.param("th_run", th_run_, 0.15); 165 | ROS_INFO("Point-to-Run Threshold: %f", th_run_); 166 | 167 | node_handle_.param("th_merge", th_merge_, 0.5); 168 | ROS_INFO("RUN-to-RUN Distance Threshold: %f", th_merge_); 169 | 170 | // Subscriber to velodyne topic 171 | points_node_sub_ = node_handle_.subscribe(point_topic_, 2, &ScanLineRun::velodyne_callback_, this); 172 | 173 | // Publisher Init 174 | std::string cluster_topic; 175 | node_handle_.param("cluster", cluster_topic, "/slr"); 176 | ROS_INFO("Cluster Output Point Cloud: %s", cluster_topic.c_str()); 177 | cluster_points_pub_ = node_handle_.advertise(cluster_topic, 10); 178 | 179 | #ifdef MARKER 180 | // Publisher for markers. 181 | ROS_INFO("Publishing jsk markers at: %s", "cluster_ma"); 182 | marker_array_pub_ = node_handle_.advertise("cluster_ma", 10); 183 | #endif 184 | 185 | 186 | #ifdef INTEREST_ONLY 187 | // For showing interest point and its neighbour points only. 188 | ROS_INFO("Showing interested points only, starts from line: %d idx: %d", interest_line_, interest_idx_); 189 | node_handle_.param("line", interest_line_, 16); 190 | node_handle_.param("idx", interest_idx_, 500); 191 | #endif 192 | } 193 | 194 | 195 | /* 196 | @brief Read points from the given scan_line. 197 | The distance of two continuous points will be labelled to the same run. 198 | Clusterred points(`Runs`) stored in `runs_[cluster_id]`. 199 | 200 | @param scan_line: The scan line to find runs. 201 | @return void 202 | */ 203 | void ScanLineRun::find_runs_(int scan_line){ 204 | // If there is no non-ground points of current scanline, skip. 205 | int point_size = ng_idx_[scan_line].size(); 206 | if(point_size<=0) 207 | return; 208 | 209 | int non_g_pt_idx = ng_idx_[scan_line][0]; // The first non ground point 210 | int non_g_pt_idx_l = ng_idx_[scan_line][point_size - 1]; // The last non ground point 211 | 212 | /* Iterate all non-ground points, and compute and compare the distance 213 | of each two continous points. At least two non-ground points are needed. 214 | */ 215 | for(int i_idx=0;i_idx1){ 263 | auto &p_0 = laser_frame_[scan_line][non_g_pt_idx]; 264 | auto &p_l = laser_frame_[scan_line][non_g_pt_idx_l]; 265 | 266 | // Skip, if one of the start point or the last point is ground point. 267 | if(p_0.label == 1u || p_l.label == 1u){ 268 | return ; 269 | }else if(dist(p_0,p_l) < th_run_){ 270 | if(p_0.label==0){ 271 | ROS_ERROR("Ring Merge to 0 label"); 272 | } 273 | /// If next point is within threshold, then merge it into the same run. 274 | merge_runs_(p_l.label, p_0.label); 275 | } 276 | }else if(point_size == 1){ 277 | // The only point, make a new run. 278 | auto &p_0 = laser_frame_[scan_line][non_g_pt_idx]; 279 | max_label_ += 1; 280 | runs_.push_back(dummy_); 281 | laser_frame_[scan_line][non_g_pt_idx].label = max_label_; 282 | runs_[p_0.label].insert_after(runs_[p_0.label].cbefore_begin(), &laser_frame_[scan_line][non_g_pt_idx]); 283 | } 284 | 285 | } 286 | 287 | 288 | /* 289 | @brief Update label between points and their smart `neighbour` point 290 | above `scan_line`. 291 | 292 | @param scan_line: The current scan line number. 293 | */ 294 | void ScanLineRun::update_labels_(int scan_line){ 295 | // Iterate each point of this scan line to update the labels. 296 | int point_size_j_idx = ng_idx_[scan_line].size(); 297 | // Current scan line is emtpy, do nothing. 298 | if(point_size_j_idx==0) return; 299 | 300 | // Iterate each point of this scan line to update the labels. 301 | for(int j_idx=0;j_idx=0;l--){ 308 | if(ng_idx_[l].size()==0) 309 | continue; 310 | 311 | // Smart index for the near enough point, after re-organized these points. 312 | int nn_idx = j; 313 | 314 | if(laser_frame_[l][nn_idx].intensity ==-1 || laser_frame_[l][nn_idx].label == 1u){ 315 | continue; 316 | } 317 | 318 | // Nearest neighbour point 319 | auto &p_nn = laser_frame_[l][nn_idx]; 320 | // Skip, if these two points already belong to the same run. 321 | if(p_j.label == p_nn.label){ 322 | continue; 323 | } 324 | double dist_min = dist(p_j, p_nn); 325 | 326 | /* Otherwise, 327 | If the distance of the `nearest point` is within `th_merge_`, 328 | then merge to the smaller run. 329 | */ 330 | if(dist_min < th_merge_){ 331 | uint16_t cur_label = 0, target_label = 0; 332 | 333 | if(p_j.label ==0 || p_nn.label==0){ 334 | ROS_ERROR("p_j.label:%u, p_nn.label:%u", p_j.label, p_nn.label); 335 | } 336 | // Merge to a smaller label cluster 337 | if(p_j.label > p_nn.label){ 338 | cur_label = p_j.label; 339 | target_label = p_nn.label; 340 | }else{ 341 | cur_label = p_nn.label; 342 | target_label = p_j.label; 343 | } 344 | 345 | // Merge these two runs. 346 | merge_runs_(cur_label, target_label); 347 | } 348 | } 349 | } 350 | 351 | } 352 | 353 | /* 354 | @brief Merge current run to the target run. 355 | 356 | @param cur_label: The run label of current run. 357 | @param target_label: The run label of target run. 358 | */ 359 | void ScanLineRun::merge_runs_(uint16_t cur_label, uint16_t target_label){ 360 | if(cur_label ==0||target_label==0){ 361 | ROS_ERROR("Error merging runs cur_label:%u target_label:%u", cur_label, target_label); 362 | } 363 | // First, modify the label of current run. 364 | for(auto &p:runs_[cur_label]){ 365 | p->label = target_label; 366 | } 367 | // Then, insert points of current run into target run. 368 | runs_[target_label].insert_after(runs_[target_label].cbefore_begin(), runs_[cur_label].begin(),runs_[cur_label].end() ); 369 | runs_[cur_label].clear(); 370 | } 371 | 372 | /* 373 | @brief Smart index for nearest neighbour on scanline `i` and scanline `j`. 374 | 375 | @param local_idx: The local index of point on current scanline. 376 | @param n_i: The number of points on scanline `i`. 377 | @param n_j: The number of points on scanline `j`. 378 | @param inverse: If true, means `local_idx` is on the outsider ring `j`. 379 | Otherwise, it's on the insider ring `i`. 380 | 381 | @return The smart index. 382 | */ 383 | [[deprecated("Not useful in my case.")]] 384 | int ScanLineRun::smart_idx_(int local_idx, int n_i, int n_j, bool inverse=false){ 385 | if(inverse==false){ 386 | // In case of zero-divide. 387 | if(n_i == 0 ) return 0; 388 | float rate = (n_j*1.0f)/n_i; 389 | int idx = floor(rate*local_idx); 390 | 391 | // In case of overflow 392 | if(idx>n_j){ 393 | idx = n_j>1?n_j-1:0; 394 | } 395 | return idx; 396 | }else{ 397 | // In case of zero-divide. 398 | if(n_j == 0 ) return 0; 399 | float rate = (n_i*1.0f)/n_j; 400 | int idx = ceil(rate*local_idx); 401 | 402 | // In case of overflow 403 | if(idx>n_i){ 404 | idx = n_i>1?n_i-1:0; 405 | } 406 | return idx; 407 | } 408 | 409 | } 410 | 411 | #ifdef MARKER 412 | /* 413 | @brief For making JSK-Markers with pointclouds. 414 | @param cloud: The clusterred cloud to make a marker. 415 | @param ns: The name string. 416 | @param id: The marker id. 417 | @param r,g,b: The clour of marker. 418 | */ 419 | visualization_msgs::Marker mark_cluster(pcl::PointCloud::Ptr cloud_cluster, 420 | std::string ns ,int id, float r, float g, float b) { 421 | Eigen::Vector4f centroid; 422 | Eigen::Vector4f min; 423 | Eigen::Vector4f max; 424 | 425 | pcl::compute3DCentroid (*cloud_cluster, centroid); 426 | pcl::getMinMax3D (*cloud_cluster, min, max); 427 | 428 | uint32_t shape = visualization_msgs::Marker::CUBE; 429 | visualization_msgs::Marker marker; 430 | marker.header.frame_id = "/velodyne"; 431 | marker.header.stamp = ros::Time::now(); 432 | 433 | marker.ns = ns; 434 | marker.id = id; 435 | marker.type = shape; 436 | marker.action = visualization_msgs::Marker::ADD; 437 | 438 | marker.pose.position.x = centroid[0]; 439 | marker.pose.position.y = centroid[1]; 440 | marker.pose.position.z = centroid[2]; 441 | marker.pose.orientation.x = 0.0; 442 | marker.pose.orientation.y = 0.0; 443 | marker.pose.orientation.z = 0.0; 444 | marker.pose.orientation.w = 1.0; 445 | 446 | marker.scale.x = (max[0]-min[0]); 447 | marker.scale.y = (max[1]-min[1]); 448 | marker.scale.z = (max[2]-min[2]); 449 | 450 | if (marker.scale.x ==0) 451 | marker.scale.x=0.1; 452 | 453 | if (marker.scale.y ==0) 454 | marker.scale.y=0.1; 455 | 456 | if (marker.scale.z ==0) 457 | marker.scale.z=0.1; 458 | 459 | marker.color.r = r; 460 | marker.color.g = g; 461 | marker.color.b = b; 462 | marker.color.a = 0.5; 463 | 464 | marker.lifetime = ros::Duration(0.5); 465 | // marker.lifetime = ros::Duration(0.5); 466 | return marker; 467 | } 468 | #endif 469 | 470 | 471 | #ifdef IO 472 | int tab=0; 473 | #endif 474 | 475 | /* 476 | @brief Velodyne pointcloud callback function, which subscribe `/all_points` 477 | and publish cluster points `slr`. 478 | */ 479 | void ScanLineRun::velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& in_cloud_msg){ 480 | // Msg to pointcloud 481 | pcl::PointCloud laserCloudIn; 482 | pcl::fromROSMsg(*in_cloud_msg, laserCloudIn); 483 | 484 | /// Clear and init. 485 | // Clear runs in the previous scan. 486 | max_label_ = 1; 487 | if(!runs_.empty()){ 488 | runs_.clear(); 489 | runs_.push_back(dummy_);// dummy for index `0` 490 | runs_.push_back(dummy_);// for ground points 491 | } 492 | 493 | // Init laser frame. 494 | SLRPointXYZIRL p_dummy; 495 | p_dummy.intensity = -1; 496 | laser_row_ = std::vector (2251, p_dummy); 497 | laser_frame_ = std::vector >(32, laser_row_); 498 | 499 | // Init non-ground index holder. 500 | for(int i=0;i=0){ 513 | 514 | #ifdef INTEREST_ONLY 515 | // Set the intensity of non-interested points to zero. 516 | point.intensity = 0; 517 | #endif 518 | // Compute and angle. 519 | // @Note: In this case, `x` points right and `y` points forward. 520 | range = sqrt(point.x*point.x + point.y*point.y + point.z*point.z); 521 | if(point.x>=0){ 522 | row = int(563 - asin(point.y/range)/0.00279111); 523 | }else if(point.x<0 && point.y <=0){ 524 | row = int(1688 + asin(point.y/range)/0.00279111); 525 | }else { 526 | row = int(1688 + asin(point.y/range)/0.00279111); 527 | } 528 | 529 | if(row>2250||row<0){ 530 | ROS_ERROR("Row: %d is out of index.", row); 531 | return; 532 | }else{ 533 | laser_frame_[point.ring][row] = point; 534 | } 535 | 536 | if(point.label != 1u){ 537 | ng_idx_[point.ring].push_back(row); 538 | }else{ 539 | runs_[1].insert_after(runs_[1].cbefore_begin(), &point); 540 | } 541 | } 542 | } 543 | 544 | 545 | // Main processing 546 | for(int i=0;i=0;i--){ 556 | if(i==interest_line_){ 557 | auto &point_d = laser_frame_[i][interest_idx_]; 558 | // Empty dummy point 559 | if(point_d.intensity == -1) 560 | continue; 561 | point_d.intensity = i; 562 | // Add key point 563 | runs_[0].insert_after(runs_[0].cbefore_begin(), &laser_frame_[i][interest_idx_]); 564 | }else{ 565 | // Add neighbour point with smart idx 566 | auto &point_d = laser_frame_[i][interest_idx_]; 567 | if(point_d.intensity == -1) 568 | continue; 569 | point_d.intensity = i; 570 | // Add neighbour point 571 | runs_[0].insert_after(runs_[0].cbefore_begin(), &laser_frame_[i][interest_idx_]); 572 | } 573 | } 574 | #endif 575 | // Extract Clusters 576 | // re-organize scan-line points into cluster point cloud 577 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 578 | pcl::PointCloud::Ptr clusters(new pcl::PointCloud()); 579 | 580 | #ifdef IO 581 | // should be modified 582 | std::string str_path = "./PCD"; 583 | if(!boost::filesystem::exists(str_path)){ 584 | boost::filesystem::create_directories(str_path); 585 | } 586 | boost::format fmt1(str_path + "/%d_%d.%s"); 587 | pcl::PointCloud::Ptr to_save(new pcl::PointCloud()); 588 | pcl::PointCloud::Ptr to_save_all(new pcl::PointCloud()); 589 | #endif 590 | 591 | #ifdef MARKER 592 | visualization_msgs::MarkerArray ma; 593 | #endif 594 | int cnt = 0; 595 | 596 | // Re-organize pointcloud clusters for PCD saving or publish 597 | #ifdef INTEREST_ONLY 598 | for(size_t i=0;i<1;i++){ 599 | #else 600 | for(size_t i=2;ilabel = cnt; 611 | laserCloud->points.push_back(*p); 612 | // clusters->points.push_back(*p); 613 | #ifdef IO 614 | to_save_all->points.push_back(*p); 615 | to_save->points.push_back(*p); 616 | #endif 617 | } 618 | 619 | #ifdef IO 620 | if(tab==1){ 621 | pcl::io::savePCDFileBinary((fmt1%(tab)%(cnt)%"pcd").str(), *to_save); 622 | to_save->clear(); 623 | } 624 | #endif 625 | 626 | #ifdef INTEREST_ONLY 627 | // Counting interested point and its neighbour points. 628 | ROS_INFO("cluster i:%d, size:%d",i, ccnt); 629 | #endif 630 | // clusters->clear(); 631 | } 632 | } 633 | ROS_INFO("Total cluster: %d", cnt); 634 | // Publish Cluster Points 635 | if(laserCloud->points.size()>0){ 636 | sensor_msgs::PointCloud2 cluster_msg; 637 | pcl::toROSMsg(*laserCloud, cluster_msg); 638 | cluster_msg.header.frame_id = "/velodyne"; 639 | cluster_points_pub_.publish(cluster_msg); 640 | } 641 | #ifdef IO 642 | // Only save the clusterred results of the first frame. 643 | if(tab==1){ 644 | to_save_all->height = to_save_all->points.size(); 645 | to_save_all->width = 1; 646 | pcl::io::savePCDFileASCII((fmt1%(0)%(0)%"pcd").str(), *to_save_all); 647 | } 648 | tab++; 649 | #endif 650 | } 651 | 652 | int main(int argc, char **argv) 653 | { 654 | 655 | ros::init(argc, argv, "ScanLineRun"); 656 | ScanLineRun node; 657 | ros::spin(); 658 | 659 | return 0; 660 | 661 | } -------------------------------------------------------------------------------- /nodes/ground_filter/groundplanfit.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | @file groundplanfit.cpp 3 | @brief ROS Node for ground plane fitting 4 | 5 | This is a ROS node to perform ground plan fitting. 6 | Implementation accoriding to 7 | 8 | In this case, it's assumed that the x,y axis points at sea-level, 9 | and z-axis points up. The sort of height is based on the Z-axis value. 10 | 11 | @author Vincent Cheung(VincentCheungm) 12 | @bug Sometimes the plane is not fit. 13 | */ 14 | 15 | #include 16 | // For disable PCL complile lib, to use PointXYZIR 17 | #define PCL_NO_PRECOMPILE 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | //Customed Point Struct for holding clustered points 31 | namespace scan_line_run 32 | { 33 | /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */ 34 | struct PointXYZIRL 35 | { 36 | PCL_ADD_POINT4D; // quad-word XYZ 37 | float intensity; ///< laser intensity reading 38 | uint16_t ring; ///< laser ring number 39 | uint16_t label; ///< point label 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 41 | } EIGEN_ALIGN16; 42 | 43 | }; // namespace scan_line_run 44 | 45 | #define SLRPointXYZIRL scan_line_run::PointXYZIRL 46 | #define VPoint velodyne_pointcloud::PointXYZIR 47 | #define RUN pcl::PointCloud 48 | // Register custom point struct according to PCL 49 | POINT_CLOUD_REGISTER_POINT_STRUCT(scan_line_run::PointXYZIRL, 50 | (float, x, x) 51 | (float, y, y) 52 | (float, z, z) 53 | (float, intensity, intensity) 54 | (uint16_t, ring, ring) 55 | (uint16_t, label, label)) 56 | 57 | // using eigen lib 58 | #include 59 | using Eigen::MatrixXf; 60 | using Eigen::JacobiSVD; 61 | using Eigen::VectorXf; 62 | 63 | pcl::PointCloud::Ptr g_seeds_pc(new pcl::PointCloud()); 64 | pcl::PointCloud::Ptr g_ground_pc(new pcl::PointCloud()); 65 | pcl::PointCloud::Ptr g_not_ground_pc(new pcl::PointCloud()); 66 | pcl::PointCloud::Ptr g_all_pc(new pcl::PointCloud()); 67 | 68 | 69 | /* 70 | @brief Compare function to sort points. Here use z axis. 71 | @return z-axis accent 72 | */ 73 | bool point_cmp(VPoint a, VPoint b){ 74 | return a.z& p_sorted); 112 | 113 | // Model parameter for ground plane fitting 114 | // The ground plane model is: ax+by+cz+d=0 115 | // Here normal:=[a,b,c], d=d 116 | // th_dist_d_ = threshold_dist - d 117 | float d_; 118 | MatrixXf normal_; 119 | float th_dist_d_; 120 | }; 121 | 122 | /* 123 | @brief Constructor of GPF Node. 124 | @return void 125 | */ 126 | GroundPlaneFit::GroundPlaneFit():node_handle_("~"){ 127 | // Init ROS related 128 | ROS_INFO("Inititalizing Ground Plane Fitter..."); 129 | node_handle_.param("point_topic", point_topic_, "/velodyne_points"); 130 | ROS_INFO("Input Point Cloud: %s", point_topic_.c_str()); 131 | 132 | node_handle_.param("sensor_model", sensor_model_, 32); 133 | ROS_INFO("Sensor Model: %d", sensor_model_); 134 | 135 | node_handle_.param("sensor_height", sensor_height_, 2.5); 136 | ROS_INFO("Sensor Height: %f", sensor_height_); 137 | 138 | node_handle_.param("num_seg", num_seg_, 1); 139 | ROS_INFO("Num of Segments: %d", num_seg_); 140 | 141 | node_handle_.param("num_iter", num_iter_, 3); 142 | ROS_INFO("Num of Iteration: %d", num_iter_); 143 | 144 | node_handle_.param("num_lpr", num_lpr_, 20); 145 | ROS_INFO("Num of LPR: %d", num_lpr_); 146 | 147 | node_handle_.param("th_seeds", th_seeds_, 1.2); 148 | ROS_INFO("Seeds Threshold: %f", th_seeds_); 149 | 150 | node_handle_.param("th_dist", th_dist_, 0.3); 151 | ROS_INFO("Distance Threshold: %f", th_dist_); 152 | 153 | // Listen to velodyne topic 154 | points_node_sub_ = node_handle_.subscribe(point_topic_, 2, &GroundPlaneFit::velodyne_callback_, this); 155 | 156 | // Publish Init 157 | std::string no_ground_topic, ground_topic; 158 | node_handle_.param("no_ground_point_topic", no_ground_topic, "/points_no_ground"); 159 | ROS_INFO("No Ground Output Point Cloud: %s", no_ground_topic.c_str()); 160 | node_handle_.param("ground_point_topic", ground_topic, "/points_ground"); 161 | ROS_INFO("Only Ground Output Point Cloud: %s", ground_topic.c_str()); 162 | groundless_points_pub_ = node_handle_.advertise(no_ground_topic, 2); 163 | ground_points_pub_ = node_handle_.advertise(ground_topic, 2); 164 | all_points_pub_ = node_handle_.advertise("/all_points", 2); 165 | } 166 | 167 | /* 168 | @brief The function to estimate plane model. The 169 | model parameter `normal_` and `d_`, and `th_dist_d_` 170 | is set here. 171 | The main step is performed SVD(UAV) on covariance matrix. 172 | Taking the sigular vector in U matrix according to the smallest 173 | sigular value in A, as the `normal_`. `d_` is then calculated 174 | according to mean ground points. 175 | 176 | @param g_ground_pc:global ground pointcloud ptr. 177 | 178 | */ 179 | void GroundPlaneFit::estimate_plane_(void){ 180 | // Create covarian matrix in single pass. 181 | // TODO: compare the efficiency. 182 | Eigen::Matrix3f cov; 183 | Eigen::Vector4f pc_mean; 184 | pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean); 185 | // Singular Value Decomposition: SVD 186 | JacobiSVD svd(cov,Eigen::DecompositionOptions::ComputeFullU); 187 | // use the least singular vector as normal 188 | normal_ = (svd.matrixU().col(2)); 189 | // mean ground seeds value 190 | Eigen::Vector3f seeds_mean = pc_mean.head<3>(); 191 | 192 | // according to normal.T*[x,y,z] = -d 193 | d_ = -(normal_.transpose()*seeds_mean)(0,0); 194 | // set distance threhold to `th_dist - d` 195 | th_dist_d_ = th_dist_ - d_; 196 | 197 | // return the equation parameters 198 | } 199 | 200 | 201 | /* 202 | @brief Extract initial seeds of the given pointcloud sorted segment. 203 | This function filter ground seeds points accoring to heigt. 204 | This function will set the `g_ground_pc` to `g_seed_pc`. 205 | @param p_sorted: sorted pointcloud 206 | 207 | @param ::num_lpr_: num of LPR points 208 | @param ::th_seeds_: threshold distance of seeds 209 | @param :: 210 | 211 | */ 212 | void GroundPlaneFit::extract_initial_seeds_(const pcl::PointCloud& p_sorted){ 213 | // LPR is the mean of low point representative 214 | double sum = 0; 215 | int cnt = 0; 216 | // Calculate the mean height value. 217 | for(int i=0;iclear(); 223 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ 224 | for(int i=0;ipoints.push_back(p_sorted.points[i]); 227 | } 228 | } 229 | // return seeds points 230 | } 231 | 232 | /* 233 | @brief Velodyne pointcloud callback function. The main GPF pipeline is here. 234 | PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud 235 | ->error points removal -> extract ground seeds -> ground plane fit mainloop 236 | */ 237 | void GroundPlaneFit::velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& in_cloud_msg){ 238 | // 1.Msg to pointcloud 239 | pcl::PointCloud laserCloudIn; 240 | pcl::fromROSMsg(*in_cloud_msg, laserCloudIn); 241 | pcl::PointCloud laserCloudIn_org; 242 | pcl::fromROSMsg(*in_cloud_msg, laserCloudIn_org); 243 | // For mark ground points and hold all points 244 | SLRPointXYZIRL point; 245 | for(size_t i=0;ipoints.push_back(point); 253 | } 254 | //std::vector indices; 255 | //pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn,indices); 256 | // 2.Sort on Z-axis value. 257 | sort(laserCloudIn.points.begin(),laserCloudIn.end(),point_cmp); 258 | // 3.Error point removal 259 | // As there are some error mirror reflection under the ground, 260 | // here regardless point under 2* sensor_height 261 | // Sort point according to height, here uses z-axis in default 262 | pcl::PointCloud::iterator it = laserCloudIn.points.begin(); 263 | for(int i=0;iclear(); 279 | g_not_ground_pc->clear(); 280 | 281 | //pointcloud to matrix 282 | MatrixXf points(laserCloudIn_org.points.size(),3); 283 | int j =0; 284 | for(auto p:laserCloudIn_org.points){ 285 | points.row(j++)<points[r].label = 1u;// means ground 293 | g_ground_pc->points.push_back(laserCloudIn_org[r]); 294 | }else{ 295 | g_all_pc->points[r].label = 0u;// means not ground and non clusterred 296 | g_not_ground_pc->points.push_back(laserCloudIn_org[r]); 297 | } 298 | } 299 | } 300 | 301 | 302 | // publish ground points 303 | sensor_msgs::PointCloud2 ground_msg; 304 | pcl::toROSMsg(*g_ground_pc, ground_msg); 305 | ground_msg.header.stamp = in_cloud_msg->header.stamp; 306 | ground_msg.header.frame_id = in_cloud_msg->header.frame_id; 307 | ground_points_pub_.publish(ground_msg); 308 | // publish not ground points 309 | sensor_msgs::PointCloud2 groundless_msg; 310 | pcl::toROSMsg(*g_not_ground_pc, groundless_msg); 311 | groundless_msg.header.stamp = in_cloud_msg->header.stamp; 312 | groundless_msg.header.frame_id = in_cloud_msg->header.frame_id; 313 | groundless_points_pub_.publish(groundless_msg); 314 | // publish all points 315 | sensor_msgs::PointCloud2 all_points_msg; 316 | pcl::toROSMsg(*g_all_pc, all_points_msg); 317 | all_points_msg.header.stamp = in_cloud_msg->header.stamp; 318 | all_points_msg.header.frame_id = in_cloud_msg->header.frame_id; 319 | all_points_pub_.publish(all_points_msg); 320 | g_all_pc->clear(); 321 | } 322 | 323 | int main(int argc, char **argv) 324 | { 325 | 326 | ros::init(argc, argv, "GroundPlaneFit"); 327 | GroundPlaneFit node; 328 | ros::spin(); 329 | 330 | return 0; 331 | 332 | } -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | points_preprocessor_usi 4 | 1.0.0 5 | The points_preprocessor_usi package 6 | 7 | vincent 8 | 9 | BSD 10 | 11 | catkin 12 | roscpp 13 | std_msgs 14 | sensor_msgs 15 | pcl_conversions 16 | pcl_ros 17 | velodyne_pointcloud 18 | 19 | message_runtime 20 | pcl_conversions 21 | pcl_ros 22 | roscpp 23 | sensor_msgs 24 | sensor_msgs 25 | velodyne_pointcloud 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /pic/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VincentCheungM/Run_based_segmentation/50f4f1bd5f4141d95968c9adb59cb55c10f1db71/pic/1.png -------------------------------------------------------------------------------- /pic/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VincentCheungM/Run_based_segmentation/50f4f1bd5f4141d95968c9adb59cb55c10f1db71/pic/2.png -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Run_based_segmentation 2 | A ROS node to perform `ground plane fitting` and `run_based_segmentation` for LiDAR pointcloud. 3 | 4 | ![pic1](./pic/1.png) 5 | ![pic2](./pic/2.png) 6 | 7 | ```bib 8 | @inproceedings{Zermas2017Fast, 9 | title={Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications}, 10 | author={Zermas, Dimitris and Izzat, Izzat and Papanikolopoulos, Nikolaos}, 11 | booktitle={IEEE International Conference on Robotics and Automation}, 12 | year={2017}, 13 | } 14 | ``` 15 | 16 | ## Requirement 17 | * [PCL](https://github.com/PointCloudLibrary/pcl) 18 | * [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) 19 | * [ROS Velodyne_driver](https://github.com/ros-drivers/velodyne) 20 | 21 | ## Run 22 | ```bash 23 | $ catkin_make 24 | $ rosrun points_preprocessor_usi groundplanfit 25 | $ rosrun points_preprocessor_usi scanlinerun 26 | ``` 27 | And cluster point cloud will be published as `cluster` with different label. 28 | --------------------------------------------------------------------------------