├── media
    └── demo_01.gif
├── cfg
    └── ri_dbscan.cfg
├── CHANGELOG.rst
├── launch
    ├── run_rviz.launch
    └── run.launch
├── CMakeLists.txt
├── package.xml
├── README.md
├── include
    ├── ri_dbscan.h
    ├── tools
    │   └── utils.hpp
    └── ground_truth.hpp
├── rviz
    └── ri_dbscan.rviz
└── src
    └── ri_dbscan.cpp


/media/demo_01.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HMX2013/FSPC-ROS/HEAD/media/demo_01.gif


--------------------------------------------------------------------------------
/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_"))


--------------------------------------------------------------------------------
/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 | * 


--------------------------------------------------------------------------------
/launch/run_rviz.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <node pkg="ri_dbscan" type="ri_dbscan_node" name="ri_dbscan_node" output="screen">
 5 |     <param name="non_ground_cloud_topic"        value="/semi_kitti/non_ground_pc"/>
 6 |     <param name="output_frame"                  value="map"/>
 7 | 
 8 |     <param name="output_cluster_array_topic"  value="/clustering/cluster_array"/>
 9 |     <param name="colored_cloud_topic"           value="/clustering/colored_cloud"/>
10 |   </node>
11 | 
12 |   <!-- Dynamic Reconfigure GUI -->
13 |   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen" />
14 | 
15 |   <!-- Rviz -->
16 |   <node type="rviz" name="rviz" pkg="rviz" args="-d $(find ri_dbscan)/rviz/ri_dbscan.rviz" output="log" respawn="false" />
17 | 
18 | </launch> 


--------------------------------------------------------------------------------
/launch/run.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <node pkg="ri_dbscan" type="ri_dbscan_node" name="ri_dbscan_node" output="screen">
 5 |     <param name="non_ground_cloud_topic"        value="/semi_kitti/non_ground_pc"/>
 6 |     <param name="output_frame"                  value="map"/>
 7 | 
 8 |     <param name="output_cluster_array_topic"  value="/clustering/cluster_array"/>
 9 |     <param name="colored_cloud_topic"           value="/clustering/colored_cloud"/>
10 |   </node>
11 | 
12 |   <!-- Dynamic Reconfigure GUI -->
13 |   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen" />
14 | 
15 |   <!-- Rviz -->
16 |   <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find ri_dbscan)/rviz/ri_dbscan.rviz" output="log" respawn="false" /> -->
17 | 
18 | </launch> 


--------------------------------------------------------------------------------
/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 | )


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>ri_dbscan</name>
 4 |   <version>1.0.0</version>
 5 |   <description>The ri_dbscan package</description>
 6 |   <maintainer email="hmx_2013@outlook.com">hmx</maintainer>
 7 |   <license>MIT</license>
 8 | 
 9 |   <buildtool_depend>catkin</buildtool_depend>
10 |   <build_depend>roscpp</build_depend>
11 |   <build_depend>rospy</build_depend>
12 |   <build_depend>std_msgs</build_depend>
13 |   <build_depend>pcl_ros</build_depend>
14 |   <build_depend>tf2_ros</build_depend>
15 |   <build_depend>tf2_geometry_msgs</build_depend>
16 |   <build_depend>dynamic_reconfigure</build_depend>
17 |   <build_depend>jsk_recognition_msgs</build_depend>
18 | 
19 |   <build_export_depend>roscpp</build_export_depend>
20 |   <build_export_depend>rospy</build_export_depend>
21 |   <build_export_depend>std_msgs</build_export_depend>
22 |   <build_export_depend>pcl_ros</build_export_depend>
23 |   <build_export_depend>tf2_ros</build_export_depend>
24 |   <build_export_depend>tf2_geometry_msgs</build_export_depend>
25 |   <build_export_depend>dynamic_reconfigure</build_export_depend>
26 |   <build_export_depend>jsk_recognition_msgs</build_export_depend>
27 | 
28 | 
29 |   <exec_depend>roscpp</exec_depend>
30 |   <exec_depend>rospy</exec_depend>
31 |   <exec_depend>std_msgs</exec_depend>
32 |   <exec_depend>pcl_ros</exec_depend>
33 |   <exec_depend>tf2_ros</exec_depend>
34 |   <exec_depend>tf2_geometry_msgs</exec_depend>  
35 |   <exec_depend>dynamic_reconfigure</exec_depend>
36 |   <exec_depend>autoware_msgs</exec_depend>
37 |   <exec_depend>jsk_recognition_msgs</exec_depend>
38 | 
39 | 
40 |   <!-- The export tag contains other, unspecified, tags -->
41 |   <export>
42 |     <!-- Other tools can request additional information be placed here -->
43 | 
44 |   </export>
45 | </package>
46 | 


--------------------------------------------------------------------------------
/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 | 


--------------------------------------------------------------------------------
/include/ri_dbscan.h:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include <ros/console.h>
 3 | #include <std_msgs/Header.h>
 4 | #include <geometry_msgs/PoseStamped.h>
 5 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 6 | #include <tf2/LinearMath/Quaternion.h>
 7 | 
 8 | #include <jsk_recognition_msgs/BoundingBox.h>
 9 | #include <jsk_recognition_msgs/BoundingBoxArray.h>
10 | 
11 | #include <pcl_ros/point_cloud.h>
12 | #include <pcl_conversions/pcl_conversions.h>
13 | 
14 | #include <pcl/segmentation/extract_clusters.h>
15 | #include <pcl_ros/transforms.h>
16 | 
17 | #include <Eigen/Dense>
18 | #include <cmath>
19 | #include <algorithm>
20 | 
21 | #include <opencv2/core/version.hpp>
22 | 
23 | #if (CV_MAJOR_VERSION == 3)
24 | #include<opencv/cv.h>
25 | #else
26 | #include <opencv2/imgproc.hpp>
27 | #endif
28 | 
29 | #include <std_msgs/Float32.h>
30 | #include <chrono>
31 | #include <dynamic_reconfigure/server.h>
32 | #include <ri_dbscan/ri_dbscan_Config.h>
33 | 
34 | #include "obsdet_msgs/CloudCluster.h"
35 | #include "obsdet_msgs/CloudClusterArray.h"
36 | 
37 | #include <tf/transform_listener.h>
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<groundtruth::DepthCluster<PointType>> 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;


--------------------------------------------------------------------------------
/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: <Fixed 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: <Fixed 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 | 


--------------------------------------------------------------------------------
/include/tools/utils.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef COMMON_H
  2 | #define COMMON_H
  3 | 
  4 | #include "math.h"
  5 | #include <sstream>
  6 | #include <vector>
  7 | #include <map>
  8 | #include <iostream>
  9 | #include <experimental/filesystem>
 10 | 
 11 | #include <tf/tf.h>
 12 | 
 13 | #include <pcl/common/common.h>
 14 | #include <pcl/common/transforms.h>
 15 | #include <pcl_ros/point_cloud.h>
 16 | #include <pcl/filters/filter.h>
 17 | #include <pcl/point_types.h>
 18 | #include <pcl/common/centroid.h>
 19 | #include <pcl_conversions/pcl_conversions.h>
 20 | #include <pcl/filters/voxel_grid.h>
 21 | #include <pcl/filters/passthrough.h>
 22 | #include <pcl/kdtree/kdtree_flann.h>
 23 | #include <pcl/filters/extract_indices.h>
 24 | #include <pcl/io/pcd_io.h>
 25 | #include <boost/format.hpp>
 26 | #include <fstream>
 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<PointXYZILID>& src,
 79 |                        pcl::PointCloud<pcl::PointXYZI>::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<int> outlier_classes = {UNLABELED, OUTLIER};
 91 | std::vector<int> ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN};
 92 | std::vector<int> ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING};
 93 | 
 94 | int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc){
 95 |   int num_ground = 0;
 96 | 
 97 |   std::vector<int>::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<int, int> set_initial_gt_counts(std::vector<int>& gt_classes){
113 |   map<int, int> gt_counts;
114 |   for (int i = 0; i< gt_classes.size(); ++i){
115 |     gt_counts.insert(pair<int,int>(gt_classes.at(i), 0));
116 |   }
117 |   return gt_counts;
118 | }
119 | 
120 | std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZILID>& pc){
121 |   int num_ground = 0;
122 |   auto gt_counts = set_initial_gt_counts(ground_classes);
123 |   std::vector<int>::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<PointXYZILID>& pc){
139 |   int num_outliers = 0;
140 | 
141 |   std::vector<int>::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<PointXYZILID>& src, pcl::PointCloud<PointXYZILID>& ground, pcl::PointCloud<PointXYZILID>& non_ground){
154 |   ground.clear();
155 |   non_ground.clear();
156 |   std::vector<int>::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<PointXYZILID>& pc_curr,
174 |                                 pcl::PointCloud<PointXYZILID>& 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<PointXYZILID>& 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<int> 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<<labels[i]<<",";
240 |     }else{
241 |       sc_output<<labels[i]<<endl;
242 |     }
243 |   }
244 |   sc_output.close();
245 | }
246 | 
247 | void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr,
248 |                       pcl::PointCloud<PointXYZILID>& ground_estimated, string acc_filename,
249 |                       double& accuracy, map<int, int>&pc_curr_gt_counts, map<int, int>&g_est_gt_counts){
250 | 
251 | 
252 | //  std::cout<<"debug: "<<acc_filename<<std::endl;
253 |   ofstream sc_output2(acc_filename, ios::app);
254 | 
255 |   int num_True = count_num_ground(pc_curr);
256 |   int num_outliers_gt = count_num_outliers(pc_curr);
257 |   int num_outliers_est = count_num_outliers(ground_estimated);
258 | 
259 |   int num_total_est = ground_estimated.points.size() - num_outliers_est;
260 |   int num_total_gt = pc_curr.points.size() - num_outliers_gt;
261 | 
262 |   int num_False = num_total_gt - num_True;
263 |   int num_TP = count_num_ground(ground_estimated);
264 |   int num_FP = num_total_est - num_TP;
265 |   accuracy = static_cast<double>(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<<g_est_gt_counts.find(class_id)->second<<","<<pc_curr_gt_counts.find(class_id)->second<<",";
273 |   }
274 |   sc_output2<<accuracy<<endl;
275 | 
276 |   sc_output2.close();
277 | }
278 | 
279 | void pc2pcdfile(const pcl::PointCloud<PointXYZILID>& TP, const pcl::PointCloud<PointXYZILID>& FP,
280 |                 const pcl::PointCloud<PointXYZILID>& FN, const pcl::PointCloud<PointXYZILID>& TN,
281 |                 std::string pcd_filename){
282 |   pcl::PointCloud<pcl::PointXYZI> 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 | 


--------------------------------------------------------------------------------
/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 <vector>
  7 | #include <algorithm>
  8 | #include <iostream>
  9 | #include <list>
 10 | #include <numeric>
 11 | #include <random>
 12 | #include <chrono>
 13 | #include <forward_list>
 14 | #include <boost/optional.hpp>
 15 | 
 16 | #include <ros/ros.h>
 17 | #include <pcl/point_cloud.h>
 18 | #include <pcl/point_types.h>
 19 | #include <pcl/common/io.h>
 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 <typename PointT>
 37 |     class DepthCluster {
 38 |         private:
 39 |             uint16_t max_label_;
 40 | 
 41 |             std::vector<float> vert_angles_;
 42 |             std::vector<std::vector<int>> clusterIndices;
 43 | 
 44 |             std::vector<int> 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<std::pair<int8_t, int8_t> > 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<PointT>::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<PointT>());
 70 |                 fullCloud->points.resize(VERT_SCAN*HORZ_SCAN);
 71 |                 index_v.resize(VERT_SCAN * HORZ_SCAN);
 72 |                 nanPoint.x = std::numeric_limits<float>::quiet_NaN();
 73 |                 nanPoint.y = std::numeric_limits<float>::quiet_NaN();
 74 |                 nanPoint.z = std::numeric_limits<float>::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<int8_t, int8_t> 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<int> _min_cluster_size=boost::none, 
114 |                             boost::optional<int> _max_cluster_size=boost::none) {
115 |    
116 |                 std::cout<<""<<std::endl;
117 |                 ROS_WARN("Set ObjectSeg Parameters");
118 | 
119 |                 // range_mat_.resize(VERT_SCAN, vector<Point>(HORZ_SCAN));
120 | 
121 |                 // valid_cnt_.resize(VERT_SCAN, 0);
122 |                 // nodes_.resize(VERT_SCAN, vector<AOSNode>());
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<pcl::PointCloud<PointT>> cloud_in, std::vector<std::vector<int>> &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<int>(i, j) == -1)
146 |                             labelComponents(i, j, clusterIndices);
147 |                     }
148 |                 }
149 |                 // return clusterIndices;
150 |             }
151 | 
152 |             void sphericalProjection(boost::shared_ptr<pcl::PointCloud<PointT>> 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<int>(rowIdn, columnIdn) = -1;
188 |                     rangeMat.at<float>(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<std::vector<int>> &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<int> 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<int>(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<int>(thisIndX, thisIndY) != -1)
281 |                             continue;
282 | 
283 |                         /*---------------------------condition----------------------------*/
284 |                         d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
285 |                                         rangeMat.at<float>(thisIndX, thisIndY));
286 |                         d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
287 |                                         rangeMat.at<float>(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<int>(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<int>(allPushedIndX[i], allPushedIndY[i]) = 0;
343 |                     }
344 |                 }
345 |             }
346 |     };
347 | }
348 | 
349 | #endif


--------------------------------------------------------------------------------
/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<float> vert_angles_;
 13 |   std::vector<int> index_v;
 14 |   std::vector<int> runtime_vector;
 15 | 
 16 |   pcl::PointCloud<PointType>::Ptr laserCloudIn;
 17 | 
 18 |   PointType nanPoint; // fill in fullCloud at each iteration
 19 | 
 20 |   std_msgs::Header cloudHeader;
 21 |   pcl::PointCloud<PointType>::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<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
 30 | 
 31 |   int labelCount;
 32 | 
 33 |   std::vector<pcl::PointIndices> cluster_indices;
 34 |   
 35 |   std::vector<std::vector<int> > clusterIndices;
 36 |   std::vector<std::vector<int> > clusterIndices_ri;
 37 |   std::vector<std::vector<int> > gt_clusterIndices;  
 38 | 
 39 |   std::vector<double> ose_vector;
 40 |   std::vector<double> use_vector;
 41 | 
 42 |   dynamic_reconfigure::Server<ri_dbscan::ri_dbscan_Config> server;
 43 |   dynamic_reconfigure::Server<ri_dbscan::ri_dbscan_Config>::CallbackType f;
 44 | 
 45 |   pcl::PointCloud<pcl::PointXYZI>::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<int> &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<int> &curCluster, const std::vector<int> &neighbors,
 62 |                      std::vector<char> &visited, std::vector<char> &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<PointType>());
 77 |     segment_visul.reset(new pcl::PointCloud<pcl::PointXYZI>());
 78 |     fullCloud.reset(new pcl::PointCloud<PointType>());
 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<std::string>("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<std::string>("output_frame", output_frame_, "velodyne_1");
128 |   ROS_INFO("output_frame: %s", output_frame_.c_str());
129 | 
130 |   private_nh.param<std::string>("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<std::string>("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<sensor_msgs::PointCloud2>(colored_cloud_topic_, 1);
143 |   pub_obsdet_clusters_ = nh.advertise<obsdet_msgs::CloudClusterArray >(output_cluster_array_topic_, 1);
144 | 
145 |   pub_segment_visul_ = nh.advertise<sensor_msgs::PointCloud2> ("/clustering/colored_cluster", 1);
146 | 
147 |   time_rviz_pub_ = nh.advertise<std_msgs::Float32>("/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<float>::quiet_NaN();
155 |   nanPoint.y = std::numeric_limits<float>::quiet_NaN();
156 |   nanPoint.z = std::numeric_limits<float>::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<int> &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<int8_t, int8_t> neighbor;
211 | 
212 |   // d_criteria = cvc_coef * rangeMat.at<float>(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<int>(thisIndX, thisIndY) != -1)
249 |       continue;
250 | 
251 |     cvc_dist = abs(rangeMat.at<float>(thisIndX, thisIndY) - rangeMat.at<float>(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<int> &curCluster, const std::vector<int> &neighbors,
265 |                                        std::vector<char> &visited, std::vector<char> &isNoise)
266 | {
267 |   int i_, j_;
268 |   bool is_core_point;
269 |   std::deque<int> 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<int> curNeighbors;
285 |       std::vector<float> 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<int> core_pt_indices;
304 |   bool is_core_point = false;
305 | 
306 |   std::vector<char> visited(VERT_SCAN * HORZ_SCAN, false);
307 |   std::vector<char> 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<int>(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<int>{static_cast<int>(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<pcl::PointXYZI>::Ptr cluster_pcl(new pcl::PointCloud<pcl::PointXYZI>);
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<PointXYZILID>::Ptr raw_pcl(new pcl::PointCloud<PointXYZILID>);
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<int>(rowIdn, columnIdn) = -1;
443 |     rangeMat.at<float>(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<int> 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<int> cluater_label;
537 |   std::vector<std::vector<int> > 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<PointType>());
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<std::chrono::milliseconds>(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<std::chrono::microseconds>(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 | }


--------------------------------------------------------------------------------