├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg └── ri_dbscan.cfg ├── include ├── ground_truth.hpp ├── ri_dbscan.h └── tools │ └── utils.hpp ├── launch ├── run.launch └── run_rviz.launch ├── media └── demo_01.gif ├── package.xml ├── rviz └── ri_dbscan.rviz └── src └── ri_dbscan.cpp /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package range image 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | ->(2022-04-26) 6 | ------------------- 7 | * [fix] Deal with Nanvalid point issue 8 | * The output of ROS_INFO will slow the speed of algorithm greatly. 9 | 10 | ->(2022-04-27) 11 | ------------------- 12 | * [fix] Deal with Nanvalid point issue 13 | * Fix the Nanvalid point effect 14 | * consider the Nanvalid point in vertical direction 15 | 16 | ->(2022-06-15) 17 | ------------------- 18 | * combine the dbscan algorithnm with range image search method to overcome over-segmentation 19 | 20 | ->(2022-06-16) 21 | ------------------- 22 | * -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ri_dbscan) 3 | 4 | add_compile_options(-std=c++17) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | pcl_ros 16 | tf2_ros 17 | tf2_geometry_msgs 18 | dynamic_reconfigure 19 | jsk_recognition_msgs 20 | ) 21 | 22 | find_package(OpenCV REQUIRED) 23 | find_package(PCL 1.8.1 REQUIRED) 24 | 25 | set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}") 26 | 27 | generate_dynamic_reconfigure_options( 28 | cfg/ri_dbscan.cfg 29 | ) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS include 33 | LIBRARIES ri_dbscan 34 | CATKIN_DEPENDS roscpp std_msgs 35 | DEPENDS system_lib 36 | ) 37 | 38 | include_directories( 39 | include 40 | ${catkin_INCLUDE_DIRS} 41 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 42 | ${OpenCV_INCLUDE_DIRS} 43 | ) 44 | 45 | link_directories(${OpenCV_LIBRARY_DIRS}) 46 | 47 | add_executable(ri_dbscan_node 48 | src/ri_dbscan.cpp 49 | ) 50 | 51 | add_dependencies(ri_dbscan_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 52 | 53 | target_link_libraries(ri_dbscan_node 54 | ${OpenCV_LIBRARIES} 55 | ${catkin_LIBRARIES} 56 | ) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Fast range image based DBSCAN clustering for 3D LiDAR Point Clouds 2 | An ROS implementation of dbscan clustering of 3D LiDAR point clouds 3 | 4 | ![Ubuntu](https://img.shields.io/badge/OS-Ubuntu-informational?style=flat&logo=ubuntu&logoColor=white&color=2bbc8a) 5 | ![ROS](https://img.shields.io/badge/Tools-ROS-informational?style=flat&logo=ROS&logoColor=white&color=2bbc8a) 6 | ![C++](https://img.shields.io/badge/Code-C++-informational?style=flat&logo=c%2B%2B&logoColor=white&color=2bbc8a) 7 | 8 | ![demo_1](media/demo_01.gif) 9 | 10 | 11 | ## Reference 12 | * Chen, Zhihui, et al. "Fast-spherical-projection-based point cloud clustering algorithm." Transportation research record 2676.6 (2022): 315-329. 13 | 14 | 15 | ## Features 16 | * Faster comparing to traditional dbscan algorithm 17 | 18 | **TODOs** 19 | * imporove the segmentation accuracy 20 | 21 | 22 | ## Dependencies 23 | * semantic_kitti_loader 24 | * obsdet_msgs 25 | 26 | ## How to use 27 | # clone the repo 28 | mkdir -p catkin_ws/src 29 | cd catkin_ws/src 30 | git clone https://github.com/HMX2013/SemanticKITTI_loader 31 | git clone https://github.com/HMX2013/FSPC-ROS 32 | download obsdet_msgs from 33 | "https://drive.google.com/file/d/1ztLk9Slm656CV-WJieUpBJPlz-Iw14Bk/view?usp=share_link" 34 | cd ../ 35 | catkin_make 36 | 37 | roslaunch ri_dbscan run_rviz.launch 38 | roslaunch semantic_kitti run_semantic.launch 39 | 40 | ## Contribution 41 | You are welcome contributing to the package by opening a pull-request 42 | 43 | We are following: 44 | [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html), 45 | [C++ Core Guidelines](https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#main), 46 | and [ROS C++ Style Guide](http://wiki.ros.org/CppStyleGuide) 47 | 48 | ## License 49 | MIT License 50 | -------------------------------------------------------------------------------- /cfg/ri_dbscan.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = "ri_dbscan" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("detect_min", double_t, 0, "Default: 0.1", 0.1, 0, 2) 10 | gen.add("detect_max", double_t, 0, "Default: 10", 10, 2, 40) 11 | 12 | gen.add("cvc_coef", double_t, 0, "Default: 1.0", 1.0, 1, 3) 13 | 14 | gen.add("MinClusterSize", int_t, 0, "Default: 20", 20, 10, 100) 15 | gen.add("MaxClusterSize", int_t, 0, "Default: 1000", 10000, 20, 10000) 16 | 17 | exit(gen.generate(PACKAGE, "ri_dbscan", "ri_dbscan_")) -------------------------------------------------------------------------------- /include/ground_truth.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_TRUTH_H 2 | #define GROUND_TRUTH_H 3 | 4 | #pragma once 5 | #define PCL_NO_PRECOMPILE 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tools/utils.hpp" 21 | 22 | // VLP-16 23 | // const int HORZ_SCAN = 1800; 24 | // const int VERT_SCAN = 16; 25 | // const float MAX_VERT_ANGLE = 15.0; 26 | // const float MIN_VERT_ANGLE = -15.0; 27 | 28 | // Kitti 29 | const int HORZ_SCAN = 2048; 30 | const int VERT_SCAN = 64; 31 | const float MAX_VERT_ANGLE = 3.0; 32 | const float MIN_VERT_ANGLE = -25.0; 33 | // horizen 0.08 vetical 0.4 34 | 35 | namespace groundtruth { 36 | template 37 | class DepthCluster { 38 | private: 39 | uint16_t max_label_; 40 | 41 | std::vector vert_angles_; 42 | std::vector> clusterIndices; 43 | 44 | std::vector index_v; 45 | 46 | uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed 47 | uint16_t *queueIndY; 48 | 49 | uint16_t *allPushedIndX; // array for tracking points of a segmented object 50 | uint16_t *allPushedIndY; 51 | 52 | int labelCount; 53 | std::vector > neighborIterator; // neighbor iterator for segmentaiton process 54 | 55 | cv::Mat rangeMat; // range matrix for range image 56 | cv::Mat labelMat; // label matrix for segmentaiton marking 57 | 58 | PointT nanPoint; // fill in fullCloud at each iteration 59 | typename pcl::PointCloud::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix 60 | 61 | public: 62 | DepthCluster() {} 63 | 64 | ~DepthCluster() {} 65 | 66 | void allocateMemory() 67 | { 68 | // clusterIndices.clear(); 69 | fullCloud.reset(new pcl::PointCloud()); 70 | fullCloud->points.resize(VERT_SCAN*HORZ_SCAN); 71 | index_v.resize(VERT_SCAN * HORZ_SCAN); 72 | nanPoint.x = std::numeric_limits::quiet_NaN(); 73 | nanPoint.y = std::numeric_limits::quiet_NaN(); 74 | nanPoint.z = std::numeric_limits::quiet_NaN(); 75 | nanPoint.intensity = -1; 76 | nanPoint.id = 0; 77 | nanPoint.label = 0; 78 | nanPoint.cid = 0; 79 | 80 | queueIndX = new uint16_t[VERT_SCAN*HORZ_SCAN]; 81 | queueIndY = new uint16_t[VERT_SCAN*HORZ_SCAN]; 82 | 83 | allPushedIndX = new uint16_t[VERT_SCAN*HORZ_SCAN]; 84 | allPushedIndY = new uint16_t[VERT_SCAN*HORZ_SCAN]; 85 | 86 | std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint); 87 | labelMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32S, cv::Scalar::all(0)); 88 | rangeMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); 89 | labelCount = 1; 90 | 91 | std::pair neighbor; 92 | 93 | int8_t neighbor_col = 60; 94 | 95 | for (int8_t i = -4; i <= 4; i++) 96 | { 97 | for (int8_t j = -neighbor_col; j <= neighbor_col; j++) 98 | { 99 | neighbor.first = i; 100 | neighbor.second = j; 101 | neighborIterator.push_back(neighbor); 102 | } 103 | } 104 | float resolution = (float)(MAX_VERT_ANGLE - MIN_VERT_ANGLE) / (float)(VERT_SCAN - 1); 105 | for (int i = 0; i < VERT_SCAN; i++) 106 | vert_angles_.push_back(MIN_VERT_ANGLE + i * resolution); 107 | } 108 | 109 | void setParams(int _vert_scan, int _horz_scan, float _min_range, float _max_range, 110 | float _min_vert_angle, float _max_vert_angle, 111 | float _horz_merge_thres, float _vert_merge_thres, int _vert_scan_size, 112 | int _horz_scan_size, int _horz_extension_size, int _horz_skip_size, 113 | boost::optional _min_cluster_size=boost::none, 114 | boost::optional _max_cluster_size=boost::none) { 115 | 116 | std::cout<<""<(HORZ_SCAN)); 120 | 121 | // valid_cnt_.resize(VERT_SCAN, 0); 122 | // nodes_.resize(VERT_SCAN, vector()); 123 | 124 | // if (_min_cluster_size) 125 | // MIN_CLUSTER_SIZE = _min_cluster_size; 126 | 127 | // if (_max_cluster_size) 128 | // MAX_CLUSTER_SIZE = _max_cluster_size; 129 | } 130 | 131 | void GTV(boost::shared_ptr> cloud_in, std::vector> &clusterIndices) 132 | { 133 | // 0. reset 134 | allocateMemory(); 135 | clusterIndices.clear(); 136 | 137 | // 1. do spherical projection 138 | sphericalProjection(cloud_in); 139 | 140 | // 2. begin clustering 141 | for (size_t i = 0; i < VERT_SCAN; ++i) 142 | { 143 | for (size_t j = 0; j < HORZ_SCAN; ++j) 144 | { 145 | if (labelMat.at(i, j) == -1) 146 | labelComponents(i, j, clusterIndices); 147 | } 148 | } 149 | // return clusterIndices; 150 | } 151 | 152 | void sphericalProjection(boost::shared_ptr> cloud_in) 153 | { 154 | float range; 155 | size_t rowIdn, columnIdn, index, cloudSize; 156 | PointT cpt; 157 | 158 | cloudSize = cloud_in->points.size(); 159 | int org_index = 0; 160 | 161 | for (size_t i = 0; i < cloudSize; ++i) 162 | { 163 | cpt.x = cloud_in->points[i].x; 164 | cpt.y = cloud_in->points[i].y; 165 | cpt.z = cloud_in->points[i].z; 166 | cpt.intensity = cloud_in->points[i].intensity; 167 | cpt.id = cloud_in->points[i].id; 168 | cpt.label = cloud_in->points[i].label; 169 | 170 | bool is_nan = std::isnan(cpt.x) || std::isnan(cpt.y) || std::isnan(cpt.z); 171 | if (is_nan) {continue;} 172 | 173 | //find the row and column index in the iamge for this point 174 | // rowIdn = getRowIdx(cpt); 175 | rowIdn = cloud_in->points[i].ring; 176 | 177 | if (rowIdn < 0 || rowIdn >= VERT_SCAN) 178 | continue; 179 | 180 | columnIdn = getColIdx(cpt); 181 | 182 | if (columnIdn < 0 || columnIdn >= HORZ_SCAN) 183 | continue; 184 | 185 | range = sqrt(cpt.x * cpt.x + cpt.y * cpt.y + cpt.z * cpt.z); 186 | 187 | labelMat.at(rowIdn, columnIdn) = -1; 188 | rangeMat.at(rowIdn, columnIdn) = range; 189 | 190 | index = columnIdn + rowIdn * HORZ_SCAN; 191 | index_v[index] = org_index; 192 | 193 | fullCloud->points[index] = cpt; 194 | org_index++; 195 | } 196 | } 197 | 198 | int getRowIdx(PointT pt) 199 | { 200 | float angle = atan2(pt.z, sqrt(pt.x * pt.x + pt.y * pt.y)) * 180 / M_PI; 201 | 202 | auto iter_geq = std::lower_bound(vert_angles_.begin(), vert_angles_.end(), angle); 203 | int row_idx; 204 | 205 | if (iter_geq == vert_angles_.begin()) 206 | { 207 | row_idx = 0; 208 | } 209 | else 210 | { 211 | float a = *(iter_geq - 1); 212 | float b = *(iter_geq); 213 | if (fabs(angle - a) < fabs(angle - b)) 214 | { 215 | row_idx = iter_geq - vert_angles_.begin() - 1; 216 | } 217 | else 218 | { 219 | row_idx = iter_geq - vert_angles_.begin(); 220 | } 221 | } 222 | return row_idx; 223 | } 224 | 225 | int getColIdx(PointT pt) 226 | { 227 | float horizonAngle = atan2(pt.x, pt.y) * 180 / M_PI; 228 | static float ang_res_x = 360.0 / float(HORZ_SCAN); 229 | int col_idx = -round((horizonAngle - 90.0) / ang_res_x) + HORZ_SCAN / 2; 230 | if (col_idx >= HORZ_SCAN) 231 | col_idx -= HORZ_SCAN; 232 | return col_idx; 233 | } 234 | 235 | void labelComponents(int row, int col, std::vector> &clusterIndices) 236 | { 237 | // use std::queue std::vector std::deque will slow the program down greatly 238 | int fromIndX, fromIndY, thisIndX, thisIndY; 239 | float d1, d2; 240 | 241 | queueIndX[0] = row; 242 | queueIndY[0] = col; 243 | int queueSize = 1; 244 | int queueStartInd = 0; 245 | int queueEndInd = 1; 246 | 247 | allPushedIndX[0] = row; 248 | allPushedIndY[0] = col; 249 | int allPushedIndSize = 1; 250 | 251 | std::vector clusterIndice; 252 | 253 | //standard BFS 254 | while (queueSize > 0) 255 | { 256 | // Pop point 257 | fromIndX = queueIndX[queueStartInd]; 258 | fromIndY = queueIndY[queueStartInd]; 259 | --queueSize; 260 | ++queueStartInd; 261 | // Mark popped point, The initial value of labelCount is 1. 262 | labelMat.at(fromIndX, fromIndY) = labelCount; 263 | 264 | // Loop through all the neighboring grids of popped grid, neighbor=[[-1,0];[0,1];[0,-1];[1,0]] 265 | for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) 266 | { 267 | // new index 268 | thisIndX = fromIndX + (*iter).first; 269 | thisIndY = fromIndY + (*iter).second; 270 | 271 | // index should be within the boundary 272 | if (thisIndX < 0 || thisIndX >= VERT_SCAN) 273 | continue; 274 | // at range image margin (left or right side) 275 | if (thisIndY < 0) 276 | thisIndY = HORZ_SCAN - 1; 277 | if (thisIndY >= HORZ_SCAN) 278 | thisIndY = 0; 279 | // prevent infinite loop (caused by put already examined point back) 280 | if (labelMat.at(thisIndX, thisIndY) != -1) 281 | continue; 282 | 283 | /*---------------------------condition----------------------------*/ 284 | d1 = std::max(rangeMat.at(fromIndX, fromIndY), 285 | rangeMat.at(thisIndX, thisIndY)); 286 | d2 = std::min(rangeMat.at(fromIndX, fromIndY), 287 | rangeMat.at(thisIndX, thisIndY)); 288 | 289 | PointT pt_from = fullCloud->points[fromIndY + fromIndX * HORZ_SCAN]; 290 | PointT pt_this = fullCloud->points[thisIndY + thisIndX * HORZ_SCAN]; 291 | 292 | bool same_cluster = false; 293 | 294 | if (pt_from.label == 10 && pt_this.label == 10) 295 | { 296 | if (pt_from.id == pt_this.id) 297 | same_cluster = true; 298 | } 299 | 300 | if (pt_from.label != 10 && pt_this.label != 10) 301 | { 302 | float dist = (pt_from.x - pt_this.x) * (pt_from.x - pt_this.x) 303 | + (pt_from.y - pt_this.y) * (pt_from.y - pt_this.y) 304 | + (pt_from.z - pt_this.z) * (pt_from.z - pt_this.z); 305 | 306 | if (pt_from.label == pt_this.label && dist < 0.5 * 0.5) 307 | same_cluster = true; 308 | } 309 | 310 | if (same_cluster) 311 | { 312 | queueIndX[queueEndInd] = thisIndX; 313 | queueIndY[queueEndInd] = thisIndY; 314 | ++queueSize; 315 | ++queueEndInd; 316 | 317 | labelMat.at(thisIndX, thisIndY) = labelCount; 318 | 319 | clusterIndice.push_back(index_v[thisIndY + thisIndX * HORZ_SCAN]); 320 | 321 | allPushedIndX[allPushedIndSize] = thisIndX; 322 | allPushedIndY[allPushedIndSize] = thisIndY; 323 | ++allPushedIndSize; 324 | } 325 | } 326 | } 327 | 328 | // check if this segment is valid 329 | bool feasibleSegment = false; 330 | 331 | if (allPushedIndSize >= 5) 332 | feasibleSegment = true; 333 | 334 | // segment is valid, mark these points 335 | if (feasibleSegment == true){ 336 | ++labelCount; 337 | clusterIndices.push_back(clusterIndice); 338 | } 339 | else{ 340 | // segment is invalid, mark these points 341 | for (size_t i = 0; i < allPushedIndSize; ++i) { 342 | labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 0; 343 | } 344 | } 345 | } 346 | }; 347 | } 348 | 349 | #endif -------------------------------------------------------------------------------- /include/ri_dbscan.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #if (CV_MAJOR_VERSION == 3) 24 | #include 25 | #else 26 | #include 27 | #endif 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "obsdet_msgs/CloudCluster.h" 35 | #include "obsdet_msgs/CloudClusterArray.h" 36 | 37 | #include 38 | 39 | #include "ground_truth.hpp" 40 | 41 | #define __APP_NAME__ "ri_dbscan" 42 | 43 | using PointType = PointXYZILID; 44 | 45 | 46 | std::string output_frame_; 47 | std::string non_ground_cloud_topic_; 48 | std::string segmented_cloud_topic_; 49 | std::string cluster_cloud_topic_; 50 | std::string colored_cloud_topic_; 51 | std::string cluster_cloud_trans_topic_; 52 | std::string output_cluster_array_topic_; 53 | std::string output_roi_topic_; 54 | std_msgs::Header ros_header; 55 | 56 | // Pointcloud Filtering Parameters 57 | float DETECT_MIN, DETECT_MAX; 58 | 59 | int CLUSTER_MAX_SIZE, CLUSTER_MIN_SIZE, MinClusterSize, MaxClusterSize; 60 | float cvc_coef; 61 | 62 | tf::TransformListener *_transform_listener; 63 | tf::StampedTransform *_transform; 64 | 65 | boost::shared_ptr> gt_verify; 66 | 67 | 68 | static ros::Publisher time_rviz_pub_; 69 | static std_msgs::Float32 time_rviz; 70 | // static double exe_time = 0.0; -------------------------------------------------------------------------------- /include/tools/utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include "math.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | // CLASSES 28 | #define SENSOR_HEIGHT 1.73 29 | 30 | #define UNLABELED 0 31 | #define OUTLIER 1 32 | #define NUM_ALL_CLASSES 34 33 | #define ROAD 40 34 | #define PARKING 44 35 | #define SIDEWALKR 48 36 | #define OTHER_GROUND 49 37 | #define BUILDING 50 38 | #define FENSE 51 39 | #define LANE_MARKING 60 40 | #define VEGETATION 70 41 | #define TERRAIN 72 42 | 43 | #define TRUEPOSITIVE 3 44 | #define TRUENEGATIVE 2 45 | #define FALSEPOSITIVE 1 46 | #define FALSENEGATIVE 0 47 | 48 | int NUM_ZEROS = 5; 49 | 50 | using namespace std; 51 | 52 | double VEGETATION_THR = - SENSOR_HEIGHT * 3 / 4; 53 | /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */ 54 | 55 | struct PointXYZILID 56 | { 57 | PCL_ADD_POINT4D; // quad-word XYZ 58 | float intensity; ///< laser intensity reading 59 | uint16_t ring; ///< laser intensity reading 60 | uint16_t label; ///< point label 61 | uint16_t id; 62 | uint16_t cid; 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 64 | } EIGEN_ALIGN16; 65 | 66 | // Register custom point struct according to PCL 67 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID, 68 | (float, x, x) 69 | (float, y, y) 70 | (float, z, z) 71 | (float, intensity, intensity) 72 | (uint16_t, ring, ring) 73 | (uint16_t, label, label) 74 | (uint16_t, id, id) 75 | (uint16_t, cid, cid)) 76 | 77 | 78 | void PointXYZILID2XYZI(pcl::PointCloud& src, 79 | pcl::PointCloud::Ptr dst){ 80 | dst->points.clear(); 81 | for (const auto &pt: src.points){ 82 | pcl::PointXYZI pt_xyzi; 83 | pt_xyzi.x = pt.x; 84 | pt_xyzi.y = pt.y; 85 | pt_xyzi.z = pt.z; 86 | pt_xyzi.intensity = pt.intensity; 87 | dst->points.push_back(pt_xyzi); 88 | } 89 | } 90 | std::vector outlier_classes = {UNLABELED, OUTLIER}; 91 | std::vector ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN}; 92 | std::vector ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING}; 93 | 94 | int count_num_ground(const pcl::PointCloud& pc){ 95 | int num_ground = 0; 96 | 97 | std::vector::iterator iter; 98 | 99 | for (auto const& pt: pc.points){ 100 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 101 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 102 | if (pt.label == VEGETATION){ 103 | if (pt.z < VEGETATION_THR){ 104 | num_ground ++; 105 | } 106 | }else num_ground ++; 107 | } 108 | } 109 | return num_ground; 110 | } 111 | 112 | std::map set_initial_gt_counts(std::vector& gt_classes){ 113 | map gt_counts; 114 | for (int i = 0; i< gt_classes.size(); ++i){ 115 | gt_counts.insert(pair(gt_classes.at(i), 0)); 116 | } 117 | return gt_counts; 118 | } 119 | 120 | std::map count_num_each_class(const pcl::PointCloud& pc){ 121 | int num_ground = 0; 122 | auto gt_counts = set_initial_gt_counts(ground_classes); 123 | std::vector::iterator iter; 124 | 125 | for (auto const& pt: pc.points){ 126 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 127 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 128 | if (pt.label == VEGETATION){ 129 | if (pt.z < VEGETATION_THR){ 130 | gt_counts.find(pt.label)->second++; 131 | } 132 | }else gt_counts.find(pt.label)->second++; 133 | } 134 | } 135 | return gt_counts; 136 | } 137 | 138 | int count_num_outliers(const pcl::PointCloud& pc){ 139 | int num_outliers = 0; 140 | 141 | std::vector::iterator iter; 142 | 143 | for (auto const& pt: pc.points){ 144 | iter = std::find(outlier_classes.begin(), outlier_classes.end(), pt.label); 145 | if (iter != outlier_classes.end()){ // corresponding class is in ground classes 146 | num_outliers ++; 147 | } 148 | } 149 | return num_outliers; 150 | } 151 | 152 | 153 | void discern_ground(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){ 154 | ground.clear(); 155 | non_ground.clear(); 156 | std::vector::iterator iter; 157 | for (auto const& pt: src.points){ 158 | if (pt.label == UNLABELED || pt.label == OUTLIER) continue; 159 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 160 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 161 | if (pt.label == VEGETATION){ 162 | if (pt.z < VEGETATION_THR){ 163 | ground.push_back(pt); 164 | }else non_ground.push_back(pt); 165 | }else ground.push_back(pt); 166 | }else{ 167 | non_ground.push_back(pt); 168 | } 169 | } 170 | } 171 | 172 | 173 | void calculate_precision_recall(const pcl::PointCloud& pc_curr, 174 | pcl::PointCloud& ground_estimated, 175 | double & precision, 176 | double& recall, 177 | bool consider_outliers=true){ 178 | 179 | int num_ground_est = ground_estimated.points.size(); 180 | int num_ground_gt = count_num_ground(pc_curr); 181 | int num_TP = count_num_ground(ground_estimated); 182 | if (consider_outliers){ 183 | int num_outliers_est = count_num_outliers(ground_estimated); 184 | precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; 185 | recall = (double)(num_TP)/num_ground_gt * 100; 186 | }else{ 187 | precision = (double)(num_TP)/num_ground_est * 100; 188 | recall = (double)(num_TP)/num_ground_gt * 100; 189 | } 190 | } 191 | 192 | int save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, string seq, int count){ 193 | 194 | std::string count_str = std::to_string(count); 195 | std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 196 | std::string output_filename = ABS_DIR + "/" + seq + "/" + count_str_padded + ".csv"; 197 | ofstream sc_output(output_filename); 198 | 199 | vector labels(NUM_ALL_CLASSES, 0); 200 | for (auto const& pt: pc.points){ 201 | if (pt.label == 0) labels[0]++; 202 | else if (pt.label == 1) labels[1]++; 203 | else if (pt.label == 10) labels[2]++; 204 | else if (pt.label == 11) labels[3]++; 205 | else if (pt.label == 13) labels[4]++; 206 | else if (pt.label == 15) labels[5]++; 207 | else if (pt.label == 16) labels[6]++; 208 | else if (pt.label == 18) labels[7]++; 209 | else if (pt.label == 20) labels[8]++; 210 | else if (pt.label == 30) labels[9]++; 211 | else if (pt.label == 31) labels[10]++; 212 | else if (pt.label == 32) labels[11]++; 213 | else if (pt.label == 40) labels[12]++; 214 | else if (pt.label == 44) labels[13]++; 215 | else if (pt.label == 48) labels[14]++; 216 | else if (pt.label == 49) labels[15]++; 217 | else if (pt.label == 50) labels[16]++; 218 | else if (pt.label == 51) labels[17]++; 219 | else if (pt.label == 52) labels[18]++; 220 | else if (pt.label == 60) labels[19]++; 221 | else if (pt.label == 70) labels[20]++; 222 | else if (pt.label == 71) labels[21]++; 223 | else if (pt.label == 72) labels[22]++; 224 | else if (pt.label == 80) labels[23]++; 225 | else if (pt.label == 81) labels[24]++; 226 | else if (pt.label == 99) labels[25]++; 227 | else if (pt.label == 252) labels[26]++; 228 | else if (pt.label == 253) labels[27]++; 229 | else if (pt.label == 254) labels[28]++; 230 | else if (pt.label == 255) labels[29]++; 231 | else if (pt.label == 256) labels[30]++; 232 | else if (pt.label == 257) labels[31]++; 233 | else if (pt.label == 258) labels[32]++; 234 | else if (pt.label == 259) labels[33]++; 235 | } 236 | 237 | for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){ 238 | if (i!=33){ 239 | sc_output<& pc_curr, 248 | pcl::PointCloud& ground_estimated, string acc_filename, 249 | double& accuracy, map&pc_curr_gt_counts, map&g_est_gt_counts){ 250 | 251 | 252 | // std::cout<<"debug: "<(num_TP + (num_False - num_FP)) / num_total_gt * 100.0; 266 | 267 | pc_curr_gt_counts = count_num_each_class(pc_curr); 268 | g_est_gt_counts = count_num_each_class(ground_estimated); 269 | 270 | // save output 271 | for (auto const& class_id: ground_classes){ 272 | sc_output2<second<<","<second<<","; 273 | } 274 | sc_output2<& TP, const pcl::PointCloud& FP, 280 | const pcl::PointCloud& FN, const pcl::PointCloud& TN, 281 | std::string pcd_filename){ 282 | pcl::PointCloud pc_out; 283 | 284 | for (auto const pt: TP.points){ 285 | pcl::PointXYZI pt_est; 286 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 287 | pt_est.intensity = TRUEPOSITIVE; 288 | pc_out.points.push_back(pt_est); 289 | } 290 | for (auto const pt: FP.points){ 291 | pcl::PointXYZI pt_est; 292 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 293 | pt_est.intensity = FALSEPOSITIVE; 294 | pc_out.points.push_back(pt_est); 295 | } 296 | for (auto const pt: FN.points){ 297 | pcl::PointXYZI pt_est; 298 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 299 | pt_est.intensity = FALSENEGATIVE; 300 | pc_out.points.push_back(pt_est); 301 | } 302 | for (auto const pt: TN.points){ 303 | pcl::PointXYZI pt_est; 304 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 305 | pt_est.intensity = TRUENEGATIVE; 306 | pc_out.points.push_back(pt_est); 307 | } 308 | pc_out.width = pc_out.points.size(); 309 | pc_out.height = 1; 310 | pcl::io::savePCDFileASCII(pcd_filename, pc_out); 311 | 312 | } 313 | 314 | 315 | #endif 316 | -------------------------------------------------------------------------------- /launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/run_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /media/demo_01.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HMX2013/FSPC-ROS/8473f0011d6e89a6abe89e5b7930a82003ad9fbb/media/demo_01.gif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ri_dbscan 4 | 1.0.0 5 | The ri_dbscan package 6 | hmx 7 | MIT 8 | 9 | catkin 10 | roscpp 11 | rospy 12 | std_msgs 13 | pcl_ros 14 | tf2_ros 15 | tf2_geometry_msgs 16 | dynamic_reconfigure 17 | jsk_recognition_msgs 18 | 19 | roscpp 20 | rospy 21 | std_msgs 22 | pcl_ros 23 | tf2_ros 24 | tf2_geometry_msgs 25 | dynamic_reconfigure 26 | jsk_recognition_msgs 27 | 28 | 29 | roscpp 30 | rospy 31 | std_msgs 32 | pcl_ros 33 | tf2_ros 34 | tf2_geometry_msgs 35 | dynamic_reconfigure 36 | autoware_msgs 37 | jsk_recognition_msgs 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /rviz/ri_dbscan.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /ground_pc1 10 | Splitter Ratio: 0.6452174186706543 11 | Tree Height: 1002 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.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: segmented_color 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: false 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: false 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | Marker Alpha: 1 61 | Marker Scale: 1 62 | Name: TF 63 | Show Arrows: true 64 | Show Axes: true 65 | Show Names: true 66 | Tree: 67 | {} 68 | Update Interval: 0 69 | Value: true 70 | - Alpha: 1 71 | Autocompute Intensity Bounds: true 72 | Autocompute Value Bounds: 73 | Max Value: 10 74 | Min Value: -10 75 | Value: true 76 | Axis: Z 77 | Channel Name: intensity 78 | Class: rviz/PointCloud2 79 | Color: 238; 238; 236 80 | Color Transformer: FlatColor 81 | Decay Time: 0 82 | Enabled: true 83 | Invert Rainbow: false 84 | Max Color: 255; 255; 255 85 | Min Color: 0; 0; 0 86 | Name: ground_pc 87 | Position Transformer: XYZ 88 | Queue Size: 10 89 | Selectable: true 90 | Size (Pixels): 3 91 | Size (m): 0.009999999776482582 92 | Style: Flat Squares 93 | Topic: /semi_kitti/ground_pc 94 | Unreliable: false 95 | Use Fixed Frame: true 96 | Use rainbow: true 97 | Value: true 98 | - Alpha: 1 99 | Autocompute Intensity Bounds: true 100 | Autocompute Value Bounds: 101 | Max Value: 10 102 | Min Value: -10 103 | Value: true 104 | Axis: Z 105 | Channel Name: intensity 106 | Class: rviz/PointCloud2 107 | Color: 255; 255; 255 108 | Color Transformer: FlatColor 109 | Decay Time: 0 110 | Enabled: false 111 | Invert Rainbow: false 112 | Max Color: 255; 255; 255 113 | Min Color: 0; 0; 0 114 | Name: map_cloud 115 | Position Transformer: XYZ 116 | Queue Size: 9 117 | Selectable: true 118 | Size (Pixels): 3 119 | Size (m): 0.029999999329447746 120 | Style: Flat Squares 121 | Topic: /laser_cloud_surround 122 | Unreliable: false 123 | Use Fixed Frame: true 124 | Use rainbow: true 125 | Value: false 126 | - Alpha: 1 127 | Autocompute Intensity Bounds: true 128 | Autocompute Value Bounds: 129 | Max Value: 10 130 | Min Value: -10 131 | Value: true 132 | Axis: Z 133 | Channel Name: intensity 134 | Class: rviz/PointCloud2 135 | Color: 255; 255; 255 136 | Color Transformer: Intensity 137 | Decay Time: 0 138 | Enabled: true 139 | Invert Rainbow: false 140 | Max Color: 255; 255; 255 141 | Min Color: 0; 0; 0 142 | Name: segmented_color 143 | Position Transformer: XYZ 144 | Queue Size: 10 145 | Selectable: true 146 | Size (Pixels): 3 147 | Size (m): 0.029999999329447746 148 | Style: Flat Squares 149 | Topic: /clustering/colored_cluster 150 | Unreliable: false 151 | Use Fixed Frame: true 152 | Use rainbow: true 153 | Value: true 154 | - Class: jsk_rviz_plugin/BoundingBoxArray 155 | Enabled: false 156 | Name: BoundingBoxArray 157 | Queue Size: 10 158 | Topic: /pca_fitting/jsk_bboxs_array 159 | Unreliable: false 160 | Value: false 161 | alpha: 0.800000011920929 162 | color: 25; 255; 0 163 | coloring: Auto 164 | line width: 0.029999999329447746 165 | only edge: true 166 | show coords: false 167 | - Buffer length: 100 168 | Class: jsk_rviz_plugin/Plotter2D 169 | Enabled: true 170 | Name: "FSPC took: [ms]" 171 | Show Value: true 172 | Topic: /clustering/time_rviz 173 | Value: true 174 | auto color change: false 175 | auto scale: true 176 | background color: 0; 0; 0 177 | backround alpha: 0 178 | border: true 179 | foreground alpha: 0.699999988079071 180 | foreground color: 25; 255; 240 181 | height: 100 182 | left: 60 183 | linewidth: 1 184 | max color: 255; 0; 0 185 | max value: 1 186 | min value: -1 187 | show caption: true 188 | text size: 12 189 | top: 80 190 | update interval: 0.03999999910593033 191 | width: 128 192 | Enabled: true 193 | Global Options: 194 | Background Color: 0; 0; 0 195 | Default Light: true 196 | Fixed Frame: map 197 | Frame Rate: 30 198 | Name: root 199 | Tools: 200 | - Class: rviz/Interact 201 | Hide Inactive Objects: true 202 | - Class: rviz/MoveCamera 203 | - Class: rviz/Select 204 | - Class: rviz/FocusCamera 205 | - Class: rviz/Measure 206 | - Class: rviz/SetInitialPose 207 | Theta std deviation: 0.2617993950843811 208 | Topic: /initialpose 209 | X std deviation: 0.5 210 | Y std deviation: 0.5 211 | - Class: rviz/SetGoal 212 | Topic: /move_base_simple/goal 213 | - Class: rviz/PublishPoint 214 | Single click: true 215 | Topic: /clicked_point 216 | Value: true 217 | Views: 218 | Current: 219 | Class: rviz/Orbit 220 | Distance: 41.01285171508789 221 | Enable Stereo Rendering: 222 | Stereo Eye Separation: 0.05999999865889549 223 | Stereo Focal Distance: 1 224 | Swap Stereo Eyes: false 225 | Value: false 226 | Field of View: 0.7853981852531433 227 | Focal Point: 228 | X: -7.163553237915039 229 | Y: -3.35589337348938 230 | Z: -1.4942269325256348 231 | Focal Shape Fixed Size: false 232 | Focal Shape Size: 0.05000000074505806 233 | Invert Z Axis: false 234 | Name: Current View 235 | Near Clip Distance: 0.009999999776482582 236 | Pitch: 0.6147976517677307 237 | Target Frame: 238 | Yaw: 3.115260124206543 239 | Saved: ~ 240 | Window Geometry: 241 | Displays: 242 | collapsed: false 243 | Height: 1308 244 | Hide Left Dock: false 245 | Hide Right Dock: true 246 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000047cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000047c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b600000040fc0100000002fb0000000800540069006d00650100000000000009b6000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000085a0000047c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 247 | Selection: 248 | collapsed: false 249 | Time: 250 | collapsed: false 251 | Tool Properties: 252 | collapsed: false 253 | Views: 254 | collapsed: true 255 | Width: 2486 256 | X: 72 257 | Y: 27 258 | -------------------------------------------------------------------------------- /src/ri_dbscan.cpp: -------------------------------------------------------------------------------- 1 | #include "ri_dbscan.h" 2 | 3 | class cloud_segmentation 4 | { 5 | private: 6 | ros::NodeHandle nh; 7 | ros::NodeHandle private_nh; 8 | 9 | cv::Mat rangeMat; // range matrix for range image 10 | cv::Mat labelMat; // label matrix for segmentaiton marking 11 | 12 | std::vector vert_angles_; 13 | std::vector index_v; 14 | std::vector runtime_vector; 15 | 16 | pcl::PointCloud::Ptr laserCloudIn; 17 | 18 | PointType nanPoint; // fill in fullCloud at each iteration 19 | 20 | std_msgs::Header cloudHeader; 21 | pcl::PointCloud::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix 22 | 23 | uint32_t *queueIndX; // array for breadth-first search process of segmentation, for speed 24 | uint32_t *queueIndY; 25 | 26 | uint32_t *allPushedIndX; // array for tracking points of a segmented object 27 | uint32_t *allPushedIndY; 28 | 29 | std::vector > neighborIterator; // neighbor iterator for segmentaiton process 30 | 31 | int labelCount; 32 | 33 | std::vector cluster_indices; 34 | 35 | std::vector > clusterIndices; 36 | std::vector > clusterIndices_ri; 37 | std::vector > gt_clusterIndices; 38 | 39 | std::vector ose_vector; 40 | std::vector use_vector; 41 | 42 | dynamic_reconfigure::Server server; 43 | dynamic_reconfigure::Server::CallbackType f; 44 | 45 | pcl::PointCloud::Ptr segment_visul; 46 | 47 | ros::Subscriber sub_lidar_points; 48 | ros::Publisher pub_colored_cluster_cloud_; 49 | ros::Publisher pub_obsdet_clusters_; 50 | ros::Publisher pub_segment_visul_; 51 | 52 | int getRowIdx(PointType pt); 53 | 54 | int getColIdx(PointType pt); 55 | 56 | void sphericalProjection(const sensor_msgs::PointCloud2::ConstPtr &laserRosCloudMsg); 57 | 58 | void range_image_search(int row, int col, std::vector &k_indices, bool &is_core_point); 59 | void RI_DBSCAN(); 60 | void calculate_index2rc(int index, int &i, int &j); 61 | void expandCluster(std::vector &curCluster, const std::vector &neighbors, 62 | std::vector &visited, std::vector &isNoise); 63 | void Mainloop(const sensor_msgs::PointCloud2::ConstPtr &lidar_points); 64 | void postSegment(obsdet_msgs::CloudClusterArray &cluster_array); 65 | 66 | void clusterIndices_Trans(); 67 | void eval_running_time(int running_time); 68 | void eval_USE(); 69 | void eval_OSE(); 70 | 71 | public: 72 | cloud_segmentation(); 73 | ~cloud_segmentation() {}; 74 | 75 | void allocateMemory(){ 76 | laserCloudIn.reset(new pcl::PointCloud()); 77 | segment_visul.reset(new pcl::PointCloud()); 78 | fullCloud.reset(new pcl::PointCloud()); 79 | 80 | fullCloud->points.resize(VERT_SCAN * HORZ_SCAN); 81 | 82 | index_v.resize(VERT_SCAN * HORZ_SCAN); 83 | queueIndX = new uint32_t[VERT_SCAN * HORZ_SCAN]; 84 | queueIndY = new uint32_t[VERT_SCAN * HORZ_SCAN]; 85 | 86 | allPushedIndX = new uint32_t[VERT_SCAN * HORZ_SCAN]; 87 | allPushedIndY = new uint32_t[VERT_SCAN * HORZ_SCAN]; 88 | } 89 | 90 | void resetParameters(){ 91 | laserCloudIn->clear(); 92 | segment_visul->clear(); 93 | std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint); 94 | 95 | labelMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32S, cv::Scalar::all(0)); 96 | rangeMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); 97 | 98 | labelCount = 1; 99 | cluster_indices.clear(); 100 | index_v.clear(); 101 | clusterIndices_ri.clear(); 102 | clusterIndices.clear(); 103 | gt_clusterIndices.clear(); 104 | } 105 | }; 106 | 107 | // Dynamic parameter server callback function 108 | void dynamicParamCallback(ri_dbscan::ri_dbscan_Config &config, uint32_t level) 109 | { 110 | // Pointcloud Filtering Parameters 111 | DETECT_MIN = config.detect_min; 112 | DETECT_MAX = config.detect_max; 113 | 114 | cvc_coef = config.cvc_coef; 115 | MinClusterSize = config.MinClusterSize; 116 | MaxClusterSize = config.MaxClusterSize; 117 | } 118 | 119 | cloud_segmentation::cloud_segmentation():private_nh("~") 120 | { 121 | allocateMemory(); 122 | 123 | /* Initialize tuning parameter */ 124 | private_nh.param("non_ground_cloud_topic", non_ground_cloud_topic_, "/semi_kitti/non_ground_pc"); 125 | ROS_INFO("non_ground_cloud_topic: %s", non_ground_cloud_topic_.c_str()); 126 | 127 | private_nh.param("output_frame", output_frame_, "velodyne_1"); 128 | ROS_INFO("output_frame: %s", output_frame_.c_str()); 129 | 130 | private_nh.param("colored_cloud_topic", colored_cloud_topic_, "/clustering/colored_cloud"); 131 | ROS_INFO("colored_cloud_topic: %s", colored_cloud_topic_.c_str()); 132 | 133 | private_nh.param("output_cluster_array_topic", output_cluster_array_topic_, "/clustering/cloudcluster_array"); 134 | ROS_INFO("output_cluster_array_topic: %s", output_cluster_array_topic_.c_str()); 135 | 136 | float resolution = (float)(MAX_VERT_ANGLE - MIN_VERT_ANGLE) / (float)(VERT_SCAN - 1); 137 | for (int i = 0; i < VERT_SCAN; i++) 138 | vert_angles_.push_back(MIN_VERT_ANGLE + i * resolution); 139 | 140 | sub_lidar_points = nh.subscribe(non_ground_cloud_topic_, 1, &cloud_segmentation::Mainloop, this); 141 | 142 | pub_colored_cluster_cloud_ = nh.advertise(colored_cloud_topic_, 1); 143 | pub_obsdet_clusters_ = nh.advertise(output_cluster_array_topic_, 1); 144 | 145 | pub_segment_visul_ = nh.advertise ("/clustering/colored_cluster", 1); 146 | 147 | time_rviz_pub_ = nh.advertise("/clustering/time_rviz", 1); 148 | 149 | // Dynamic Parameter Server & Function 150 | f = boost::bind(&dynamicParamCallback, _1, _2); 151 | server.setCallback(f); 152 | 153 | // Create point processor 154 | nanPoint.x = std::numeric_limits::quiet_NaN(); 155 | nanPoint.y = std::numeric_limits::quiet_NaN(); 156 | nanPoint.z = std::numeric_limits::quiet_NaN(); 157 | nanPoint.intensity = -1; 158 | 159 | resetParameters(); 160 | } 161 | 162 | int cloud_segmentation::getRowIdx(PointType pt) 163 | { 164 | float angle = atan2(pt.z, sqrt(pt.x * pt.x + pt.y * pt.y)) * 180 / M_PI; 165 | auto iter_geq = std::lower_bound(vert_angles_.begin(), vert_angles_.end(), angle); 166 | int row_idx; 167 | 168 | if (iter_geq == vert_angles_.begin()) 169 | { 170 | row_idx = 0; 171 | } 172 | else 173 | { 174 | float a = *(iter_geq - 1); 175 | float b = *(iter_geq); 176 | if (fabs(angle - a) < fabs(angle - b)) 177 | { 178 | row_idx = iter_geq - vert_angles_.begin() - 1; 179 | } 180 | else 181 | { 182 | row_idx = iter_geq - vert_angles_.begin(); 183 | } 184 | } 185 | return row_idx; 186 | } 187 | 188 | int cloud_segmentation::getColIdx(PointType pt) 189 | { 190 | float horizonAngle = atan2(pt.x, pt.y) * 180 / M_PI; 191 | static float ang_res_x = 360.0 / float(HORZ_SCAN); 192 | int col_idx = -round((horizonAngle - 90.0) / ang_res_x) + HORZ_SCAN / 2; 193 | if (col_idx >= HORZ_SCAN) 194 | col_idx -= HORZ_SCAN; 195 | return col_idx; 196 | } 197 | 198 | void cloud_segmentation::range_image_search(int row, int col, std::vector &k_indices, bool &is_core_point) 199 | { 200 | k_indices.clear(); 201 | k_indices.push_back(col + row * HORZ_SCAN); 202 | 203 | float dist, cvc_dist, d_criteria; 204 | 205 | int fromIndX, fromIndY, thisIndX, thisIndY; 206 | 207 | fromIndX = row; 208 | fromIndY = col; 209 | 210 | std::pair neighbor; 211 | 212 | // d_criteria = cvc_coef * rangeMat.at(fromIndX, fromIndY) * (neighbor_col * 0.08) * M_PI / 180; 213 | 214 | d_criteria = 0.4; 215 | 216 | neighborIterator.clear(); 217 | 218 | int neighbor_col = 10; 219 | 220 | for (int8_t i = -2; i <= 2; i++) 221 | { 222 | for (int8_t j = -neighbor_col; j <= neighbor_col; j++) 223 | { 224 | neighbor.first = i; 225 | neighbor.second = j; 226 | neighborIterator.push_back(neighbor); 227 | } 228 | } 229 | 230 | // Loop through all the neighboring grids of popped grid 231 | for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) 232 | { 233 | // new index 234 | thisIndX = fromIndX + (*iter).first; 235 | thisIndY = fromIndY + (*iter).second; 236 | 237 | // index should be within the boundary 238 | if (thisIndX < 0 || thisIndX >= VERT_SCAN) 239 | continue; 240 | 241 | // at range image margin (left or right side) 242 | if (thisIndY < 0) 243 | thisIndY = HORZ_SCAN - 1; 244 | if (thisIndY >= HORZ_SCAN) 245 | thisIndY = 0; 246 | 247 | // prevent infinite loop (caused by put already examined point back) 248 | if (labelMat.at(thisIndX, thisIndY) != -1) 249 | continue; 250 | 251 | cvc_dist = abs(rangeMat.at(thisIndX, thisIndY) - rangeMat.at(fromIndX, fromIndY)); 252 | 253 | if (cvc_dist < d_criteria) 254 | { 255 | k_indices.push_back(thisIndY + thisIndX * HORZ_SCAN); 256 | } 257 | } 258 | 259 | if (k_indices.size() > 10) 260 | is_core_point = true; 261 | } 262 | 263 | 264 | void cloud_segmentation::expandCluster(std::vector &curCluster, const std::vector &neighbors, 265 | std::vector &visited, std::vector &isNoise) 266 | { 267 | int i_, j_; 268 | bool is_core_point; 269 | std::deque neighborDeque(neighbors.begin(), neighbors.end()); 270 | 271 | while (!neighborDeque.empty()) { 272 | int curIdx = neighborDeque.front(); 273 | neighborDeque.pop_front(); 274 | 275 | if (isNoise[curIdx]) { 276 | curCluster.emplace_back(curIdx); 277 | continue; 278 | } 279 | 280 | if (!visited[curIdx]) { 281 | visited[curIdx] = true; 282 | curCluster.emplace_back(curIdx); 283 | 284 | std::vector curNeighbors; 285 | std::vector nn_distances; 286 | 287 | calculate_index2rc(curIdx, i_, j_); 288 | 289 | range_image_search(i_, j_, curNeighbors, is_core_point); 290 | 291 | if (!is_core_point){ 292 | continue; 293 | } 294 | 295 | std::copy(curNeighbors.begin(), curNeighbors.end(), std::back_inserter(neighborDeque)); 296 | } 297 | } 298 | } 299 | 300 | // segmentation process 301 | void cloud_segmentation::RI_DBSCAN() 302 | { 303 | std::vector core_pt_indices; 304 | bool is_core_point = false; 305 | 306 | std::vector visited(VERT_SCAN * HORZ_SCAN, false); 307 | std::vector isNoise(VERT_SCAN * HORZ_SCAN, false); 308 | 309 | for (size_t i = 0; i < VERT_SCAN; ++i) 310 | { 311 | for (size_t j = 0; j < HORZ_SCAN; ++j) 312 | { 313 | int index_ = j + i * HORZ_SCAN; 314 | 315 | if (visited[index_]){ 316 | continue; 317 | } 318 | 319 | visited[index_] = true; 320 | 321 | if (labelMat.at(i, j) == -1){ 322 | range_image_search(i, j, core_pt_indices, is_core_point); 323 | } 324 | else{continue;}; 325 | 326 | if (!is_core_point){ 327 | isNoise[index_] = true; 328 | } 329 | else{ 330 | clusterIndices_ri.emplace_back(std::vector{static_cast(index_)}); 331 | expandCluster(clusterIndices_ri.back(), core_pt_indices, visited, isNoise); 332 | } 333 | } 334 | } 335 | } 336 | 337 | void cloud_segmentation::calculate_index2rc(int index, int &r, int &c){ 338 | int j; 339 | for (int i = 0; i < VERT_SCAN; ++i) 340 | { 341 | j = index - i * HORZ_SCAN; 342 | if (j <= HORZ_SCAN){ 343 | r=i; 344 | c=j; 345 | break; 346 | } 347 | } 348 | } 349 | 350 | void cloud_segmentation::postSegment(obsdet_msgs::CloudClusterArray &cluster_array) 351 | { 352 | int intensity_mark = 1; 353 | 354 | pcl::PointXYZI cluster_color; 355 | 356 | obsdet_msgs::CloudCluster cluster_pc; 357 | 358 | sensor_msgs::PointCloud2 cloud_msg; 359 | pcl::PointCloud::Ptr cluster_pcl(new pcl::PointCloud); 360 | 361 | for (auto &getIndices : clusterIndices) 362 | { 363 | if (getIndices.size() < 10) 364 | continue; 365 | 366 | cluster_pcl->clear(); 367 | for (auto &index : getIndices) 368 | { 369 | cluster_color.x = laserCloudIn->points[index].x; 370 | cluster_color.y = laserCloudIn->points[index].y; 371 | cluster_color.z = laserCloudIn->points[index].z; 372 | cluster_color.intensity = intensity_mark; 373 | segment_visul->push_back(cluster_color); 374 | 375 | cluster_pcl->push_back(cluster_color); 376 | } 377 | intensity_mark++; 378 | 379 | cloud_msg.header = ros_header; 380 | pcl::toROSMsg(*cluster_pcl, cloud_msg); 381 | 382 | cluster_pc.header = ros_header; 383 | cluster_pc.cloud = cloud_msg; 384 | cluster_array.clusters.push_back(cluster_pc); 385 | } 386 | 387 | sensor_msgs::PointCloud2 segment_visul_ros; 388 | pcl::toROSMsg(*segment_visul, segment_visul_ros); 389 | segment_visul_ros.header = ros_header; 390 | pub_segment_visul_.publish(segment_visul_ros); 391 | 392 | cluster_array.header = ros_header; 393 | pub_obsdet_clusters_.publish(cluster_array); 394 | } 395 | 396 | 397 | void cloud_segmentation::sphericalProjection(const sensor_msgs::PointCloud2::ConstPtr &laserRosCloudMsg) 398 | { 399 | float range; 400 | size_t rowIdn, columnIdn, index, cloudSize; 401 | PointType curpt; 402 | 403 | pcl::PointCloud::Ptr raw_pcl(new pcl::PointCloud); 404 | 405 | cloudHeader = laserRosCloudMsg->header; 406 | cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line 407 | pcl::fromROSMsg(*laserRosCloudMsg, *raw_pcl); 408 | 409 | cloudSize = raw_pcl->points.size(); 410 | 411 | int oindex = 0; 412 | laserCloudIn->clear(); 413 | 414 | for (size_t i = 0; i < cloudSize; ++i) 415 | { 416 | curpt.x = raw_pcl->points[i].x; 417 | curpt.y = raw_pcl->points[i].y; 418 | curpt.z = raw_pcl->points[i].z; 419 | curpt.intensity = raw_pcl->points[i].intensity; 420 | curpt.ring = raw_pcl->points[i].ring; 421 | curpt.id = raw_pcl->points[i].id; 422 | curpt.label = raw_pcl->points[i].label; 423 | curpt.cid = 9999; 424 | 425 | bool is_nan = std::isnan(curpt.x) || std::isnan(curpt.y) || std::isnan(curpt.z); 426 | if (is_nan) 427 | continue; 428 | 429 | //find the row and column index in the iamge for this point 430 | rowIdn = getRowIdx(curpt); 431 | 432 | if (rowIdn < 0 || rowIdn >= VERT_SCAN) 433 | continue; 434 | 435 | columnIdn = getColIdx(curpt); 436 | 437 | if (columnIdn < 0 || columnIdn >= HORZ_SCAN) 438 | continue; 439 | 440 | range = sqrt(curpt.x * curpt.x + curpt.y * curpt.y + curpt.z * curpt.z); 441 | 442 | labelMat.at(rowIdn, columnIdn) = -1; 443 | rangeMat.at(rowIdn, columnIdn) = range; 444 | 445 | index = columnIdn + rowIdn * HORZ_SCAN; 446 | 447 | fullCloud->points[index] = curpt; 448 | 449 | index_v[index] = oindex; 450 | 451 | laserCloudIn->points.push_back(curpt); 452 | oindex++; 453 | } 454 | } 455 | 456 | void cloud_segmentation::clusterIndices_Trans() 457 | { 458 | int row, col; 459 | std::vector clusterIndice; 460 | 461 | for (auto &getIndices : clusterIndices_ri) 462 | { 463 | clusterIndice.clear(); 464 | for (auto &index : getIndices) 465 | { 466 | calculate_index2rc(index, row, col); 467 | clusterIndice.push_back(index_v[col + row * HORZ_SCAN]); 468 | } 469 | clusterIndices.push_back(clusterIndice); 470 | } 471 | } 472 | 473 | void cloud_segmentation::eval_OSE() 474 | { 475 | double ose_i = 0.0; 476 | 477 | int cluster_id = 0; 478 | 479 | for (auto &getIndices : clusterIndices) 480 | { 481 | for (auto &index : getIndices) 482 | { 483 | laserCloudIn->points[index].cid = cluster_id; 484 | } 485 | cluster_id++; 486 | } 487 | 488 | for (auto &getIndices : gt_clusterIndices) 489 | { 490 | int object_cluster[clusterIndices.size()] = {0}; 491 | int N = getIndices.size(); 492 | 493 | for (auto &index : getIndices) 494 | { 495 | if (laserCloudIn->points[index].cid != 9999) 496 | object_cluster[laserCloudIn->points[index].cid]++; 497 | } 498 | 499 | for (size_t i = 0; i < clusterIndices.size(); i++) 500 | { 501 | if (object_cluster[i] == 0) 502 | continue; 503 | 504 | double ose_ii = -(object_cluster[i] / (1.0*N)) * log(object_cluster[i] / (1.0*N)); 505 | 506 | ose_i += ose_ii; 507 | } 508 | } 509 | 510 | ose_vector.push_back(ose_i); 511 | 512 | double ose_total_v = 0.0; 513 | double ose_sqr_sum = 0.0; 514 | double ose_mean; 515 | double ose_std; 516 | 517 | for (size_t i = 0; i < ose_vector.size(); i++) 518 | { 519 | ose_total_v += ose_vector[i]; 520 | } 521 | ose_mean = ose_total_v / ose_vector.size(); 522 | 523 | for (size_t i = 0; i < ose_vector.size(); i++) 524 | { 525 | ose_sqr_sum += (ose_vector[i] - ose_mean) * (ose_vector[i] - ose_mean); 526 | } 527 | ose_std = sqrt(ose_sqr_sum / ose_vector.size()); 528 | 529 | std::cout << "current ose_i is = " << ose_i << std::endl; 530 | std::cout << "\033[1;34mose_mean is = " << ose_mean << "\033[0m" << std::endl; 531 | std::cout << "ose_std is = " << ose_std << std::endl; 532 | } 533 | 534 | void cloud_segmentation::eval_USE() 535 | { 536 | std::vector cluater_label; 537 | std::vector > cluaters_label; 538 | 539 | int label[34] = {0, 1, 10, 11, 13, 15, 16, 18, 20, 30, 31, 32, 40, 44, 48, 49, 50, 51, 540 | 52, 60, 70, 71, 72, 80, 81, 99, 252, 253, 254, 255, 256, 257, 258, 259}; 541 | 542 | double use_i_sum = 0; 543 | 544 | for (auto& getIndices : clusterIndices) 545 | { 546 | int cluster_label[34] = {0}; 547 | for (auto& index : getIndices) 548 | { 549 | for (size_t i = 0; i < 34; i++) 550 | { 551 | if (laserCloudIn->points[index].label == label[i]) 552 | cluster_label[i]++; 553 | } 554 | } 555 | 556 | int M = getIndices.size(); 557 | 558 | for (size_t i = 0; i < 34; i++) 559 | { 560 | if (cluster_label[i] == 0) 561 | continue; 562 | 563 | double use_i = -(cluster_label[i] / (M * 1.0)) * log(cluster_label[i] / (M * 1.0)); 564 | use_i_sum += use_i; 565 | } 566 | } 567 | 568 | use_vector.push_back(use_i_sum); 569 | 570 | double use_total_v = 0.0; 571 | double use_sqr_sum = 0.0; 572 | double use_mean; 573 | double use_std; 574 | 575 | for (size_t i = 0; i < use_vector.size(); i++) 576 | { 577 | use_total_v += use_vector[i]; 578 | } 579 | use_mean = use_total_v / use_vector.size(); 580 | 581 | for (size_t i = 0; i < use_vector.size(); i++) 582 | { 583 | use_sqr_sum += (use_vector[i] - use_mean) * (use_vector[i] - use_mean); 584 | } 585 | 586 | use_std = sqrt(use_sqr_sum / use_vector.size()); 587 | 588 | std::cout << "current use_i is = " << use_i_sum << std::endl; 589 | std::cout << "\033[1;32muse_mean is = " << use_mean << "\033[0m" << std::endl; 590 | std::cout << "use_std is = " << use_std << std::endl; 591 | } 592 | 593 | 594 | void cloud_segmentation::eval_running_time(int running_time) 595 | { 596 | double runtime_std; 597 | double runtime_aver; 598 | double runtime_total_v = 0.0; 599 | double runtime_sqr_sum = 0.0; 600 | 601 | runtime_vector.push_back(running_time); 602 | 603 | for (size_t i = 0; i < runtime_vector.size(); i++) 604 | { 605 | runtime_total_v += runtime_vector[i]; 606 | } 607 | 608 | runtime_aver = runtime_total_v / runtime_vector.size(); 609 | 610 | for (size_t i = 0; i < runtime_vector.size(); i++) 611 | { 612 | runtime_sqr_sum += (runtime_vector[i] - runtime_aver) * (runtime_vector[i] - runtime_aver); 613 | } 614 | 615 | runtime_std = sqrt(runtime_sqr_sum / runtime_vector.size()); 616 | 617 | std::cout << "runtime_vector.size() is = " << runtime_vector.size() << std::endl; 618 | std::cout << "current running_time is = " << running_time << "ms" << std::endl; 619 | std::cout << "\033[1;36mruntime_aver is = " << runtime_aver << "ms" 620 | << "\033[0m" << std::endl; 621 | std::cout << "runtime_std is = " << runtime_std << "ms" << std::endl; 622 | } 623 | 624 | 625 | void cloud_segmentation::Mainloop(const sensor_msgs::PointCloud2::ConstPtr& lidar_points) 626 | { 627 | ros_header = lidar_points->header; 628 | 629 | gt_verify.reset(new groundtruth::DepthCluster()); 630 | 631 | const auto start_time = std::chrono::steady_clock::now(); 632 | 633 | sphericalProjection(lidar_points); 634 | 635 | RI_DBSCAN(); 636 | 637 | // Time the whole process 638 | const auto end_time = std::chrono::steady_clock::now(); 639 | const auto elapsed_time = std::chrono::duration_cast(end_time - start_time); 640 | eval_running_time(elapsed_time.count()); 641 | 642 | // get the ground truth 643 | gt_verify->GTV(laserCloudIn, gt_clusterIndices); 644 | clusterIndices_Trans(); 645 | 646 | eval_OSE(); // evaluate over-segmentation 647 | eval_USE(); // evaluate under-segmentation 648 | 649 | //visualization, use indensity to show different color for each cluster. 650 | obsdet_msgs::CloudClusterArray cluster_array; 651 | postSegment(cluster_array); 652 | 653 | resetParameters(); 654 | 655 | // Time the whole process 656 | const auto exe_time = std::chrono::duration_cast(end_time - start_time).count() / 1000.0; 657 | time_rviz.data = exe_time; 658 | time_rviz_pub_.publish(time_rviz); 659 | std::cout << "---------------------------------" << std::endl; 660 | } 661 | 662 | int main(int argc, char** argv) 663 | { 664 | ros::init(argc, argv, "spc_node"); 665 | 666 | tf::StampedTransform transform; 667 | tf::TransformListener listener; 668 | 669 | _transform = &transform; 670 | _transform_listener = &listener; 671 | 672 | cloud_segmentation cloud_segmentation_node; 673 | 674 | ros::spin(); 675 | 676 | return 0; 677 | } --------------------------------------------------------------------------------