├── media
    └── demo_01.gif
├── cfg
    └── cvc_ros.cfg
├── launch
    └── run_rviz.launch
├── CMakeLists.txt
├── package.xml
├── README.md
├── config
    └── semantic-kitti-all.yaml
├── rviz
    └── cvc_rviz.rviz
├── src
    ├── cvc.hpp
    └── cvc_ros_node.cpp
└── include
    ├── tools
        └── utils.hpp
    └── ground_truth.hpp


/media/demo_01.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/morte2025/CVC-ROS/HEAD/media/demo_01.gif


--------------------------------------------------------------------------------
/cfg/cvc_ros.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | PACKAGE = "cvc_ros"
 4 | 
 5 | from dynamic_reconfigure.parameter_generator_catkin import *
 6 | 
 7 | gen = ParameterGenerator()
 8 | 
 9 | gen.add("delta_a",             double_t, 0, "Default: 0.4",    0.4,  0.1,  3.0)
10 | gen.add("delta_p",             double_t, 0, "Default: 1.2",    1.2,  1.0,  12)
11 | gen.add("delta_r",             double_t, 0, "Default: 0.2",    0.2,  0.1,  2)
12 | 
13 | exit(gen.generate(PACKAGE, "cvc_ros", "cvc_ros_"))


--------------------------------------------------------------------------------
/launch/run_rviz.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 |   <node name="cvc_ros" pkg="cvc_ros" type="cvc_ros_node" output="screen">
 4 |     <param name="non_ground_cloud"                value="/semi_kitti/non_ground_pc"/>
 5 |     <param name="output_frame"                     value="map"/>
 6 |   </node>
 7 | 
 8 |   <!-- Dynamic Reconfigure GUI -->
 9 |   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen" />
10 | 
11 |   <!-- Rviz -->
12 |   <node type="rviz" name="rviz" pkg="rviz" args="-d $(find cvc_ros)/rviz/cvc_rviz.rviz" output="log" respawn="true" />
13 | 
14 | </launch> 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(cvc_ros)
 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 |   autoware_msgs
20 |   jsk_recognition_msgs
21 | )
22 | 
23 | find_package(OpenCV REQUIRED)
24 | 
25 | set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
26 | 
27 | generate_dynamic_reconfigure_options(
28 |   cfg/cvc_ros.cfg
29 | )
30 | 
31 | catkin_package(
32 |   INCLUDE_DIRS include
33 |   LIBRARIES cvc_ros
34 |   CATKIN_DEPENDS roscpp std_msgs
35 | )
36 | 
37 | include_directories(
38 |   include
39 |   ${catkin_INCLUDE_DIRS}
40 |   ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake
41 |   ${OpenCV_INCLUDE_DIRS}
42 | )
43 | 
44 | link_directories(${OpenCV_LIBRARY_DIRS})
45 | 
46 | add_executable(cvc_ros_node
47 |   src/cvc_ros_node.cpp
48 | )
49 | 
50 | add_dependencies(cvc_ros_node 
51 |   ${${PROJECT_NAME}_EXPORTED_TARGETS} 
52 |   ${catkin_EXPORTED_TARGETS}
53 | )
54 | 
55 | target_link_libraries(cvc_ros_node
56 |   ${OpenCV_LIBRARIES}
57 |   ${catkin_LIBRARIES}
58 | )


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


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # CVC ROS
 2 | An ROS implementation of paper "Curved-Voxel Clustering for Accurate Segmentation of 3D LiDAR Point Clouds with Real-Time Performance"
 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 | ## Features
11 | * New Spatial Primitive. curved-voxel, a LiDAR-optimized spatial unit reflecting distinct characteristics of 3D LiDAR point clouds. 
12 | * Algorithm. an efficient method for segmenting 3D LiDAR point clouds by utilizing LiDAR-optimized curved-voxels and efficient hashbased data structure.
13 | * ROS dynamic reconfigure, you can tune the parameters easily.
14 | 
15 | ## Reference
16 | * Curved-Voxel Clustering for Accurate Segmentation of 3D LiDAR Point Clouds with Real-Time Performance. (IROS) 2019
17 | * https://github.com/wangx1996/Lidar-Segementation
18 | * https://github.com/SS47816/lidar_obstacle_detector
19 | 
20 | **TODOs**
21 | * imporove the efficiency of algorithm
22 | * imporove the segmentation accuracy
23 | 
24 | **Known Issues**
25 | * the segementation result is not very ideal.
26 | 
27 | ## Dependencies
28 | * ground cloud filter: https://github.com/HMX2013/patchwork-VLP16
29 | * sudo apt-get install ros-melodic-jsk-rviz-plugins
30 | 
31 | ## How to use
32 |     # clone the repo
33 |     mkdir -p catkin_ws/src
34 |     cd catkin_ws/src
35 |     git clone https://github.com/HMX2013/SemanticKITTI_loader
36 |     git clone https://github.com/HMX2013/CVC-ROS
37 |     download obsdet_msgs from
38 |     "https://drive.google.com/file/d/1ztLk9Slm656CV-WJieUpBJPlz-Iw14Bk/view?usp=share_link"
39 |     cd ../
40 |     catkin_make
41 |     roslaunch semantic_kitti run_semantic.launch
42 |     roslaunch cvc_ros run_rviz.launch
43 | 
44 | 
45 | ## Contribution
46 | You are welcome contributing to the package by opening a pull-request
47 | 
48 | We are following: 
49 | [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html), 
50 | [C++ Core Guidelines](https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#main), 
51 | and [ROS C++ Style Guide](http://wiki.ros.org/CppStyleGuide)
52 | 
53 | ## License
54 | MIT License
55 | 


--------------------------------------------------------------------------------
/config/semantic-kitti-all.yaml:
--------------------------------------------------------------------------------
  1 | # This file is covered by the LICENSE file in the root of this project.
  2 | labels: 
  3 |   0 : "unlabeled"
  4 |   1 : "outlier"
  5 |   10: "car"
  6 |   11: "bicycle"
  7 |   13: "bus"
  8 |   15: "motorcycle"
  9 |   16: "on-rails"
 10 |   18: "truck"
 11 |   20: "other-vehicle"
 12 |   30: "person"
 13 |   31: "bicyclist"
 14 |   32: "motorcyclist"
 15 |   40: "road"
 16 |   44: "parking"
 17 |   48: "sidewalk"
 18 |   49: "other-ground"
 19 |   50: "building"
 20 |   51: "fence"
 21 |   52: "other-structure"
 22 |   60: "lane-marking"
 23 |   70: "vegetation"
 24 |   71: "trunk"
 25 |   72: "terrain"
 26 |   80: "pole"
 27 |   81: "traffic-sign"
 28 |   99: "other-object"
 29 |   252: "moving-car"
 30 |   253: "moving-bicyclist"
 31 |   254: "moving-person"
 32 |   255: "moving-motorcyclist"
 33 |   256: "moving-on-rails"
 34 |   257: "moving-bus"
 35 |   258: "moving-truck"
 36 |   259: "moving-other-vehicle"
 37 | color_map: # bgr
 38 |   0 : [0, 0, 0]
 39 |   1 : [0, 0, 255]
 40 |   10: [245, 150, 100]
 41 |   11: [245, 230, 100]
 42 |   13: [250, 80, 100]
 43 |   15: [150, 60, 30]
 44 |   16: [255, 0, 0]
 45 |   18: [180, 30, 80]
 46 |   20: [255, 0, 0]
 47 |   30: [30, 30, 255]
 48 |   31: [200, 40, 255]
 49 |   32: [90, 30, 150]
 50 |   40: [255, 0, 255]
 51 |   44: [255, 150, 255]
 52 |   48: [75, 0, 75]
 53 |   49: [75, 0, 175]
 54 |   50: [0, 200, 255]
 55 |   51: [50, 120, 255]
 56 |   52: [0, 150, 255]
 57 |   60: [170, 255, 150]
 58 |   70: [0, 175, 0]
 59 |   71: [0, 60, 135]
 60 |   72: [80, 240, 150]
 61 |   80: [150, 240, 255]
 62 |   81: [0, 0, 255]
 63 |   99: [255, 255, 50]
 64 |   252: [245, 150, 100]
 65 |   256: [255, 0, 0]
 66 |   253: [200, 40, 255]
 67 |   254: [30, 30, 255]
 68 |   255: [90, 30, 150]
 69 |   257: [250, 80, 100]
 70 |   258: [180, 30, 80]
 71 |   259: [255, 0, 0]
 72 | content: # as a ratio with the total number of points
 73 |   0: 0.018889854628292943
 74 |   1: 0.0002937197336781505
 75 |   10: 0.040818519255974316
 76 |   11: 0.00016609538710764618
 77 |   13: 2.7879693665067774e-05
 78 |   15: 0.00039838616015114444
 79 |   16: 0.0
 80 |   18: 0.0020633612104619787
 81 |   20: 0.0016218197275284021
 82 |   30: 0.00017698551338515307
 83 |   31: 1.1065903904919655e-08
 84 |   32: 5.532951952459828e-09
 85 |   40: 0.1987493871255525
 86 |   44: 0.014717169549888214
 87 |   48: 0.14392298360372
 88 |   49: 0.0039048553037472045
 89 |   50: 0.1326861944777486
 90 |   51: 0.0723592229456223
 91 |   52: 0.002395131480328884
 92 |   60: 4.7084144280367186e-05
 93 |   70: 0.26681502148037506
 94 |   71: 0.006035012012626033
 95 |   72: 0.07814222006271769
 96 |   80: 0.002855498193863172
 97 |   81: 0.0006155958086189918
 98 |   99: 0.009923127583046915
 99 |   252: 0.001789309418528068
100 |   253: 0.00012709999297008662
101 |   254: 0.00016059776092534436
102 |   255: 3.745553104802113e-05
103 |   256: 0.0
104 |   257: 0.00011351574470342043
105 |   258: 0.00010157861367183268
106 |   259: 4.3840131989471124e-05
107 | # classes that are indistinguishable from single scan or inconsistent in
108 | # ground truth are mapped to their closest equivalent
109 | learning_map:
110 |   0 : 0     # "unlabeled"
111 |   1 : 0     # "outlier" mapped to "unlabeled" --------------------------mapped
112 |   10: 1     # "car"
113 |   11: 2     # "bicycle"
114 |   13: 5     # "bus" mapped to "other-vehicle" --------------------------mapped
115 |   15: 3     # "motorcycle"
116 |   16: 5     # "on-rails" mapped to "other-vehicle" ---------------------mapped
117 |   18: 4     # "truck"
118 |   20: 5     # "other-vehicle"
119 |   30: 6     # "person"
120 |   31: 7     # "bicyclist"
121 |   32: 8     # "motorcyclist"
122 |   40: 9     # "road"
123 |   44: 10    # "parking"
124 |   48: 11    # "sidewalk"
125 |   49: 12    # "other-ground"
126 |   50: 13    # "building"
127 |   51: 14    # "fence"
128 |   52: 0     # "other-structure" mapped to "unlabeled" ------------------mapped
129 |   60: 9     # "lane-marking" to "road" ---------------------------------mapped
130 |   70: 15    # "vegetation"
131 |   71: 16    # "trunk"
132 |   72: 17    # "terrain"
133 |   80: 18    # "pole"
134 |   81: 19    # "traffic-sign"
135 |   99: 0     # "other-object" to "unlabeled" ----------------------------mapped
136 |   252: 20    # "moving-car"
137 |   253: 21    # "moving-bicyclist"
138 |   254: 22    # "moving-person"
139 |   255: 23    # "moving-motorcyclist"
140 |   256: 24    # "moving-on-rails" mapped to "moving-other-vehicle" ------mapped
141 |   257: 24    # "moving-bus" mapped to "moving-other-vehicle" -----------mapped
142 |   258: 25    # "moving-truck"
143 |   259: 24    # "moving-other-vehicle"
144 | learning_map_inv: # inverse of previous map
145 |   0: 0      # "unlabeled", and others ignored
146 |   1: 10     # "car"
147 |   2: 11     # "bicycle"
148 |   3: 15     # "motorcycle"
149 |   4: 18     # "truck"
150 |   5: 20     # "other-vehicle"
151 |   6: 30     # "person"
152 |   7: 31     # "bicyclist"
153 |   8: 32     # "motorcyclist"
154 |   9: 40     # "road"
155 |   10: 44    # "parking"
156 |   11: 48    # "sidewalk"
157 |   12: 49    # "other-ground"
158 |   13: 50    # "building"
159 |   14: 51    # "fence"
160 |   15: 70    # "vegetation"
161 |   16: 71    # "trunk"
162 |   17: 72    # "terrain"
163 |   18: 80    # "pole"
164 |   19: 81    # "traffic-sign"
165 |   20: 252    # "moving-car"
166 |   21: 253    # "moving-bicyclist"
167 |   22: 254    # "moving-person"
168 |   23: 255    # "moving-motorcyclist"
169 |   24: 259    # "moving-other-vehicle"
170 |   25: 258    # "moving-truck"
171 | learning_ignore: # Ignore classes
172 |   0: True      # "unlabeled", and others ignored
173 |   1: False     # "car"
174 |   2: False     # "bicycle"
175 |   3: False     # "motorcycle"
176 |   4: False     # "truck"
177 |   5: False     # "other-vehicle"
178 |   6: False     # "person"
179 |   7: False     # "bicyclist"
180 |   8: False     # "motorcyclist"
181 |   9: False     # "road"
182 |   10: False    # "parking"
183 |   11: False    # "sidewalk"
184 |   12: False    # "other-ground"
185 |   13: False    # "building"
186 |   14: False    # "fence"
187 |   15: False    # "vegetation"
188 |   16: False    # "trunk"
189 |   17: False    # "terrain"
190 |   18: False    # "pole"
191 |   19: False    # "traffic-sign"
192 |   20: False    # "moving-car"
193 |   21: False    # "moving-bicyclist"
194 |   22: False    # "moving-person"
195 |   23: False    # "moving-motorcyclist"
196 |   24: False    # "moving-other-vehicle"
197 |   25: False    # "moving-truck"
198 | split: # sequence numbers
199 |   train:
200 |     - 0
201 |     - 1
202 |     - 2
203 |     - 3
204 |     - 4
205 |     - 5
206 |     - 6
207 |     - 7
208 |     - 9
209 |     - 10
210 |   valid:
211 |     - 8
212 |   test:
213 |     - 11
214 |     - 12
215 |     - 13
216 |     - 14
217 |     - 15
218 |     - 16
219 |     - 17
220 |     - 18
221 |     - 19
222 |     - 20
223 |     - 21
224 | 


--------------------------------------------------------------------------------
/rviz/cvc_rviz.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 138
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /TF1/Frames1
 10 |         - /TF1/Tree1
 11 |         - /Ground Cloud1
 12 |       Splitter Ratio: 0.6185566782951355
 13 |     Tree Height: 897
 14 |   - Class: rviz/Selection
 15 |     Name: Selection
 16 |   - Class: rviz/Tool Properties
 17 |     Expanded:
 18 |       - /2D Pose Estimate1
 19 |       - /2D Nav Goal1
 20 |       - /Publish Point1
 21 |     Name: Tool Properties
 22 |     Splitter Ratio: 0.5886790156364441
 23 |   - Class: rviz/Views
 24 |     Expanded:
 25 |       - /Current View1
 26 |     Name: Views
 27 |     Splitter Ratio: 0.5
 28 |   - Class: rviz/Time
 29 |     Name: Time
 30 |     SyncMode: 0
 31 |     SyncSource: segamented_color
 32 | Preferences:
 33 |   PromptSaveOnExit: true
 34 | Toolbars:
 35 |   toolButtonStyle: 2
 36 | Visualization Manager:
 37 |   Class: ""
 38 |   Displays:
 39 |     - Alpha: 0.5
 40 |       Cell Size: 1
 41 |       Class: rviz/Grid
 42 |       Color: 160; 160; 164
 43 |       Enabled: false
 44 |       Line Style:
 45 |         Line Width: 0.029999999329447746
 46 |         Value: Lines
 47 |       Name: Grid
 48 |       Normal Cell Count: 0
 49 |       Offset:
 50 |         X: 0
 51 |         Y: 0
 52 |         Z: 0
 53 |       Plane: XY
 54 |       Plane Cell Count: 10
 55 |       Reference Frame: <Fixed Frame>
 56 |       Value: false
 57 |     - Class: rviz/TF
 58 |       Enabled: true
 59 |       Frame Timeout: 15
 60 |       Frames:
 61 |         All Enabled: false
 62 |       Marker Alpha: 1
 63 |       Marker Scale: 1
 64 |       Name: TF
 65 |       Show Arrows: true
 66 |       Show Axes: true
 67 |       Show Names: true
 68 |       Tree:
 69 |         {}
 70 |       Update Interval: 0
 71 |       Value: true
 72 |     - Alpha: 1
 73 |       Autocompute Intensity Bounds: true
 74 |       Autocompute Value Bounds:
 75 |         Max Value: 4.370005130767822
 76 |         Min Value: -1.7799855470657349
 77 |         Value: true
 78 |       Axis: Z
 79 |       Channel Name: intensity
 80 |       Class: rviz/PointCloud2
 81 |       Color: 255; 255; 255
 82 |       Color Transformer: FlatColor
 83 |       Decay Time: 0
 84 |       Enabled: true
 85 |       Invert Rainbow: false
 86 |       Max Color: 255; 255; 255
 87 |       Min Color: 0; 0; 0
 88 |       Name: raw_pc
 89 |       Position Transformer: XYZ
 90 |       Queue Size: 10
 91 |       Selectable: true
 92 |       Size (Pixels): 3
 93 |       Size (m): 0.009999999776482582
 94 |       Style: Flat Squares
 95 |       Topic: /semi_kitti/cloud
 96 |       Unreliable: false
 97 |       Use Fixed Frame: true
 98 |       Use rainbow: true
 99 |       Value: true
100 |     - Alpha: 1
101 |       Autocompute Intensity Bounds: true
102 |       Autocompute Value Bounds:
103 |         Max Value: 10
104 |         Min Value: -10
105 |         Value: true
106 |       Axis: Z
107 |       Channel Name: intensity
108 |       Class: rviz/PointCloud2
109 |       Color: 125; 125; 125
110 |       Color Transformer: FlatColor
111 |       Decay Time: 0
112 |       Enabled: true
113 |       Invert Rainbow: false
114 |       Max Color: 255; 255; 255
115 |       Min Color: 0; 0; 0
116 |       Name: Ground Cloud
117 |       Position Transformer: XYZ
118 |       Queue Size: 10
119 |       Selectable: true
120 |       Size (Pixels): 3
121 |       Size (m): 0.019999999552965164
122 |       Style: Flat Squares
123 |       Topic: /semi_kitti/ground_pc
124 |       Unreliable: false
125 |       Use Fixed Frame: true
126 |       Use rainbow: true
127 |       Value: true
128 |     - Alpha: 1
129 |       Autocompute Intensity Bounds: true
130 |       Autocompute Value Bounds:
131 |         Max Value: 10
132 |         Min Value: -10
133 |         Value: true
134 |       Axis: Z
135 |       Channel Name: intensity
136 |       Class: rviz/PointCloud2
137 |       Color: 255; 255; 255
138 |       Color Transformer: Intensity
139 |       Decay Time: 0
140 |       Enabled: true
141 |       Invert Rainbow: false
142 |       Max Color: 255; 255; 255
143 |       Min Color: 0; 0; 0
144 |       Name: segamented_color
145 |       Position Transformer: XYZ
146 |       Queue Size: 10
147 |       Selectable: true
148 |       Size (Pixels): 3
149 |       Size (m): 0.029999999329447746
150 |       Style: Flat Squares
151 |       Topic: /segmentation/segmented_cloud_color
152 |       Unreliable: false
153 |       Use Fixed Frame: true
154 |       Use rainbow: true
155 |       Value: true
156 |     - Buffer length: 100
157 |       Class: jsk_rviz_plugin/Plotter2D
158 |       Enabled: true
159 |       Name: cvc took [ms]
160 |       Show Value: true
161 |       Topic: /cvc/time_cvc
162 |       Value: true
163 |       auto color change: false
164 |       auto scale: true
165 |       background color: 0; 0; 0
166 |       backround alpha: 0
167 |       border: true
168 |       foreground alpha: 0.699999988079071
169 |       foreground color: 25; 255; 240
170 |       height: 100
171 |       left: 60
172 |       linewidth: 1
173 |       max color: 255; 0; 0
174 |       max value: 1
175 |       min value: -1
176 |       show caption: true
177 |       text size: 12
178 |       top: 50
179 |       update interval: 0.03999999910593033
180 |       width: 150
181 |     - Class: jsk_rviz_plugin/BoundingBoxArray
182 |       Enabled: true
183 |       Name: BoundingBoxArray
184 |       Queue Size: 10
185 |       Topic: /pca_fitting/jsk_bboxs_array
186 |       Unreliable: false
187 |       Value: true
188 |       alpha: 0.800000011920929
189 |       color: 25; 255; 0
190 |       coloring: Auto
191 |       line width: 0.10000000149011612
192 |       only edge: true
193 |       show coords: false
194 |     - Alpha: 1
195 |       Autocompute Intensity Bounds: true
196 |       Autocompute Value Bounds:
197 |         Max Value: 10
198 |         Min Value: -10
199 |         Value: true
200 |       Axis: Z
201 |       Channel Name: intensity
202 |       Class: rviz/PointCloud2
203 |       Color: 255; 255; 255
204 |       Color Transformer: Intensity
205 |       Decay Time: 0
206 |       Enabled: false
207 |       Invert Rainbow: false
208 |       Max Color: 255; 255; 255
209 |       Min Color: 0; 0; 0
210 |       Name: PointCloud2
211 |       Position Transformer: XYZ
212 |       Queue Size: 10
213 |       Selectable: true
214 |       Size (Pixels): 3
215 |       Size (m): 0.029999999329447746
216 |       Style: Flat Squares
217 |       Topic: /segmentation/gt_pc
218 |       Unreliable: false
219 |       Use Fixed Frame: true
220 |       Use rainbow: true
221 |       Value: false
222 |   Enabled: true
223 |   Global Options:
224 |     Background Color: 0; 0; 0
225 |     Default Light: true
226 |     Fixed Frame: map
227 |     Frame Rate: 30
228 |   Name: root
229 |   Tools:
230 |     - Class: rviz/Interact
231 |       Hide Inactive Objects: true
232 |     - Class: rviz/MoveCamera
233 |     - Class: rviz/Select
234 |     - Class: rviz/FocusCamera
235 |     - Class: rviz/Measure
236 |     - Class: rviz/SetInitialPose
237 |       Theta std deviation: 0.2617993950843811
238 |       Topic: /initialpose
239 |       X std deviation: 0.5
240 |       Y std deviation: 0.5
241 |     - Class: rviz/SetGoal
242 |       Topic: /move_base_simple/goal
243 |     - Class: rviz/PublishPoint
244 |       Single click: true
245 |       Topic: /clicked_point
246 |   Value: true
247 |   Views:
248 |     Current:
249 |       Class: rviz/Orbit
250 |       Distance: 29.867748260498047
251 |       Enable Stereo Rendering:
252 |         Stereo Eye Separation: 0.05999999865889549
253 |         Stereo Focal Distance: 1
254 |         Swap Stereo Eyes: false
255 |         Value: false
256 |       Field of View: 0.7853981852531433
257 |       Focal Point:
258 |         X: -0.457008957862854
259 |         Y: 3.3725626468658447
260 |         Z: -3.368089199066162
261 |       Focal Shape Fixed Size: false
262 |       Focal Shape Size: 0.05000000074505806
263 |       Invert Z Axis: false
264 |       Name: Current View
265 |       Near Clip Distance: 0.009999999776482582
266 |       Pitch: 1.039797067642212
267 |       Target Frame: <Fixed Frame>
268 |       Yaw: 4.270313739776611
269 |     Saved: ~
270 | Window Geometry:
271 |   Displays:
272 |     collapsed: false
273 |   Height: 1308
274 |   Hide Left Dock: false
275 |   Hide Right Dock: true
276 |   QMainWindow State: 000000ff00000000fd00000004000000000000029600000448fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000448000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000342fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000342000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b600000074fc0100000002fb0000000800540069006d00650100000000000009b6000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000071a0000044800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
277 |   Selection:
278 |     collapsed: false
279 |   Time:
280 |     collapsed: false
281 |   Tool Properties:
282 |     collapsed: false
283 |   Views:
284 |     collapsed: true
285 |   Width: 2486
286 |   X: 72
287 |   Y: 27
288 | 


--------------------------------------------------------------------------------
/src/cvc.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef CVC_CLUSTER_H
  2 | #define CVC_CLUSTER_H
  3 | 
  4 | #include <pcl/io/pcd_io.h>
  5 | #include <pcl/features/integral_image_normal.h>
  6 | #include <boost/thread/thread.hpp>
  7 | #include <pcl/visualization/pcl_visualizer.h>
  8 | #include <pcl/visualization/cloud_viewer.h>
  9 | #include <pcl/common/transforms.h>
 10 | #include <pcl/ModelCoefficients.h>
 11 | #include <pcl/sample_consensus/method_types.h>
 12 | #include <pcl/sample_consensus/model_types.h>
 13 | #include <pcl/segmentation/sac_segmentation.h>
 14 | #include <pcl/io/pcd_io.h>
 15 | #include <pcl/point_types.h>
 16 | #include <pcl/point_cloud.h>
 17 | #include <pcl/visualization/cloud_viewer.h>
 18 | #include <pcl/filters/passthrough.h>
 19 | #include <pcl/filters/impl/passthrough.hpp>
 20 | #include <pcl/features/normal_3d.h>
 21 | #include <pcl/filters/extract_indices.h>
 22 | 
 23 | #include <iostream>
 24 | #include <stdlib.h>
 25 | #include <unordered_map>
 26 | #include <algorithm>
 27 | #include <limits>
 28 | #include "tools/utils.hpp"
 29 | using PointType = PointXYZIRLICO;
 30 | 
 31 | template <typename T>
 32 | std::string toString(const T &t)
 33 | {
 34 |    std::ostringstream oss;
 35 |    oss << t;
 36 |    return oss.str();
 37 | }
 38 | 
 39 | struct PointAPR
 40 | {
 41 |    float azimuth;
 42 |    float polar_angle;
 43 |    float range;
 44 |    float range_horizon;
 45 | };
 46 | 
 47 | struct Voxel
 48 | {
 49 |    bool haspoint = false;
 50 |    int cluster = -1;
 51 |    std::vector<int> index;
 52 | };
 53 | 
 54 | class CVC
 55 | {
 56 | public:
 57 | 
 58 |    void set_deltaA(float deltaA)
 59 |    {
 60 |       deltaA_ = deltaA;
 61 |    }
 62 | 
 63 |    void set_deltaR(float deltaR)
 64 |    {
 65 |       deltaR_ = deltaR;
 66 |    }
 67 | 
 68 |    void set_deltaP(float deltaP)
 69 |    {
 70 |       deltaP_ = deltaP;
 71 |    }
 72 | 
 73 |    void calculateAPR(const pcl::PointCloud<PointType> &cloud_in, std::vector<PointAPR> &vapr);
 74 |    void build_hash_table(const std::vector<PointAPR> &vapr, std::unordered_map<int, Voxel> &map_out);
 75 |    void find_neighbors(int azimuth, int range, int polar, std::vector<int> &neighborindex);
 76 |    bool most_frequent_value(std::vector<int> values, std::vector<int> &cluster_index);
 77 |    void mergeClusters(std::vector<int> &cluster_indices, int idx1, int idx2);
 78 |    std::vector<int> cluster(std::unordered_map<int, Voxel> &map_in, const std::vector<PointAPR> &vapr);
 79 |    void process();
 80 | 
 81 | private:
 82 |    //azimuth angle
 83 |    float deltaA_ = 0.4;
 84 | 
 85 |    //cvc range
 86 |    float deltaR_ = 0.3;
 87 | 
 88 |    //polar angle
 89 |    float deltaP_ = 4.0;
 90 | 
 91 |    float min_range_ = std::numeric_limits<float>::max();
 92 |    float max_range_ = std::numeric_limits<float>::min();
 93 |    float min_polar_ = -25.0 * M_PI / 180;
 94 |    float max_polar_ = 3.0 * M_PI / 180;
 95 | 
 96 |    int length_ = 0;
 97 |    int width_ = 0;
 98 |    int height_ = 0;
 99 | };
100 | 
101 | 
102 | bool compare_cluster(std::pair<int,int> a,std::pair<int,int> b){
103 |     return a.second>b.second;
104 | }//upper sort
105 | 
106 | float azimuth_angle_cal(float x, float y)
107 | {
108 |    float temp_tangle = 0;
109 |    if (x == 0 && y == 0)
110 |    {
111 |       temp_tangle = 0;
112 |    }
113 |    else if (y >= 0)
114 |    {
115 |       temp_tangle = (float)atan2(y, x);
116 |    }
117 |    else if (y < 0)
118 |    {
119 |       temp_tangle = (float)atan2(y, x) + 2 * M_PI;
120 |    }
121 |    return temp_tangle;
122 | }
123 | 
124 | void CVC::calculateAPR(const pcl::PointCloud<PointType>& cloud_in, std::vector<PointAPR>& vapr){
125 |    for (int i =0; i<cloud_in.points.size(); ++i){
126 |       PointAPR parp;
127 |       parp.azimuth = azimuth_angle_cal(cloud_in.points[i].x, cloud_in.points[i].y);
128 |       parp.range = sqrt(cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z);
129 |       parp.range_horizon = sqrt(cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y);
130 |       parp.polar_angle = (float)atan2(cloud_in.points[i].z, parp.range_horizon);
131 | 
132 |       if(parp.range < min_range_){
133 |          min_range_ = parp.range;
134 |       }
135 |       if(parp.range > max_range_){
136 |          max_range_ = parp.range;
137 |       }
138 |       vapr.push_back(parp);
139 |    }
140 | 
141 | 	length_ = int((max_range_ - min_range_)/deltaR_)+1;
142 | 	width_  = round(360/deltaA_);
143 | 	height_ = int(((max_polar_ - min_polar_)*180/M_PI)/deltaP_)+1;
144 | }
145 | 
146 | void CVC::build_hash_table(const std::vector<PointAPR>& vapr, std::unordered_map<int, Voxel> &map_out)
147 | {
148 |    for(int i =0; i< vapr.size(); ++i){
149 |       int azimuth_index = int(vapr[i].azimuth*180/M_PI/deltaA_);      
150 |       int range_index = int((vapr[i].range - min_range_)/deltaR_);
151 |       int polar_index = int(((vapr[i].polar_angle-min_polar_)*180/M_PI)/deltaP_);
152 |       int voxel_index = (azimuth_index*(length_)+range_index)+polar_index*(length_)*(width_);
153 | 
154 |       std::unordered_map<int, Voxel>::iterator it_find;
155 |       it_find = map_out.find(voxel_index);
156 |       if (it_find != map_out.end())
157 |       {
158 |          it_find->second.index.push_back(i);
159 |       }
160 |       else
161 |       {
162 |          Voxel vox;
163 |          vox.haspoint = true;
164 |          vox.index.push_back(i);
165 |          vox.index.swap(vox.index);
166 |          map_out.insert(std::make_pair(voxel_index, vox));
167 |       }
168 |    }
169 | }
170 | 
171 | void CVC::find_neighbors(int azimuth, int range, int polar, std::vector<int> &neighborindex)
172 | {
173 |    for (int z = polar - 1; z <= polar + 1; z++)
174 |    {
175 |       if (z < 0 || z > (height_ - 1))
176 |       {
177 |          continue;
178 |       }
179 | 
180 |       for (int y = range - 1; y <= range + 1; y++)
181 |       {
182 |          if (y < 0 || y > (length_ - 1))
183 |          {
184 |             continue;
185 |          }
186 |          for (int x = azimuth - 1; x <= azimuth + 1; x++)
187 |          {
188 |             int px = x;
189 |             if (x < 0)
190 |             {
191 |                px = width_ - 1;
192 |             }
193 |             if (x > (width_ - 1))
194 |             {
195 |                px = 0;
196 |             }
197 |             neighborindex.push_back((px * (length_) + y) + z * (length_) * (width_));
198 |          }
199 |       }
200 |    }
201 | }
202 | 
203 | std::vector<int> CVC::cluster(std::unordered_map<int, Voxel> &map_in, const std::vector<PointAPR> &vapr)
204 | {
205 |    int current_cluster = 0;
206 | 
207 |    std::vector<int> cluster_indices = std::vector<int>(vapr.size(), -1);
208 | 
209 |    for (int i = 0; i < vapr.size(); ++i)
210 |    {
211 |       if (cluster_indices[i] != -1)
212 |          continue;
213 | 
214 |       int azimuth_index = int(vapr[i].azimuth * 180 / M_PI / deltaA_);
215 |       int range_index = int((vapr[i].range - min_range_) / deltaR_);
216 |       int polar_index = int(((vapr[i].polar_angle - min_polar_) * 180 / M_PI) / deltaP_);
217 |       int voxel_index = (azimuth_index * (length_) + range_index) + polar_index * (length_) * (width_);
218 | 
219 |       std::unordered_map<int, Voxel>::iterator it_find;
220 |       std::unordered_map<int, Voxel>::iterator it_find2;
221 | 
222 |       it_find = map_in.find(voxel_index);
223 |       std::vector<int> neightbors;
224 | 
225 |       if (it_find != map_in.end())
226 |       {
227 |          std::vector<int> neighborid;
228 |          find_neighbors(azimuth_index, range_index, polar_index, neighborid);
229 |          for (int k = 0; k < neighborid.size(); ++k)
230 |          {
231 |             it_find2 = map_in.find(neighborid[k]);
232 | 
233 |             if (it_find2 != map_in.end())
234 |             {
235 |                for (int j = 0; j < it_find2->second.index.size(); ++j)
236 |                {
237 |                   neightbors.push_back(it_find2->second.index[j]);
238 |                }
239 |             }
240 |          }
241 |       }
242 | 
243 |       neightbors.swap(neightbors);
244 | 
245 |       if (neightbors.size() > 0)
246 |       {
247 |          for (int j = 0; j < neightbors.size(); ++j)
248 |          {
249 |             int oc = cluster_indices[i];
250 |             int nc = cluster_indices[neightbors[j]];
251 |             if (oc != -1 && nc != -1)
252 |             {
253 |                if (oc != nc)
254 |                   mergeClusters(cluster_indices, oc, nc);
255 |             }
256 |             else
257 |             {
258 |                if (nc != -1)
259 |                {
260 |                   cluster_indices[i] = nc;
261 |                }
262 |                else
263 |                {
264 |                   if (oc != -1)
265 |                   {
266 |                      cluster_indices[neightbors[j]] = oc;
267 |                   }
268 |                }
269 |             }
270 |          }
271 |       }
272 | 
273 |       if (cluster_indices[i] == -1)
274 |       {
275 |          current_cluster++;
276 |          cluster_indices[i] = current_cluster;
277 |          for (int s = 0; s < neightbors.size(); ++s)
278 |          {
279 |             cluster_indices[neightbors[s]] = current_cluster;
280 |          }
281 |       }
282 |    }
283 |    return cluster_indices;
284 | }
285 | 
286 | void CVC::mergeClusters(std::vector<int>& cluster_indices, int idx1, int idx2)
287 | {
288 | 	for (int i = 0; i < cluster_indices.size(); i++) {
289 | 		if (cluster_indices[i] == idx1) {
290 | 			cluster_indices[i] = idx2;
291 | 		}
292 | 	}
293 | }
294 | 
295 | bool CVC::most_frequent_value(std::vector<int> values, std::vector<int> &cluster_index)
296 | {
297 |    std::unordered_map<int, int> histcounts;
298 |    for (int i = 0; i < values.size(); i++)
299 |    {
300 |       if (histcounts.find(values[i]) == histcounts.end())
301 |       {
302 |          histcounts[values[i]] = 1;
303 |       }
304 |       else
305 |       {
306 |          histcounts[values[i]] += 1;
307 |       }
308 |    }
309 |    int max = 0, maxi;
310 |    std::vector<std::pair<int, int>> tr(histcounts.begin(), histcounts.end());
311 |    sort(tr.begin(), tr.end(), compare_cluster);
312 |    for (int i = 0; i < tr.size(); ++i)
313 |    {
314 |       if (tr[i].second > 10)
315 |       {
316 |          cluster_index.push_back(tr[i].first);
317 |       }
318 |    }
319 |    return true;
320 | }
321 | 
322 | #endif


--------------------------------------------------------------------------------
/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 |   
 56 | struct PointXYZIRLICO
 57 | {
 58 |   PCL_ADD_POINT4D;                    // quad-word XYZ
 59 |   float    intensity;                 ///< laser intensity reading
 60 |   uint16_t ring;                      ///< the lidar ring
 61 |   uint16_t label;                     ///< point label
 62 |   uint16_t id;
 63 |   uint16_t cid;
 64 |   uint16_t oid;
 65 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // ensure proper alignment
 66 | } EIGEN_ALIGN16;
 67 | 
 68 | // Register custom point struct according to PCL
 69 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRLICO,
 70 |                                   (float, x, x)
 71 |                                   (float, y, y)
 72 |                                   (float, z, z)
 73 |                                   (float, intensity, intensity)
 74 |                                   (float, ring, ring)
 75 |                                   (uint16_t, label, label)
 76 |                                   (uint16_t, id, id)
 77 |                                   (uint16_t, cid, cid)      
 78 |                                   (uint16_t, oid, oid)                               
 79 | )
 80 | 
 81 | 
 82 | void PointXYZIRLICO2XYZI(pcl::PointCloud<PointXYZIRLICO>& src,
 83 |                        pcl::PointCloud<pcl::PointXYZI>::Ptr dst){
 84 |   dst->points.clear();
 85 |   for (const auto &pt: src.points){
 86 |     pcl::PointXYZI pt_xyzi;
 87 |     pt_xyzi.x = pt.x;
 88 |     pt_xyzi.y = pt.y;
 89 |     pt_xyzi.z = pt.z;
 90 |     pt_xyzi.intensity = pt.intensity;
 91 |     dst->points.push_back(pt_xyzi);
 92 |   }
 93 | }
 94 | std::vector<int> outlier_classes = {UNLABELED, OUTLIER};
 95 | std::vector<int> ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN};
 96 | std::vector<int> ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING};
 97 | 
 98 | int count_num_ground(const pcl::PointCloud<PointXYZIRLICO>& pc){
 99 |   int num_ground = 0;
100 | 
101 |   std::vector<int>::iterator iter;
102 | 
103 |   for (auto const& pt: pc.points){
104 |     iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);
105 |     if (iter != ground_classes.end()){ // corresponding class is in ground classes
106 |       if (pt.label == VEGETATION){
107 |         if (pt.z < VEGETATION_THR){
108 |            num_ground ++;
109 |         }
110 |       }else num_ground ++;
111 |     }
112 |   }
113 |   return num_ground;
114 | }
115 | 
116 | std::map<int, int> set_initial_gt_counts(std::vector<int>& gt_classes){
117 |   map<int, int> gt_counts;
118 |   for (int i = 0; i< gt_classes.size(); ++i){
119 |     gt_counts.insert(pair<int,int>(gt_classes.at(i), 0));
120 |   }
121 |   return gt_counts;
122 | }
123 | 
124 | std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZIRLICO>& pc){
125 |   int num_ground = 0;
126 |   auto gt_counts = set_initial_gt_counts(ground_classes);
127 |   std::vector<int>::iterator iter;
128 | 
129 |   for (auto const& pt: pc.points){
130 |     iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);
131 |     if (iter != ground_classes.end()){ // corresponding class is in ground classes
132 |       if (pt.label == VEGETATION){
133 |         if (pt.z < VEGETATION_THR){
134 |            gt_counts.find(pt.label)->second++;
135 |         }
136 |       }else gt_counts.find(pt.label)->second++;
137 |     }
138 |   }
139 |   return gt_counts;
140 | }
141 | 
142 | int count_num_outliers(const pcl::PointCloud<PointXYZIRLICO>& pc){
143 |   int num_outliers = 0;
144 | 
145 |   std::vector<int>::iterator iter;
146 | 
147 |   for (auto const& pt: pc.points){
148 |     iter = std::find(outlier_classes.begin(), outlier_classes.end(), pt.label);
149 |     if (iter != outlier_classes.end()){ // corresponding class is in ground classes
150 |       num_outliers ++;
151 |     }
152 |   }
153 |   return num_outliers;
154 | }
155 | 
156 | 
157 | void discern_ground(const pcl::PointCloud<PointXYZIRLICO>& src, pcl::PointCloud<PointXYZIRLICO>& ground, pcl::PointCloud<PointXYZIRLICO>& non_ground){
158 |   ground.clear();
159 |   non_ground.clear();
160 |   std::vector<int>::iterator iter;
161 |   for (auto const& pt: src.points){
162 |     if (pt.label == UNLABELED || pt.label == OUTLIER) continue;
163 |     iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);
164 |     if (iter != ground_classes.end()){ // corresponding class is in ground classes
165 |       if (pt.label == VEGETATION){
166 |         if (pt.z < VEGETATION_THR){
167 |           ground.push_back(pt);
168 |         }else non_ground.push_back(pt);
169 |       }else  ground.push_back(pt);
170 |     }else{
171 |       non_ground.push_back(pt);
172 |     }
173 |   }
174 | }
175 | 
176 | 
177 | void calculate_precision_recall(const pcl::PointCloud<PointXYZIRLICO>& pc_curr,
178 |                                 pcl::PointCloud<PointXYZIRLICO>& ground_estimated,
179 |                                 double & precision,
180 |                                 double& recall,
181 |                                 bool consider_outliers=true){
182 | 
183 |   int num_ground_est = ground_estimated.points.size();
184 |   int num_ground_gt = count_num_ground(pc_curr);
185 |   int num_TP = count_num_ground(ground_estimated);
186 |   if (consider_outliers){
187 |     int num_outliers_est = count_num_outliers(ground_estimated);
188 |     precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100;
189 |     recall = (double)(num_TP)/num_ground_gt * 100;
190 |   }else{
191 |     precision = (double)(num_TP)/num_ground_est * 100;
192 |     recall = (double)(num_TP)/num_ground_gt * 100;
193 |   }
194 | }
195 | 
196 | int save_all_labels(const pcl::PointCloud<PointXYZIRLICO>& pc, string ABS_DIR, string seq, int count){
197 | 
198 |   std::string count_str = std::to_string(count);
199 |   std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str;
200 |   std::string output_filename = ABS_DIR + "/" + seq + "/" + count_str_padded + ".csv";
201 |   ofstream sc_output(output_filename);
202 | 
203 |   vector<int> labels(NUM_ALL_CLASSES, 0);
204 |   for (auto const& pt: pc.points){
205 |     if (pt.label == 0) labels[0]++;
206 |     else if (pt.label == 1) labels[1]++;
207 |     else if (pt.label == 10) labels[2]++;
208 |     else if (pt.label == 11) labels[3]++;
209 |     else if (pt.label == 13) labels[4]++;
210 |     else if (pt.label == 15) labels[5]++;
211 |     else if (pt.label == 16) labels[6]++;
212 |     else if (pt.label == 18) labels[7]++;
213 |     else if (pt.label == 20) labels[8]++;
214 |     else if (pt.label == 30) labels[9]++;
215 |     else if (pt.label == 31) labels[10]++;
216 |     else if (pt.label == 32) labels[11]++;
217 |     else if (pt.label == 40) labels[12]++;
218 |     else if (pt.label == 44) labels[13]++;
219 |     else if (pt.label == 48) labels[14]++;
220 |     else if (pt.label == 49) labels[15]++;
221 |     else if (pt.label == 50) labels[16]++;
222 |     else if (pt.label == 51) labels[17]++;
223 |     else if (pt.label == 52) labels[18]++;
224 |     else if (pt.label == 60) labels[19]++;
225 |     else if (pt.label == 70) labels[20]++;
226 |     else if (pt.label == 71) labels[21]++;
227 |     else if (pt.label == 72) labels[22]++;
228 |     else if (pt.label == 80) labels[23]++;
229 |     else if (pt.label == 81) labels[24]++;
230 |     else if (pt.label == 99) labels[25]++;
231 |     else if (pt.label == 252) labels[26]++;
232 |     else if (pt.label == 253) labels[27]++;
233 |     else if (pt.label == 254) labels[28]++;
234 |     else if (pt.label == 255) labels[29]++;
235 |     else if (pt.label == 256) labels[30]++;
236 |     else if (pt.label == 257) labels[31]++;
237 |     else if (pt.label == 258) labels[32]++;
238 |     else if (pt.label == 259) labels[33]++;
239 |   }
240 | 
241 |   for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){
242 |     if (i!=33){
243 |       sc_output<<labels[i]<<",";
244 |     }else{
245 |       sc_output<<labels[i]<<endl;
246 |     }
247 |   }
248 |   sc_output.close();
249 | }
250 | 
251 | void save_all_accuracy(const pcl::PointCloud<PointXYZIRLICO>& pc_curr,
252 |                       pcl::PointCloud<PointXYZIRLICO>& ground_estimated, string acc_filename,
253 |                       double& accuracy, map<int, int>&pc_curr_gt_counts, map<int, int>&g_est_gt_counts){
254 | 
255 | 
256 | //  std::cout<<"debug: "<<acc_filename<<std::endl;
257 |   ofstream sc_output2(acc_filename, ios::app);
258 | 
259 |   int num_True = count_num_ground(pc_curr);
260 |   int num_outliers_gt = count_num_outliers(pc_curr);
261 |   int num_outliers_est = count_num_outliers(ground_estimated);
262 | 
263 |   int num_total_est = ground_estimated.points.size() - num_outliers_est;
264 |   int num_total_gt = pc_curr.points.size() - num_outliers_gt;
265 | 
266 |   int num_False = num_total_gt - num_True;
267 |   int num_TP = count_num_ground(ground_estimated);
268 |   int num_FP = num_total_est - num_TP;
269 |   accuracy = static_cast<double>(num_TP + (num_False - num_FP)) / num_total_gt * 100.0;
270 | 
271 |   pc_curr_gt_counts = count_num_each_class(pc_curr);
272 |   g_est_gt_counts = count_num_each_class(ground_estimated);
273 | 
274 |   // save output
275 |   for (auto const& class_id: ground_classes){
276 |     sc_output2<<g_est_gt_counts.find(class_id)->second<<","<<pc_curr_gt_counts.find(class_id)->second<<",";
277 |   }
278 |   sc_output2<<accuracy<<endl;
279 | 
280 |   sc_output2.close();
281 | }
282 | 
283 | void pc2pcdfile(const pcl::PointCloud<PointXYZIRLICO>& TP, const pcl::PointCloud<PointXYZIRLICO>& FP,
284 |                 const pcl::PointCloud<PointXYZIRLICO>& FN, const pcl::PointCloud<PointXYZIRLICO>& TN,
285 |                 std::string pcd_filename){
286 |   pcl::PointCloud<pcl::PointXYZI> pc_out;
287 | 
288 |   for (auto const pt: TP.points){
289 |     pcl::PointXYZI pt_est;
290 |     pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;
291 |     pt_est.intensity = TRUEPOSITIVE;
292 |     pc_out.points.push_back(pt_est);
293 |   }
294 |   for (auto const pt: FP.points){
295 |     pcl::PointXYZI pt_est;
296 |     pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;
297 |     pt_est.intensity = FALSEPOSITIVE;
298 |     pc_out.points.push_back(pt_est);
299 |   }
300 |   for (auto const pt: FN.points){
301 |     pcl::PointXYZI pt_est;
302 |     pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;
303 |     pt_est.intensity = FALSENEGATIVE;
304 |     pc_out.points.push_back(pt_est);
305 |   }
306 |   for (auto const pt: TN.points){
307 |     pcl::PointXYZI pt_est;
308 |     pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;
309 |     pt_est.intensity = TRUENEGATIVE;
310 |     pc_out.points.push_back(pt_est);
311 |   }
312 |   pc_out.width = pc_out.points.size();
313 |   pc_out.height = 1;
314 |   pcl::io::savePCDFileASCII(pcd_filename, pc_out);
315 | 
316 | }
317 | 
318 | 
319 | #endif
320 | 


--------------------------------------------------------------------------------
/src/cvc_ros_node.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <ros/console.h>
  3 | #include <std_msgs/Header.h>
  4 | #include <std_msgs/Float32.h>
  5 | #include <geometry_msgs/PoseStamped.h>
  6 | 
  7 | #include "obsdet_msgs/CloudCluster.h"
  8 | #include "obsdet_msgs/CloudClusterArray.h"
  9 | 
 10 | #include <pcl_ros/point_cloud.h>
 11 | #include <pcl_conversions/pcl_conversions.h>
 12 | #include <pcl/kdtree/kdtree.h>
 13 | #include <pcl/segmentation/conditional_euclidean_clustering.h>
 14 | 
 15 | #include <opencv2/core/version.hpp>
 16 | 
 17 | #if (CV_MAJOR_VERSION == 3)
 18 |   #include <opencv/cv.h>
 19 | #else
 20 |   #include <opencv2/imgproc.hpp>
 21 | #endif
 22 | 
 23 | #include <dynamic_reconfigure/server.h>
 24 | #include <cvc_ros/cvc_ros_Config.h>
 25 | #include "cvc.hpp"
 26 | #include "tools/utils.hpp"
 27 | #include "ground_truth.hpp"
 28 | 
 29 | // using namespace cv;
 30 | using PointType = PointXYZIRLICO;
 31 | 
 32 | static ros::Publisher pub_jskrviz_time_;
 33 | static std_msgs::Float32 time_spent;
 34 | 
 35 | std::string output_frame_;
 36 | std::string non_ground_cloud_;
 37 | std_msgs::Header rosmsg_header_;
 38 | 
 39 | // Pointcloud Filtering Parameters
 40 | float DELTA_A, DELTA_P, DELTA_R;
 41 | 
 42 | 
 43 | boost::shared_ptr<groundtruth::DepthCluster<PointType>> gt_verify;
 44 | 
 45 | class cloud_segmentation
 46 | {
 47 |  private:
 48 |   ros::NodeHandle nh;
 49 | 
 50 |   pcl::PointCloud<PointType>::Ptr laserCloudIn;
 51 | 
 52 |   dynamic_reconfigure::Server<cvc_ros::cvc_ros_Config> server;
 53 |   dynamic_reconfigure::Server<cvc_ros::cvc_ros_Config>::CallbackType f;
 54 | 
 55 |   pcl::PointCloud<pcl::PointXYZI>::Ptr segmentedCloudColor;
 56 | 
 57 |   ros::Subscriber sub_lidar_points_;
 58 | 
 59 |   ros::Publisher pub_obsdet_clusters_;
 60 |   ros::Publisher pubSegmentedCloudColor_;
 61 |   ros::Publisher pubgt_pc_;
 62 | 
 63 |   std::vector<std::vector<int> > clusterIndices;
 64 |   std::vector<std::vector<int> > objectIndices;
 65 | 
 66 |   std::vector<std::vector<int> > gt_clusterIndices;
 67 | 
 68 |   std::vector<int> runtime_vector;
 69 |   std::vector<double> use_vector;
 70 |   std::vector<double> ose_vector;
 71 | 
 72 |   std::shared_ptr<CVC> cvc;
 73 | 
 74 |   void preprocessing(const sensor_msgs::PointCloud2::ConstPtr& laserRosCloudMsg);
 75 |   void MainLoop(const sensor_msgs::PointCloud2::ConstPtr& lidar_points);
 76 |   void eval_running_time(int running_time);
 77 |   void eval_USE();
 78 |   void eval_OSE();
 79 | 
 80 |   void copyPointCloudCEC(pcl::PointCloud<PointType> &src, pcl::PointCloud<pcl::PointXYZINormal> &dst);
 81 | 
 82 |   void CVC_CLUSTER(const pcl::PointCloud<PointType>::Ptr in_cloud_ptr,
 83 |                    const float deltaA, const float deltaR, const float deltaP);
 84 |   void postSegment(obsdet_msgs::CloudClusterArray &in_out_cluster_array);
 85 | 
 86 |  public:
 87 |   cloud_segmentation();
 88 |   ~cloud_segmentation() {};
 89 | 
 90 |   void allocateMemory(){
 91 |     segmentedCloudColor.reset(new pcl::PointCloud<pcl::PointXYZI>());
 92 |     laserCloudIn.reset(new pcl::PointCloud<PointType>());
 93 |   }
 94 | 
 95 |   void resetParameters(){
 96 |     segmentedCloudColor->clear();
 97 |     laserCloudIn->clear();
 98 |     clusterIndices.clear();
 99 |     gt_clusterIndices.clear();
100 |     objectIndices.clear();
101 |   }
102 | 
103 | };
104 | 
105 | // Dynamic parameter server callback function
106 | void dynamicParamCallback(cvc_ros::cvc_ros_Config& config, uint32_t level)
107 | {
108 |   // Pointcloud Filtering Parameters
109 |   DELTA_A=config.delta_a;
110 |   DELTA_P=config.delta_p;
111 |   DELTA_R=config.delta_r;
112 | }
113 | 
114 | 
115 | cloud_segmentation::cloud_segmentation()
116 | {
117 |   ros::NodeHandle private_nh("~");
118 | 
119 |   runtime_vector.clear();
120 |   use_vector.clear();
121 |   ose_vector.clear();
122 | 
123 |   std::string cloud_ground_topic;
124 |   std::string cloud_clusters_topic;
125 |   std::string jsk_bboxes_topic;
126 | 
127 |   private_nh.param<std::string>("non_ground_cloud", non_ground_cloud_, "/points_no_ground");
128 |   private_nh.param<std::string>("output_frame", output_frame_, "velodyne");
129 | 
130 |   sub_lidar_points_ = nh.subscribe(non_ground_cloud_, 1, &cloud_segmentation::MainLoop, this);
131 |   pubSegmentedCloudColor_ = nh.advertise<sensor_msgs::PointCloud2> ("/segmentation/segmented_cloud_color", 1);
132 | 
133 |   pub_obsdet_clusters_ = nh.advertise<obsdet_msgs::CloudClusterArray>("/clustering/cluster_array", 1);
134 | 
135 |   pubgt_pc_= nh.advertise<sensor_msgs::PointCloud2> ("/segmentation/gt_pc", 1);
136 | 
137 |   pub_jskrviz_time_ = nh.advertise<std_msgs::Float32>("/cvc/time_cvc", 1);
138 | 
139 |   // Dynamic Parameter Server & Function
140 |   f = boost::bind(&dynamicParamCallback, _1, _2);
141 |   server.setCallback(f);
142 | 
143 |   // Create point processor
144 |   cvc = std::make_shared<CVC>();
145 | 
146 |   allocateMemory();
147 |   resetParameters();
148 | }
149 | 
150 | void cloud_segmentation::CVC_CLUSTER(const pcl::PointCloud<PointType>::Ptr in_cloud_ptr,
151 |                                      const float deltaA, const float deltaR, const float deltaP)
152 | {
153 |   std::vector<pcl::PointCloud<PointType>::Ptr> clusters;
154 | 
155 |   cvc->set_deltaA(deltaA);
156 |   cvc->set_deltaR(deltaR);
157 |   cvc->set_deltaP(deltaP);
158 |   std::vector<PointAPR> capr;
159 |   cvc->calculateAPR(*in_cloud_ptr, capr);
160 | 
161 | 	std::unordered_map<int, Voxel> hash_table;
162 | 	cvc->build_hash_table(capr, hash_table);
163 | 	std::vector<int> cluster_indices;
164 | 	cluster_indices = cvc->cluster(hash_table, capr);
165 | 	std::vector<int> cluster_id;
166 | 	cvc->most_frequent_value(cluster_indices, cluster_id);
167 | 
168 |   uint16_t cid_index = 0;
169 | 
170 |   for (int j = 0; j < cluster_id.size(); ++j)
171 |   {
172 |     std::vector<int> clusterIndice;
173 |     for (int i = 0; i < cluster_indices.size(); ++i)
174 |     {
175 |       if (cluster_indices[i] == cluster_id[j])
176 |       {
177 |         clusterIndice.push_back(i);
178 |         laserCloudIn->points[i].cid = cid_index;
179 |       }
180 |     }
181 |     clusterIndices.push_back(clusterIndice);
182 |     cid_index++;
183 |   }
184 | }
185 | 
186 | 
187 | void cloud_segmentation::eval_USE()
188 | {
189 |   std::vector<int> cluater_label;
190 |   std::vector<std::vector<int> > cluaters_label;
191 | 
192 |   int label[34] = {0, 1, 10, 11, 13, 15, 16, 18, 20, 30, 31, 32, 40, 44, 48, 49, 50, 51,
193 |                    52, 60, 70, 71, 72, 80, 81, 99, 252, 253, 254, 255, 256, 257, 258, 259};
194 | 
195 |   double use_i_sum = 0;
196 | 
197 |   for (auto& getIndices : clusterIndices)
198 |   {
199 |     int cluster_label[34] = {0};
200 |     for (auto& index : getIndices)
201 |     {
202 |       for (size_t i = 0; i < 34; i++)
203 |       {
204 |         if (laserCloudIn->points[index].label == label[i])
205 |           cluster_label[i]++;
206 |       }
207 |     }
208 | 
209 |     int M = getIndices.size();
210 | 
211 |     for (size_t i = 0; i < 34; i++)
212 |     {
213 |       if (cluster_label[i] == 0)
214 |         continue;
215 | 
216 |       double use_i = -(cluster_label[i] / (M * 1.0)) * log(cluster_label[i] / (M * 1.0));
217 |       use_i_sum += use_i;
218 |     }
219 |   }
220 | 
221 |   use_vector.push_back(use_i_sum);  
222 | 
223 |   double use_total_v = 0.0;
224 |   double use_sqr_sum = 0.0;
225 |   double use_mean;
226 |   double use_std;
227 | 
228 |   for (size_t i = 0; i < use_vector.size(); i++)
229 |   {
230 |     use_total_v += use_vector[i];
231 |   }
232 |   use_mean = use_total_v / use_vector.size();
233 | 
234 |   for (size_t i = 0; i < use_vector.size(); i++)
235 |   {
236 |     use_sqr_sum += (use_vector[i] - use_mean) * (use_vector[i] - use_mean);
237 |   }
238 | 
239 |   use_std = sqrt(use_sqr_sum / use_vector.size());
240 | 
241 |   std::cout << "current use_i is = " << use_i_sum << std::endl;
242 |   std::cout << "\033[1;32muse_mean is = " << use_mean << "\033[0m" << std::endl;
243 |   std::cout << "use_std is = " << use_std << std::endl;
244 | }
245 | 
246 | 
247 | void cloud_segmentation::eval_OSE()
248 | {
249 |   double ose_i = 0.0;
250 | 
251 |   int cluster_id = 0;
252 |   for (auto &getIndices : clusterIndices)
253 |   {
254 |     for (auto &index : getIndices)
255 |     {
256 |       laserCloudIn->points[index].cid = cluster_id;
257 |     }
258 |     cluster_id++;
259 |   }
260 | 
261 |   for (auto &getIndices : gt_clusterIndices)
262 |   {
263 |     int object_cluster[clusterIndices.size()] = {0};
264 |     int N = getIndices.size();
265 | 
266 |     for (auto &index : getIndices)
267 |     {
268 |       if (laserCloudIn->points[index].cid != 9999)
269 |         object_cluster[laserCloudIn->points[index].cid]++;
270 |     }
271 | 
272 |     for (size_t i = 0; i < clusterIndices.size(); i++)
273 |     {
274 |       if (object_cluster[i] == 0)
275 |         continue;
276 | 
277 |       double ose_ii = -(object_cluster[i] / (1.0*N)) * log(object_cluster[i] / (1.0*N));
278 | 
279 |       ose_i += ose_ii;
280 |     }
281 |   }
282 | 
283 |   ose_vector.push_back(ose_i);
284 | 
285 |   std::cout << "ose_vector.size() is = " << ose_vector.size() << std::endl;
286 | 
287 |   double ose_total_v = 0.0;
288 |   double ose_sqr_sum = 0.0;
289 |   double ose_mean;
290 |   double ose_std;
291 | 
292 |   for (size_t i = 0; i < ose_vector.size(); i++)
293 |   {
294 |     ose_total_v += ose_vector[i];
295 |   }
296 |   ose_mean = ose_total_v / ose_vector.size();
297 | 
298 |   for (size_t i = 0; i < ose_vector.size(); i++)
299 |   {
300 |     ose_sqr_sum += (ose_vector[i] - ose_mean) * (ose_vector[i] - ose_mean);
301 |   }
302 | 
303 |   ose_std = sqrt(ose_sqr_sum / ose_vector.size());
304 | 
305 |   std::cout << "current ose_i is = " << ose_i << std::endl;
306 |   std::cout << "\033[1;34mose_mean is = " << ose_mean << "\033[0m" << std::endl;
307 |   std::cout << "ose_std is = " << ose_std << std::endl;
308 | }
309 | 
310 | 
311 | void cloud_segmentation::eval_running_time(int running_time)
312 | {
313 |   double runtime_std;
314 |   double runtime_aver;
315 |   double runtime_total_v = 0.0;
316 |   double runtime_sqr_sum = 0.0;
317 | 
318 |   runtime_vector.push_back(running_time);
319 | 
320 |   for (size_t i = 0; i < runtime_vector.size(); i++)
321 |   {
322 |     runtime_total_v += runtime_vector[i];
323 |   }
324 | 
325 |   runtime_aver = runtime_total_v / runtime_vector.size();
326 | 
327 |   for (size_t i = 0; i < runtime_vector.size(); i++)
328 |   {
329 |     runtime_sqr_sum += (runtime_vector[i] - runtime_aver) * (runtime_vector[i] - runtime_aver);
330 |   }
331 | 
332 |   runtime_std = sqrt(runtime_sqr_sum / runtime_vector.size());
333 | 
334 |   std::cout << "current running_time is = " << running_time << "ms" << std::endl;
335 |   std::cout << "\033[1;36mruntime_aver is = " << runtime_aver << "ms"
336 |             << "\033[0m" << std::endl;
337 |   std::cout << "runtime_std is = " << runtime_std << "ms" << std::endl;
338 | }
339 | 
340 | void cloud_segmentation::preprocessing(const sensor_msgs::PointCloud2::ConstPtr& laserRosCloudMsg){
341 |   rosmsg_header_ = laserRosCloudMsg->header;
342 |   rosmsg_header_.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
343 | 
344 |   pcl::PointCloud<PointType>::Ptr raw_pcl(new pcl::PointCloud<PointType>());
345 | 
346 |   pcl::fromROSMsg(*laserRosCloudMsg, *raw_pcl);
347 | 
348 |   size_t cloudSize = raw_pcl->points.size();
349 |   PointType this_pt;
350 |   laserCloudIn->clear();
351 |   
352 |   int scan_num=0;
353 |   for (size_t i = 0; i < cloudSize; ++i)
354 |   {
355 |     this_pt.x = raw_pcl->points[i].x;
356 |     this_pt.y = raw_pcl->points[i].y;
357 |     this_pt.z = raw_pcl->points[i].z;
358 |     this_pt.intensity = raw_pcl->points[i].intensity;
359 |     this_pt.id = raw_pcl->points[i].id;
360 |     this_pt.label = raw_pcl->points[i].label;
361 |     this_pt.cid = 9999;
362 | 
363 |     bool is_nan = std::isnan(this_pt.x) || std::isnan(this_pt.y) || std::isnan(this_pt.z);
364 |     if (is_nan)
365 |       continue;
366 |     laserCloudIn->points.push_back(this_pt);
367 |   }
368 | }
369 | 
370 | void cloud_segmentation::postSegment(obsdet_msgs::CloudClusterArray &in_out_cluster_array)
371 | {
372 |   unsigned int intensity_mark = 1;
373 |   
374 |   pcl::PointXYZI cluster_color;
375 |   sensor_msgs::PointCloud2 cloud_msg;
376 |   sensor_msgs::PointCloud2 colored_cluster_ros;
377 |   pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_pcl(new pcl::PointCloud<pcl::PointXYZI>);
378 | 
379 |   // clusterIndices  objectIndices
380 | 
381 |   for (auto& getIndices : clusterIndices)
382 |   {
383 |     cluster_pcl->clear();
384 |     for (auto& index : getIndices){
385 |       cluster_color.x = laserCloudIn->points[index].x;
386 |       cluster_color.y = laserCloudIn->points[index].y;
387 |       cluster_color.z = laserCloudIn->points[index].z;
388 |       cluster_color.intensity = intensity_mark;
389 |       cluster_pcl->push_back(cluster_color);
390 |       segmentedCloudColor->push_back(cluster_color);
391 |     }
392 | 
393 |     intensity_mark++;
394 |     cloud_msg.header = rosmsg_header_;
395 |     pcl::toROSMsg(*cluster_pcl, cloud_msg);
396 | 
397 |     obsdet_msgs::CloudCluster cluster_;
398 |     cluster_.header = rosmsg_header_;
399 |     cluster_.cloud = cloud_msg;
400 |     in_out_cluster_array.clusters.push_back(cluster_);
401 |   }
402 | 
403 |   in_out_cluster_array.header = rosmsg_header_;
404 |   pub_obsdet_clusters_.publish(in_out_cluster_array);
405 | 
406 |   pcl::toROSMsg(*segmentedCloudColor, colored_cluster_ros);
407 |   colored_cluster_ros.header.frame_id = rosmsg_header_.frame_id;
408 |   colored_cluster_ros.header.stamp = ros::Time::now();
409 |   pubSegmentedCloudColor_.publish(colored_cluster_ros);
410 | }
411 | 
412 | 
413 | void cloud_segmentation::MainLoop(const sensor_msgs::PointCloud2::ConstPtr& lidar_points)
414 | {
415 |   gt_verify.reset(new groundtruth::DepthCluster<PointType>());
416 | 
417 |   obsdet_msgs::CloudClusterArray cluster_array;
418 | 
419 |   preprocessing(lidar_points);
420 | 
421 |   gt_verify->GTV(laserCloudIn, gt_clusterIndices);
422 | 
423 |   std::vector<int> cluster_indices;
424 | 
425 |   const auto start_time = std::chrono::steady_clock::now();
426 | 
427 |   CVC_CLUSTER(laserCloudIn, DELTA_A, DELTA_R, DELTA_P);
428 | 
429 |   const auto end_time = std::chrono::steady_clock::now();
430 |   const auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
431 | 
432 |   eval_running_time(elapsed_time.count());
433 | 
434 |   eval_USE();
435 |   eval_OSE();
436 | 
437 |   // extract segmented cloud for visualization
438 |   postSegment(cluster_array);
439 | 
440 |   time_spent.data = elapsed_time.count();
441 |   pub_jskrviz_time_.publish(time_spent);
442 | 
443 |   resetParameters();
444 |   std::cout << "--------------------------" << std::endl;
445 | }
446 | 
447 | int main(int argc, char** argv)
448 | {
449 |   ros::init(argc, argv, "cvc_ros_node");
450 | 
451 |   cloud_segmentation cloud_segmentation_node;
452 | 
453 |   ros::spin();
454 | 
455 |   return 0;
456 | }


--------------------------------------------------------------------------------
/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 | 
 34 | 
 35 | 
 36 | namespace groundtruth {
 37 |     template <typename PointT>
 38 |     class DepthCluster {
 39 |         private:
 40 |             uint16_t max_label_;
 41 | 
 42 |             std::vector<float> vert_angles_;
 43 |             std::vector<std::vector<int>> clusterIndices;
 44 | 
 45 |             std::vector<int> index_v;
 46 | 
 47 |             uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
 48 |             uint16_t *queueIndY;
 49 | 
 50 |             uint16_t *allPushedIndX; // array for tracking points of a segmented object
 51 |             uint16_t *allPushedIndY;
 52 | 
 53 |             int labelCount;
 54 |             std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
 55 |             
 56 |             cv::Mat rangeMat; // range matrix for range image
 57 |             cv::Mat labelMat; // label matrix for segmentaiton marking
 58 | 
 59 |             PointT nanPoint; // fill in fullCloud at each iteration
 60 |             typename pcl::PointCloud<PointT>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
 61 | 
 62 |         public:
 63 |             DepthCluster() {}
 64 | 
 65 |             ~DepthCluster() {}
 66 | 
 67 |             void allocateMemory()
 68 |             {
 69 |                 // clusterIndices.clear();
 70 |                 fullCloud.reset(new pcl::PointCloud<PointT>());
 71 |                 fullCloud->points.resize(VERT_SCAN*HORZ_SCAN);
 72 |                 index_v.resize(VERT_SCAN * HORZ_SCAN);
 73 |                 nanPoint.x = std::numeric_limits<float>::quiet_NaN();
 74 |                 nanPoint.y = std::numeric_limits<float>::quiet_NaN();
 75 |                 nanPoint.z = std::numeric_limits<float>::quiet_NaN();
 76 |                 nanPoint.intensity = -1;
 77 |                 nanPoint.id = 0;
 78 |                 nanPoint.label = 0;
 79 |                 nanPoint.cid = 0;
 80 | 
 81 |                 queueIndX = new uint16_t[VERT_SCAN*HORZ_SCAN];
 82 |                 queueIndY = new uint16_t[VERT_SCAN*HORZ_SCAN];
 83 | 
 84 |                 allPushedIndX = new uint16_t[VERT_SCAN*HORZ_SCAN];
 85 |                 allPushedIndY = new uint16_t[VERT_SCAN*HORZ_SCAN];
 86 | 
 87 |                 std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
 88 |                 labelMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32S, cv::Scalar::all(0));
 89 |                 rangeMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
 90 |                 labelCount = 1;
 91 | 
 92 |                 std::pair<int8_t, int8_t> neighbor;
 93 | 
 94 |                 int8_t neighbor_col = 50;
 95 |                 
 96 |                 for (int8_t i = -4; i <= 4; i++)
 97 |                 {
 98 |                     for (int8_t j = -neighbor_col; j <= neighbor_col; j++)
 99 |                     {
100 |                         neighbor.first = i;
101 |                         neighbor.second = j;
102 |                         neighborIterator.push_back(neighbor);
103 |                     }
104 |                 }
105 |                 float resolution = (float)(MAX_VERT_ANGLE - MIN_VERT_ANGLE) / (float)(VERT_SCAN - 1);
106 |                 for (int i = 0; i < VERT_SCAN; i++)
107 |                     vert_angles_.push_back(MIN_VERT_ANGLE + i * resolution);
108 |             }
109 | 
110 |             // void resetParameters(){
111 |             //     std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
112 |             //     labelMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32S, cv::Scalar::all(0));
113 |             //     rangeMat = cv::Mat(VERT_SCAN, HORZ_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
114 |             //     labelCount = 1;
115 |             // }
116 | 
117 | 
118 |             void setParams(int _vert_scan, int _horz_scan, float _min_range, float _max_range,
119 |                             float _min_vert_angle, float _max_vert_angle,
120 |                             float _horz_merge_thres, float _vert_merge_thres, int _vert_scan_size,
121 |                             int _horz_scan_size, int _horz_extension_size, int _horz_skip_size,
122 |                             boost::optional<int> _min_cluster_size=boost::none, 
123 |                             boost::optional<int> _max_cluster_size=boost::none) {
124 |    
125 |                 std::cout<<""<<std::endl;
126 |                 ROS_WARN("Set ObjectSeg Parameters");
127 | 
128 |                 // range_mat_.resize(VERT_SCAN, vector<Point>(HORZ_SCAN));
129 | 
130 |                 // valid_cnt_.resize(VERT_SCAN, 0);
131 |                 // nodes_.resize(VERT_SCAN, vector<AOSNode>());
132 | 
133 |                 // if (_min_cluster_size)
134 |                 //     MIN_CLUSTER_SIZE = _min_cluster_size;
135 |             
136 |                 // if (_max_cluster_size)
137 |                 //     MAX_CLUSTER_SIZE = _max_cluster_size;
138 |             }
139 | 
140 |             void GTV(boost::shared_ptr<pcl::PointCloud<PointT>> cloud_in, std::vector<std::vector<int>> &clusterIndices)
141 |             {
142 |                 // 0. reset
143 |                 allocateMemory();
144 |                 clusterIndices.clear();
145 | 
146 |                 // 1. do spherical projection
147 |                 sphericalProjection(cloud_in);
148 | 
149 |                 // 2. begin clustering
150 |                 for (size_t i = 0; i < VERT_SCAN; ++i)
151 |                 {
152 |                     for (size_t j = 0; j < HORZ_SCAN; ++j)
153 |                     {
154 |                         if (labelMat.at<int>(i, j) == -1)
155 |                             labelComponents(i, j, clusterIndices);
156 |                     }
157 |                 }
158 |                 // return clusterIndices;
159 |             }
160 | 
161 |             void sphericalProjection(boost::shared_ptr<pcl::PointCloud<PointT>> cloud_in) 
162 |             {
163 |                 float range;
164 |                 size_t rowIdn, columnIdn, index, cloudSize; 
165 |                 PointT cpt;
166 | 
167 |                 cloudSize = cloud_in->points.size();
168 |                 int org_index = 0;
169 | 
170 |                 for (size_t i = 0; i < cloudSize; ++i)
171 |                 {
172 |                     cpt.x = cloud_in->points[i].x;
173 |                     cpt.y = cloud_in->points[i].y;
174 |                     cpt.z = cloud_in->points[i].z;
175 |                     cpt.intensity = cloud_in->points[i].intensity;
176 |                     cpt.id = cloud_in->points[i].id;
177 |                     cpt.label = cloud_in->points[i].label;
178 | 
179 |                     bool is_nan = std::isnan(cpt.x) || std::isnan(cpt.y) || std::isnan(cpt.z);
180 |                     if (is_nan)
181 |                         continue;
182 | 
183 |                     //find the row and column index in the iamge for this point
184 |                     rowIdn = getRowIdx(cpt);
185 | 
186 |                     if (rowIdn < 0 || rowIdn >= VERT_SCAN)
187 |                     continue;
188 | 
189 |                     columnIdn = getColIdx(cpt);
190 | 
191 |                     if (columnIdn < 0 || columnIdn >= HORZ_SCAN)
192 |                     continue;
193 | 
194 |                     range = sqrt(cpt.x * cpt.x + cpt.y * cpt.y + cpt.z * cpt.z);
195 | 
196 |                     labelMat.at<int>(rowIdn, columnIdn) = -1;
197 |                     rangeMat.at<float>(rowIdn, columnIdn) = range;
198 | 
199 |                     index = columnIdn + rowIdn * HORZ_SCAN;
200 |                     index_v[index] = org_index;
201 | 
202 |                     fullCloud->points[index] = cpt;
203 |                     org_index++;
204 |                 }
205 |             }
206 | 
207 |             int getRowIdx(PointT pt)
208 |             {
209 |                 float angle = atan2(pt.z, sqrt(pt.x * pt.x + pt.y * pt.y)) * 180 / M_PI;
210 | 
211 |                 auto iter_geq = std::lower_bound(vert_angles_.begin(), vert_angles_.end(), angle);
212 |                 int row_idx;
213 | 
214 |                 if (iter_geq == vert_angles_.begin())
215 |                 {
216 |                     row_idx = 0;
217 |                 }
218 |                 else
219 |                 {
220 |                     float a = *(iter_geq - 1);
221 |                     float b = *(iter_geq);
222 |                     if (fabs(angle - a) < fabs(angle - b))
223 |                     {
224 |                     row_idx = iter_geq - vert_angles_.begin() - 1;
225 |                     }
226 |                     else
227 |                     {
228 |                     row_idx = iter_geq - vert_angles_.begin();
229 |                     }
230 |                 }
231 |                 return row_idx;
232 |             }
233 | 
234 |             int getColIdx(PointT pt)
235 |             {
236 |                 float horizonAngle = atan2(pt.x, pt.y) * 180 / M_PI;
237 |                 static float ang_res_x = 360.0 / float(HORZ_SCAN);
238 |                 int col_idx = -round((horizonAngle - 90.0) / ang_res_x) + HORZ_SCAN / 2;
239 |                 if (col_idx >= HORZ_SCAN)
240 |                     col_idx -= HORZ_SCAN;
241 |                 return col_idx;
242 |             }
243 | 
244 |             void labelComponents(int row, int col, std::vector<std::vector<int>> &clusterIndices)
245 |             {
246 |                 // use std::queue std::vector std::deque will slow the program down greatly
247 |                 int fromIndX, fromIndY, thisIndX, thisIndY;
248 |                 float d1, d2, alpha, angle_c;
249 |                 
250 |                 queueIndX[0] = row;
251 |                 queueIndY[0] = col;
252 |                 int queueSize = 1;
253 |                 int queueStartInd = 0;
254 |                 int queueEndInd = 1;
255 | 
256 |                 allPushedIndX[0] = row;
257 |                 allPushedIndY[0] = col;
258 |                 int allPushedIndSize = 1;
259 | 
260 |                 std::vector<int> clusterIndice;
261 | 
262 |                 //standard BFS 
263 |                 while (queueSize > 0)
264 |                 {
265 |                     // Pop point
266 |                     fromIndX = queueIndX[queueStartInd];
267 |                     fromIndY = queueIndY[queueStartInd];
268 |                     --queueSize;
269 |                     ++queueStartInd;
270 |                     // Mark popped point, The initial value of labelCount is 1.
271 |                     labelMat.at<int>(fromIndX, fromIndY) = labelCount;
272 | 
273 |                     // Loop through all the neighboring grids of popped grid, neighbor=[[-1,0];[0,1];[0,-1];[1,0]]
274 |                     for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter)
275 |                     {
276 |                         // new index
277 |                         thisIndX = fromIndX + (*iter).first;
278 |                         thisIndY = fromIndY + (*iter).second;
279 | 
280 |                         // index should be within the boundary
281 |                         if (thisIndX < 0 || thisIndX >= VERT_SCAN)
282 |                             continue;
283 |                         // at range image margin (left or right side)
284 |                         if (thisIndY < 0)
285 |                             thisIndY = HORZ_SCAN - 1;
286 |                         if (thisIndY >= HORZ_SCAN)
287 |                             thisIndY = 0;
288 |                         // prevent infinite loop (caused by put already examined point back)
289 |                         if (labelMat.at<int>(thisIndX, thisIndY) != -1)
290 |                             continue;
291 | 
292 |                         /*---------------------------condition----------------------------*/
293 |                         d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
294 |                                         rangeMat.at<float>(thisIndX, thisIndY));
295 |                         d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
296 |                                         rangeMat.at<float>(thisIndX, thisIndY));
297 | 
298 |                         PointT pt_from = fullCloud->points[fromIndY + fromIndX * HORZ_SCAN];
299 |                         PointT pt_this = fullCloud->points[thisIndY + thisIndX * HORZ_SCAN];
300 | 
301 |                         bool same_cluster = false;
302 | 
303 |                         if (pt_from.label == 10 && pt_this.label == 10)
304 |                         {
305 |                             if (pt_from.id == pt_this.id)
306 |                                 same_cluster = true;
307 |                         }
308 | 
309 |                         if (pt_from.label != 10)
310 |                         {
311 |                             float dist = (pt_from.x - pt_this.x) * (pt_from.x - pt_this.x) 
312 |                                         + (pt_from.y - pt_this.y) * (pt_from.y - pt_this.y) 
313 |                                         + (pt_from.z - pt_this.z) * (pt_from.z - pt_this.z);
314 | 
315 |                             if (pt_from.label == pt_this.label && pt_from.id == pt_this.id && dist < 0.35 * 0.35)
316 |                                 same_cluster = true;
317 |                         }
318 | 
319 |                         if (same_cluster)
320 |                         {
321 |                             queueIndX[queueEndInd] = thisIndX;
322 |                             queueIndY[queueEndInd] = thisIndY;
323 |                             ++queueSize;
324 |                             ++queueEndInd;
325 | 
326 |                             labelMat.at<int>(thisIndX, thisIndY) = labelCount;
327 | 
328 |                             clusterIndice.push_back(index_v[thisIndY + thisIndX * HORZ_SCAN]);
329 | 
330 |                             allPushedIndX[allPushedIndSize] = thisIndX;
331 |                             allPushedIndY[allPushedIndSize] = thisIndY;
332 |                             ++allPushedIndSize;
333 |                         }
334 |                     }
335 |                 }
336 | 
337 |                 // check if this segment is valid
338 |                 bool feasibleSegment = false;
339 | 
340 |                 if (allPushedIndSize >= 5)
341 |                     feasibleSegment = true;
342 |                 
343 |                 // segment is valid, mark these points
344 |                 if (feasibleSegment == true){
345 |                     ++labelCount;
346 |                     clusterIndices.push_back(clusterIndice);
347 |                 }
348 |                 else{
349 |                     // segment is invalid, mark these points
350 |                     for (size_t i = 0; i < allPushedIndSize; ++i){
351 |                     labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 0;
352 |                     }
353 |                 }
354 |             }
355 |     };
356 | }
357 | 
358 | #endif


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