├── .clang-format
├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── assets
├── example.cfg
├── labels.xml
├── semantic_kitti.cfg
├── semantic_kitti_dense.cfg
├── settings.cfg
└── voxelizer.png
├── external
└── glow.cmake
└── src
├── VoxelizerMainFrame.ui
├── data
├── Pointcloud.h
├── VoxelGrid.cpp
├── VoxelGrid.h
├── common.h
├── geometry.h
├── kitti_utils.cpp
├── kitti_utils.h
├── label_utils.cpp
├── label_utils.h
├── misc.cpp
├── misc.h
├── voxelize_utils.cpp
└── voxelize_utils.h
├── gen_data.cpp
├── rv
├── Stopwatch.cpp
├── Stopwatch.h
├── string_utils.cpp
└── string_utils.h
├── shaders
├── blinnphong.frag
├── color.glsl
├── draw_points.vert
├── draw_pose.geom
├── draw_voxels.geom
├── draw_voxels.vert
├── empty.frag
├── empty.vert
├── passthrough.frag
└── quad.geom
├── voxelizer.cpp
└── widget
├── KittiReader.cpp
├── KittiReader.h
├── Mainframe.cpp
├── Mainframe.h
├── Viewport.cpp
└── Viewport.h
/.clang-format:
--------------------------------------------------------------------------------
1 | BasedOnStyle: Google
2 | IndentWidth: 2
3 |
4 | ---
5 | Language: Cpp
6 | ColumnLimit: 120
7 | Standard: Cpp11
8 |
9 |
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 |
2 | # Created by https://www.gitignore.io/api/c,qt,cmake
3 |
4 | ### C ###
5 | # Object files
6 | *.o
7 | *.ko
8 | *.obj
9 | *.elf
10 |
11 | # Precompiled Headers
12 | *.gch
13 | *.pch
14 |
15 | # Libraries
16 | *.lib
17 | *.a
18 | *.la
19 | *.lo
20 |
21 | # Shared objects (inc. Windows DLLs)
22 | *.dll
23 | *.so
24 | *.so.*
25 | *.dylib
26 |
27 | # Executables
28 | *.exe
29 | *.out
30 | *.app
31 | *.i*86
32 | *.x86_64
33 | *.hex
34 |
35 | # Debug files
36 | *.dSYM/
37 |
38 |
39 | ### Qt ###
40 | # C++ objects and libs
41 |
42 | *.slo
43 | *.lo
44 | *.o
45 | *.a
46 | *.la
47 | *.lai
48 | *.so
49 | *.dll
50 | *.dylib
51 |
52 | # Qt-es
53 |
54 | /.qmake.cache
55 | /.qmake.stash
56 | *.pro.user
57 | *.pro.user.*
58 | *.qbs.user
59 | *.qbs.user.*
60 | *.moc
61 | moc_*.cpp
62 | qrc_*.cpp
63 | ui_*.h
64 | Makefile*
65 | *build-*
66 |
67 | # QtCreator
68 |
69 | *.autosave
70 |
71 | # QtCtreator Qml
72 | *.qmlproject.user
73 | *.qmlproject.user.*
74 |
75 | # QtCtreator CMake
76 | CMakeLists.txt.user
77 |
78 |
79 |
80 | ### CMake ###
81 | CMakeCache.txt
82 | CMakeFiles
83 | CMakeScripts
84 | Makefile
85 | cmake_install.cmake
86 | install_manifest.txt
87 |
88 | ### Eclipse ###
89 | .cproject
90 | .project
91 |
92 | ### gedit backup ###
93 | *~
94 |
95 | .settings
96 |
97 | build
98 | bin
99 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.16...3.25)
2 |
3 | PROJECT(voxelizer)
4 |
5 | set(OPENGL_VERSION "430" CACHE STRING "" FORCE)
6 | set(ENABLE_NVIDIA_EXT On CACHE BOOL "" FORCE)
7 |
8 | include(external/glow.cmake)
9 |
10 | set(CMAKE_C_FLAGS "-Wall -O3 -g")
11 | set(CMAKE_CXX_FLAGS "-Wall -O3 -g")
12 |
13 | find_package(OpenGL REQUIRED)
14 | find_package(Qt5Xml REQUIRED)
15 | find_package(Qt5Gui REQUIRED)
16 | find_package(Qt5OpenGL REQUIRED)
17 | find_package(GLEW REQUIRED)
18 | find_package(Boost REQUIRED COMPONENTS filesystem system)
19 |
20 |
21 | include_directories(${catkin_INCLUDE_DIRS} src/ ${QT5_INCLUDE_DIRS} ${GLEW_INCLUDE_DIRS} /usr/include/eigen3 /usr/local/include)
22 |
23 | set(CMAKE_INCLUDE_CURRENT_DIR ON) # needs to be activated for qt generated files in build directory.
24 | set(CMAKE_AUTOMOC ON)
25 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -Wall ${CMAKE_CXX_FLAGS}")
26 | set(CMAKE_CXX_FLAGS "-UDEBUG_GL -UDEBUG -DNDEBUG -g2 ${CMAKE_CXX_FLAGS}")
27 |
28 | # since now everything resides in "bin", we have to copy some stuff.
29 | set(CMAKE_BUILD_TYPE Release)
30 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
31 |
32 | configure_file(assets/labels.xml ${CMAKE_SOURCE_DIR}/bin/labels.xml COPYONLY)
33 | configure_file(assets/settings.cfg ${CMAKE_SOURCE_DIR}/bin/settings.cfg COPYONLY)
34 |
35 | QT5_WRAP_UI(UI_HDRS
36 | src/VoxelizerMainFrame.ui)
37 |
38 | set(VIZ_SHADER_SRC ${CMAKE_BINARY_DIR}/visualization_shaders.cpp)
39 |
40 | COMPILE_SHADERS(${VIZ_SHADER_SRC}
41 | src/shaders/color.glsl
42 |
43 | src/shaders/empty.vert
44 | src/shaders/empty.frag
45 | src/shaders/quad.geom
46 | src/shaders/passthrough.frag
47 |
48 | src/shaders/draw_pose.geom
49 | src/shaders/draw_voxels.vert
50 | src/shaders/draw_voxels.geom
51 | src/shaders/draw_points.vert
52 | src/shaders/blinnphong.frag
53 | )
54 |
55 | add_executable(voxelizer
56 | ${UI_HDRS}
57 | ${VIZ_SHADER_SRC}
58 |
59 | src/data/label_utils.cpp
60 | src/data/kitti_utils.cpp
61 | src/data/VoxelGrid.cpp
62 | src/rv/string_utils.cpp
63 | src/rv/Stopwatch.cpp
64 | src/data/misc.cpp
65 | src/voxelizer.cpp
66 | src/widget/Mainframe.cpp
67 | src/widget/Viewport.cpp
68 | src/widget/KittiReader.cpp
69 | src/data/voxelize_utils.cpp
70 |
71 | )
72 |
73 | add_executable(gen_data
74 | src/data/label_utils.cpp
75 | src/data/kitti_utils.cpp
76 | src/data/VoxelGrid.cpp
77 | src/rv/string_utils.cpp
78 | src/rv/Stopwatch.cpp
79 | src/data/misc.cpp
80 | src/gen_data.cpp
81 |
82 | src/widget/KittiReader.cpp
83 | src/data/voxelize_utils.cpp
84 | )
85 |
86 | target_link_libraries(voxelizer glow::glow glow::util pthread Qt5::Xml Qt5::OpenGL Qt5::Widgets)
87 | target_link_libraries(gen_data glow::glow glow::util pthread Qt5::Xml Qt5::OpenGL Qt5::Widgets)
88 |
89 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2019 Jens Behley
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Voxelize point clouds
2 |
3 | Tool to voxelize annotated point clouds.
4 |
5 | 
6 |
7 | ## Dependencies
8 |
9 | * Eigen >= 3.2
10 | * boost >= 1.54
11 | * QT >= 5.2
12 |
13 | ## Build
14 |
15 | On Ubuntu 22.04/20.04, most of the dependencies can be installed from the package manager:
16 | ```bash
17 | sudo apt install build-essential cmake libeigen3-dev libboost-all-dev qtbase5-dev libglew-dev
18 | ```
19 |
20 | Use the usual cmake out-of-source build process:
21 | ```bash
22 | mkdir build && cd build
23 | cmake ..
24 | ```
25 | Now the project root directory should contain a `bin` directory containing the voxelizer.
26 |
27 | ## Usage
28 |
29 |
30 | In the `bin` directory, just run `./voxelizer` to start the voxelizer.
31 |
32 |
33 | In the `settings.cfg` files you can change the followings options:
34 |
35 |
36 | max scans: 500 # number of scans to load for a tile. (should be maybe 1000), but this currently very memory consuming.
37 | min range: 2.5 # minimum distance of points to consider.
38 | max range: 50.0 # maximum distance of points in the point cloud.
39 | ignore: 0,250,251,252,253,254 # label ids of labels that should be ignored when building a voxelgrid.
40 |
41 |
42 | To generate the data by iterating over a sequence directory, call `./gen_data` in the `bin` directory.
43 |
44 |
45 |
46 | ## Folder structure
47 |
48 | When loading a dataset, the data must be organized as follows:
49 |
50 |
51 | point cloud folder
52 | ├── velodyne/ -- directory containing ".bin" files with Velodyne point clouds.
53 | ├── labels/ -- label directory, will be generated if not present.
54 | ├── calib.txt -- calibration of velodyne vs. camera. needed for projection of point cloud into camera.
55 | └── poses.txt -- file containing the poses of every scan.
56 |
57 |
--------------------------------------------------------------------------------
/assets/example.cfg:
--------------------------------------------------------------------------------
1 | max scans: 100
2 | min range: 2.5
3 | max range: 25.0
4 | voxel size: 0.3
5 | prior scans: 0
6 | past scans: 10
7 | min extent: [0, -20, -2]
8 | max extent: [40, 20, 1]
9 | ignore: [0,250,251,252,253,254]
10 | # join: [{70: [71]}, {40: [44]}, {112:[130,140,135]}]
11 | stride num: 5
12 | stride distance: 0.0
13 |
14 |
--------------------------------------------------------------------------------
/assets/labels.xml:
--------------------------------------------------------------------------------
1 |
3 |
4 |
5 | 0
6 | unlabeled
7 | all points with no special label.
8 | 0 0 0
9 | void
10 | unlabeled
12 |
13 |
14 |
15 | 1
16 | outlier
17 | all points which are outliers, i.e., caused by reflection, etc..
18 | 255 0 0
19 | void
20 | outlier
22 |
23 |
24 |
25 | 10
26 | car
27 | This includes cars, jeeps, SUVs, vans with a continuous body shape (i.e. the driver cabin and cargo compartment are one)
28 | 150 170 200
29 | vehicle
30 | vehicle
31 | car
32 | 252
33 |
34 |
35 | 110
36 | autoauto
37 | This label represents automatically generated cars.
38 |
39 | 100 0 0
40 | vehicle
41 | vehicle
42 | autoauto
43 | 210
44 |
45 |
46 | 11
47 | bicycle
48 | Includes bicycles without the cyclist or possibly other passengers. The cyclist and passengers recieve the label cyclist.
49 | 100 230 245
50 | vehicle
51 | vehicle
52 | bicycle
53 |
54 |
55 | 15
56 | motorcycle
57 | This includes motorcycles, mopeds without the driver or other passengers. Both driver and passengers recieve the label motorcyclist.
58 | 30 60 150
59 | vehicle
60 | vehicle
61 | motorcycle
62 |
63 |
64 | 16
65 | on rails
66 | All vehicles moving on rails, e.g., trams, trains.
67 | 0 0 255
68 | vehicle
69 | vehicle
70 | on rails
71 | 256
72 |
73 |
74 | 13
75 | bus
76 | Includes buses that are intended for 9+ persons for public or long-distance transport.
77 | 100 80 250
78 | vehicle
79 | vehicle
80 | bus
81 | 257
82 |
83 |
84 | 18
85 | truck
86 | This includes trucks, vans with a body that is separate from the driver cabin, pickup trucks, as well as their attached trailers.
87 | 80 30 180
88 | vehicle
89 | vehicle
90 | truck
91 | 258
92 |
93 |
94 | 20
95 | other-vehicle
96 | Cravans, Trailers and fallback category for vehicles not explicitly defined otherwise in meta category level vehicle.
97 | 0 0 255
98 | vehicle
99 | vehicle
100 | other-vehicle
101 | 259
102 |
103 |
104 |
105 | 30
106 | person
107 | Persons moving by their own legs, sitting, or any unusual pose, but not meant to drive a vehicle.
108 | 255 30 30
109 | human
110 | person
112 | 254
113 |
114 |
115 | 31
116 | bicyclist
117 | Humans driving a bicycle.
118 | 255 40 200
119 | human
120 | rider
121 | bicyclist
122 | 253
123 |
124 |
125 | 32
126 | motorcyclist
127 | Persons riding a motorcycle.
128 | 150 30 90
129 | human
130 | rider
131 | motorcyclist
132 | 255
133 |
134 |
135 |
136 | 40
137 | road
138 | 255 0 255
140 | ground
141 | flat
142 | road
143 |
144 |
145 |
146 | 44
147 | parking
148 | 255 150 255
150 | ground
151 | flat
152 | parking
153 |
154 |
163 |
164 |
173 |
174 | 48
175 | sidewalk
176 | Areas used mainly by pedestrians, bicycles, but not meant for driving.
177 | 75 0 75
178 | ground
179 | flat
180 | sidewalk
181 |
182 |
183 | 49
184 | other-ground
185 | Other areas that are not used by pedestrians or meant for driving.
186 | 175 0 75
187 | ground
188 | flat
189 | other-ground
190 |
191 |
192 |
193 | 50
194 | building
195 | Building walls, doors, etc.
196 | 255 200 0
197 | structure
198 | structure
199 | building
200 |
201 |
202 | 51
203 | fence
204 | fences, small walls, crash barriers, etc.
205 | 255 120 50
206 | structure
207 | structure
208 | other structure
209 |
210 |
211 |
212 | 52
213 | other-structure
214 | 255 150 0
216 | structure
217 | structure
218 | other-structure
219 |
220 |
221 |
230 |
231 | 60
232 | lane marking
233 | Other lane markings, including dashed and
234 | solid lane seperation makrings, stop lines, text, arrows,
235 | or restricted areas.
236 | 150 255 170
237 | ground
238 | lane marking
240 |
241 |
242 | 70
243 | vegetation
244 | Trees, and other forms of vertical growing vegetation.
245 | 0 175 0
246 | nature
247 | vegetation
249 |
250 |
251 | 71
252 | trunk
253 | tree trunks.
254 | 135 60 0
255 | nature
256 | trunk
258 |
259 |
260 | 72
261 | terrain
262 | grass and all other types of horizontal spreading vegetation, including soil.
263 | 150 240 80
264 | nature
265 | terrain
267 |
268 |
269 |
270 | 80
271 | pole
272 | thin and elongated, typically vertically oriented poles, eg. sing or traffic signs.
273 | 255 240 150
274 | object
275 | support
276 | pole
277 |
278 |
279 | 81
280 | traffic sign
281 | traffic signs without pole.
282 | 255 0 0
283 | object
284 | traffic-sign
285 | traffic sign
286 |
287 |
288 |
297 |
306 |
307 |
308 | 99
309 | other-object
310 | other small/convex objects on the street, where we now have not a specific label.
311 | 50 255 255
312 | object
313 | object
314 | other-object
315 |
316 |
317 |
318 |
--------------------------------------------------------------------------------
/assets/semantic_kitti.cfg:
--------------------------------------------------------------------------------
1 | max scans: 100
2 | min range: 2.5
3 | max range: 70
4 | prior scans: 0
5 | past scans: 70
6 |
7 | min extent: [0, -25.6, -2]
8 | max extent: [51.2, 25.6, 4.4]
9 | voxel size: 0.2
10 |
11 | # Join "unlabeled" with "outlier"
12 | join: [{0: [1]}]
13 | # join: [{0: [1]}, {10: [252]}, {20: [13,16,256,257,259]}, {18: [258]}, {30: [254]}, {31: [253]}, {32: [255]}]
14 |
15 | stride num: 5
16 | stride distance: 0.0
--------------------------------------------------------------------------------
/assets/semantic_kitti_dense.cfg:
--------------------------------------------------------------------------------
1 | max scans: 100
2 | min range: 2.5
3 | max range: 70
4 | prior scans: 0
5 | past scans: 70
6 |
7 | min extent: [0, -25.6, -2]
8 | max extent: [51.2, 25.6, 4.4]
9 | voxel size: 0.2
10 |
11 | # Join "unlabeled" with "outlier"
12 | join: [{0: [1]}]
13 |
14 | stride num: 1
15 | stride distance: 0.0
--------------------------------------------------------------------------------
/assets/settings.cfg:
--------------------------------------------------------------------------------
1 | max scans: 100
2 | min range: 2.5
3 | max range: 70
4 | prior scans: 0
5 | past scans: 70
6 |
7 | min extent: [0, -25.6, -2]
8 | max extent: [51.2, 25.6, 4.4]
9 | voxel size: 0.2
10 |
11 | join: [{0: [1]}]
12 | ignore: [0,2,250,251,252,253,254]
--------------------------------------------------------------------------------
/assets/voxelizer.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jbehley/voxelizer/0b19167f8fe1c292951fb2cbaae9e1e7fd99fc01/assets/voxelizer.png
--------------------------------------------------------------------------------
/external/glow.cmake:
--------------------------------------------------------------------------------
1 | message(STATUS "Fetching glow.")
2 |
3 | include(FetchContent)
4 | FetchContent_Declare(glow GIT_REPOSITORY https://github.com/jbehley/glow.git)
5 |
6 | FetchContent_MakeAvailable(glow)
7 |
8 | # TODO: Can we get rid of this explicit inclusion?
9 | include(GenCppFile)
10 | include(GlowShaderCompilation)
--------------------------------------------------------------------------------
/src/VoxelizerMainFrame.ui:
--------------------------------------------------------------------------------
1 |
2 |
3 | MainWindow
4 |
5 |
6 |
7 | 0
8 | 0
9 | 972
10 | 674
11 |
12 |
13 |
14 | Qt::WheelFocus
15 |
16 |
17 | MainWindow
18 |
19 |
20 |
21 | -
22 |
23 |
24 |
25 | 250
26 | 16777215
27 |
28 |
29 |
30 | QTabWidget::North
31 |
32 |
33 | QTabWidget::Rounded
34 |
35 |
36 | 0
37 |
38 |
39 |
40 | Voxels
41 |
42 |
43 | -
44 |
45 |
46 | 5
47 |
48 |
49 | 5
50 |
51 |
52 | 5
53 |
54 |
55 | 5
56 |
57 | -
58 |
59 |
60 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter
61 |
62 |
63 | 0.010000000000000
64 |
65 |
66 | 0.100000000000000
67 |
68 |
69 | 0.500000000000000
70 |
71 |
72 |
73 | -
74 |
75 |
76 | voxel size
77 |
78 |
79 |
80 | -
81 |
82 |
83 | max range
84 |
85 |
86 |
87 | -
88 |
89 |
90 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter
91 |
92 |
93 | 50.000000000000000
94 |
95 |
96 |
97 | -
98 |
99 |
100 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter
101 |
102 |
103 | 0
104 |
105 |
106 | 0
107 |
108 |
109 |
110 | -
111 |
112 |
113 | #prior scans
114 |
115 |
116 |
117 | -
118 |
119 |
120 | #past scans
121 |
122 |
123 |
124 | -
125 |
126 |
127 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter
128 |
129 |
130 | 1
131 |
132 |
133 | 10
134 |
135 |
136 |
137 | -
138 |
139 |
140 | train
141 |
142 |
143 | true
144 |
145 |
146 |
147 | -
148 |
149 |
150 | show points
151 |
152 |
153 | true
154 |
155 |
156 |
157 | -
158 |
159 |
160 | show voxels
161 |
162 |
163 |
164 | -
165 |
166 |
167 | occluded voxels
168 |
169 |
170 |
171 | -
172 |
173 |
174 | invalid voxels
175 |
176 |
177 |
178 | -
179 |
180 |
181 | test
182 |
183 |
184 |
185 |
186 |
187 | -
188 |
189 |
190 | 5
191 |
192 |
193 | 5
194 |
195 |
196 | 5
197 |
198 |
199 | 5
200 |
201 | -
202 |
203 |
204 | y
205 |
206 |
207 |
208 | -
209 |
210 |
211 | z
212 |
213 |
214 |
215 | -
216 |
217 |
218 | 1
219 |
220 |
221 | -1000.000000000000000
222 |
223 |
224 | 1000.000000000000000
225 |
226 |
227 |
228 | -
229 |
230 |
231 | min
232 |
233 |
234 |
235 | -
236 |
237 |
238 | max
239 |
240 |
241 |
242 | -
243 |
244 |
245 | x
246 |
247 |
248 |
249 | -
250 |
251 |
252 | 1
253 |
254 |
255 | -1000.000000000000000
256 |
257 |
258 | 1000.000000000000000
259 |
260 |
261 |
262 | -
263 |
264 |
265 | 1
266 |
267 |
268 | -1000.000000000000000
269 |
270 |
271 | 1000.000000000000000
272 |
273 |
274 |
275 | -
276 |
277 |
278 | 1
279 |
280 |
281 | -1000.000000000000000
282 |
283 |
284 | 1000.000000000000000
285 |
286 |
287 |
288 | -
289 |
290 |
291 | 1
292 |
293 |
294 | -1000.000000000000000
295 |
296 |
297 | 1000.000000000000000
298 |
299 |
300 |
301 | -
302 |
303 |
304 | 1
305 |
306 |
307 | -1000.000000000000000
308 |
309 |
310 | 1000.000000000000000
311 |
312 |
313 |
314 | -
315 |
316 |
317 | Extent
318 |
319 |
320 |
321 |
322 |
323 | -
324 |
325 |
326 | Qt::Vertical
327 |
328 |
329 |
330 | 20
331 | 40
332 |
333 |
334 |
335 |
336 |
337 |
338 |
339 |
340 | Visuals
341 |
342 |
343 | -
344 |
345 |
346 | 5
347 |
348 |
349 | 5
350 |
351 |
352 | 5
353 |
354 |
355 | 5
356 |
357 | -
358 |
359 |
360 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter
361 |
362 |
363 | 1
364 |
365 |
366 | 20
367 |
368 |
369 |
370 | -
371 |
372 |
373 | point size
374 |
375 |
376 |
377 |
378 |
379 | -
380 |
381 |
382 | color
383 |
384 |
385 |
386 | -
387 |
388 |
389 | remission
390 |
391 |
392 |
393 | -
394 |
395 |
396 | Qt::Vertical
397 |
398 |
399 |
400 | 20
401 | 40
402 |
403 |
404 |
405 |
406 |
407 |
408 |
409 |
410 | -
411 |
412 |
413 | 5
414 |
415 |
416 | 5
417 |
418 |
419 | 5
420 |
421 |
422 | 5
423 |
424 | -
425 |
426 |
427 |
428 | 0
429 | 0
430 |
431 |
432 |
433 |
434 | 640
435 | 480
436 |
437 |
438 |
439 | Qt::WheelFocus
440 |
441 |
442 |
443 | -
444 |
445 |
446 | 5
447 |
448 |
449 | 5
450 |
451 |
452 | 5
453 |
454 |
455 | 5
456 |
457 | -
458 |
459 |
460 | false
461 |
462 |
463 |
464 | 25
465 | 25
466 |
467 |
468 |
469 |
470 | 25
471 | 25
472 |
473 |
474 |
475 | <
476 |
477 |
478 |
479 | -
480 |
481 |
482 | Qt::Horizontal
483 |
484 |
485 | QSlider::TicksBelow
486 |
487 |
488 | 100
489 |
490 |
491 |
492 | -
493 |
494 |
495 | false
496 |
497 |
498 |
499 | 25
500 | 25
501 |
502 |
503 |
504 |
505 | 25
506 | 25
507 |
508 |
509 |
510 | >
511 |
512 |
513 |
514 |
515 |
516 |
517 |
518 |
519 |
520 |
522 |
523 | toolBar
524 |
525 |
526 | TopToolBarArea
527 |
528 |
529 | false
530 |
531 |
537 |
538 |
539 | &Open...
540 |
541 |
542 |
543 |
544 | true
545 |
546 |
547 | Paint
548 |
549 |
550 |
551 |
552 | true
553 |
554 |
555 | Cylinder
556 |
557 |
558 |
559 |
560 | NewCylinder
561 |
562 |
563 |
564 |
565 | &Save...
566 |
567 |
568 |
569 |
570 | &Save
571 |
572 |
573 |
574 |
575 | &Labels...
576 |
577 |
578 |
579 |
580 | Tiles
581 |
582 |
583 |
584 |
585 | true
586 |
587 |
588 | record
589 |
590 |
591 |
592 |
593 | snapshot
594 |
595 |
596 |
597 |
598 |
599 | Viewport
600 | QWidget
601 |
602 | 1
603 |
604 |
605 |
608 |
--------------------------------------------------------------------------------
/src/data/Pointcloud.h:
--------------------------------------------------------------------------------
1 | #ifndef SRC_DATA_POINTCLOUD_H_
2 | #define SRC_DATA_POINTCLOUD_H_
3 |
4 | #include
5 | #include "geometry.h"
6 | #include
7 |
8 | /** \brief a laserscan with possibly remission.
9 | *
10 | * \author behley
11 | */
12 |
13 | class Laserscan {
14 | public:
15 | void clear() {
16 | points.clear();
17 | remissions.clear();
18 | }
19 | uint32_t size() const { return points.size(); }
20 | bool hasRemissions() const { return (points.size() > 0) && (points.size() == remissions.size()); }
21 |
22 | Eigen::Matrix4f pose;
23 | std::vector points;
24 | std::vector remissions;
25 | };
26 |
27 | #endif /* SRC_DATA_POINTCLOUD_H_ */
28 |
--------------------------------------------------------------------------------
/src/data/VoxelGrid.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 | #include
4 |
5 | void VoxelGrid::initialize(float resolution, const Eigen::Vector4f& min, const Eigen::Vector4f& max) {
6 | clear();
7 |
8 | resolution_ = resolution;
9 | sizex_ = std::ceil((max.x() - min.x()) / resolution_);
10 | sizey_ = std::ceil((max.y() - min.y()) / resolution_);
11 | sizez_ = std::ceil((max.z() - min.z()) / resolution_);
12 |
13 | voxels_.resize(sizex_ * sizey_ * sizez_);
14 | // ensure that min, max are always inside the voxel grid.
15 | float ox = min.x() - 0.5 * (sizex_ * resolution - (max.x() - min.x()));
16 | float oy = min.y() - 0.5 * (sizey_ * resolution - (max.y() - min.y()));
17 | float oz = min.z() - 0.5 * (sizez_ * resolution - (max.z() - min.z()));
18 | offset_ = Eigen::Vector4f(ox, oy, oz, 1);
19 |
20 | // center_.head(3) = 0.5 * (max - min) + min;
21 | // center_[3] = 0;
22 |
23 | occlusions_.resize(sizex_ * sizey_ * sizez_);
24 | occludedBy_.resize(sizex_ * sizey_ * sizez_);
25 |
26 | std::cout << "[Voxelgrid::initialize] " << resolution_ << "; num. voxels = [" << sizex_ << ", " << sizey_ << ", "
27 | << sizez_ << "], maxExtent = " << max.transpose() << ", minExtent" << min.transpose() << std::endl;
28 | }
29 |
30 | void VoxelGrid::clear() {
31 | for (auto idx : occupied_) {
32 | voxels_[idx].count = 0;
33 | voxels_[idx].labels.clear();
34 | }
35 |
36 | // for (auto idx : occluded_) {
37 | // voxels_[idx].count = 0;
38 | // voxels_[idx].labels.clear();
39 | // }
40 |
41 | occupied_.clear();
42 | // occluded_.clear();
43 | }
44 |
45 | void VoxelGrid::insert(const Eigen::Vector4f& p, uint32_t label) {
46 | Eigen::Vector4f tp = p - offset_;
47 | int32_t i = std::floor(tp.x() / resolution_);
48 | int32_t j = std::floor(tp.y() / resolution_);
49 | int32_t k = std::floor(tp.z() / resolution_);
50 |
51 | if ((i >= int32_t(sizex_)) || (j >= int32_t(sizey_)) || (k >= int32_t(sizez_))) return;
52 | if ((i < 0) || (j < 0) || (k < 0)) return;
53 |
54 | int32_t gidx = index(i, j, k);
55 | if (gidx < 0 || gidx >= int32_t(voxels_.size())) return;
56 |
57 | if (voxels_[gidx].count == 0) occupied_.push_back(gidx);
58 |
59 | // float n = voxels_[gidx].count;
60 | voxels_[gidx].labels[label] += 1; //(1. / (n + 1)) * (n * voxels_[gidx].point + p);
61 | voxels_[gidx].count += 1;
62 | occlusionsValid_ = false;
63 | }
64 |
65 | bool VoxelGrid::isOccluded(int32_t i, int32_t j, int32_t k) const { return occlusions_[index(i, j, k)] > -1; }
66 |
67 | bool VoxelGrid::isFree(int32_t i, int32_t j, int32_t k) const { return occlusions_[index(i, j, k)] == -1; }
68 |
69 | bool VoxelGrid::isInvalid(int32_t i, int32_t j, int32_t k) const {
70 | if (int32_t(invalid_.size()) <= index(i, j, k)) return true;
71 |
72 | return (invalid_[index(i, j, k)] > -1) && (invalid_[index(i, j, k)] != index(i, j, k));
73 | }
74 |
75 | void VoxelGrid::insertOcclusionLabels() {
76 | if (!occlusionsValid_) updateOcclusions();
77 |
78 | for (uint32_t i = 0; i < sizex_; ++i) {
79 | for (uint32_t j = 0; j < sizey_; ++j) {
80 | for (uint32_t k = 0; k < sizez_; ++k) {
81 | // heuristic: find label from above.
82 | if (occlusions_[index(i, j, k)] != index(i, j, k)) {
83 | int32_t n = 1;
84 | while ((k + n < sizez_) && isOccluded(i, j, k + n) && voxels_[index(i, j, k + n)].count == 0) n += 1;
85 | if (k + n < sizez_ && voxels_[index(i, j, k + n)].count > 0) {
86 | int32_t gidx = index(i, j, k);
87 | if (voxels_[gidx].count == 0) occupied_.push_back(gidx);
88 |
89 | voxels_[gidx].count = voxels_[index(i, j, k + n)].count;
90 | voxels_[gidx].labels = voxels_[index(i, j, k + n)].labels;
91 | }
92 | }
93 | }
94 | }
95 | }
96 | }
97 |
98 | void VoxelGrid::updateOcclusions() {
99 | std::fill(occludedBy_.begin(), occludedBy_.end(), -2);
100 | std::fill(occlusions_.begin(), occlusions_.end(), -1);
101 | uint32_t occludedByCalls = 0;
102 |
103 | // move from outer to inner voxels.
104 | uint32_t num_shells = std::min(sizex_, std::ceil(0.5 * sizey_));
105 |
106 | for (uint32_t o = 0; o < num_shells; ++o) {
107 | uint32_t i = sizex_ - o - 1;
108 |
109 | for (uint32_t j = 0; j < sizey_; ++j) {
110 | for (uint32_t k = 0; k < sizez_; ++k) {
111 | int32_t idx = index(i, j, k);
112 | if (occludedBy_[idx] == -2) {
113 | occludedByCalls += 1;
114 | occludedBy_[idx] = occludedBy(i, j, k);
115 | }
116 | occlusions_[idx] = occludedBy_[idx];
117 | // if (occlusions_[idx] != idx) occluded_.push_back(idx);
118 | }
119 | }
120 |
121 | uint32_t j = o;
122 | for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
123 | for (uint32_t k = 0; k < sizez_; ++k) {
124 | int32_t idx = index(i, j, k);
125 | if (occludedBy_[idx] == -2) {
126 | occludedByCalls += 1;
127 | occludedBy_[idx] = occludedBy(i, j, k);
128 | }
129 | occlusions_[idx] = occludedBy_[idx];
130 | // if (occlusions_[idx] != idx) occluded_.push_back(idx);
131 | }
132 | }
133 |
134 | j = sizey_ - o - 1;
135 | for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
136 | for (uint32_t k = 0; k < sizez_; ++k) {
137 | int32_t idx = index(i, j, k);
138 | if (occludedBy_[idx] == -2) {
139 | occludedByCalls += 1;
140 | occludedBy_[idx] = occludedBy(i, j, k);
141 | }
142 | occlusions_[idx] = occludedBy_[idx];
143 | // if (occlusions_[idx] != idx) occluded_.push_back(idx);
144 | }
145 | }
146 | }
147 |
148 | // sanity check:
149 | // for (uint32_t i = 0; i < occludedBy_.size(); ++i) {
150 | // if (occludedBy_[i] == -2) std::cout << "occludedBy == -2" << std::endl;
151 | // }
152 |
153 | // for (uint32_t i = 0; i < sizex_; ++i) {
154 | // for (uint32_t j = 0; j < sizey_; ++j) {
155 | // for (uint32_t k = 0; k < sizez_; ++k) {
156 | // int32_t idx = index(i, j, k);
157 | // if (occludedBy_[idx] == -2) {
158 | // occludedByCalls += 1;
159 | // occludedBy_[idx] = occludedBy(i, j, k);
160 | // }
161 | // occlusions_[idx] = occludedBy_[idx];
162 | // if (occlusions_[idx] != idx) occluded_.push_back(idx);
163 | // }
164 | // }
165 | // }
166 |
167 | // std::cout << "occludedBy called " << occludedByCalls << " times." << std::endl;
168 |
169 | invalid_ = occlusions_;
170 | occlusionsValid_ = true;
171 | }
172 |
173 | void VoxelGrid::updateInvalid(const Eigen::Vector3f& position) {
174 | if (!occlusionsValid_) updateOcclusions();
175 |
176 | std::fill(occludedBy_.begin(), occludedBy_.end(), -2);
177 |
178 | // move from outer to inner voxels.
179 | uint32_t num_shells = std::min(sizex_, std::ceil(0.5 * sizey_));
180 |
181 | for (uint32_t o = 0; o < num_shells; ++o) {
182 | uint32_t i = sizex_ - o - 1;
183 |
184 | for (uint32_t j = 0; j < sizey_; ++j) {
185 | for (uint32_t k = 0; k < sizez_; ++k) {
186 | int32_t idx = index(i, j, k);
187 | if (invalid_[idx] == -1) continue; // just skip. invalid cannot be less then -1.
188 |
189 | if (occludedBy_[idx] == -2) {
190 | occludedBy_[idx] = occludedBy(i, j, k, position);
191 | }
192 | invalid_[idx] = std::min(invalid_[idx], occludedBy_[idx]);
193 | }
194 | }
195 |
196 | uint32_t j = o;
197 | for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
198 | for (uint32_t k = 0; k < sizez_; ++k) {
199 | int32_t idx = index(i, j, k);
200 | if (invalid_[idx] == -1) continue; // just skip. invalid cannot be less then -1.
201 |
202 | if (occludedBy_[idx] == -2) {
203 | occludedBy_[idx] = occludedBy(i, j, k, position);
204 | }
205 | invalid_[idx] = std::min(invalid_[idx], occludedBy_[idx]);
206 | }
207 | }
208 |
209 | j = sizey_ - o - 1;
210 | for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
211 | for (uint32_t k = 0; k < sizez_; ++k) {
212 | int32_t idx = index(i, j, k);
213 | if (invalid_[idx] == -1) continue; // just skip. invalid cannot be less then -1.
214 |
215 | if (occludedBy_[idx] == -2) {
216 | occludedBy_[idx] = occludedBy(i, j, k, position);
217 | }
218 | invalid_[idx] = std::min(invalid_[idx], occludedBy_[idx]);
219 | }
220 | }
221 | }
222 |
223 | // for (uint32_t x = 0; x < sizex_; ++x) {
224 | // for (uint32_t y = 0; y < sizey_; ++y) {
225 | // for (uint32_t z = 0; z < sizez_; ++z) {
226 | // int32_t idx = index(x, y, z);
227 | // // idea: if voxel is not occluded, the value should be -1.
228 | // if (occludedBy_[idx] == -2) occludedBy_[idx] = occludedBy(x, y, z, position);
229 | // invalid_[idx] = std::min(invalid_[idx], occludedBy_[idx]);
230 | // }
231 | // }
232 | // }
233 | }
234 |
235 | int32_t VoxelGrid::occludedBy(int32_t i, int32_t j, int32_t k, const Eigen::Vector3f& endpoint,
236 | std::vector* visited) {
237 | float NextCrossingT[3], DeltaT[3]; /** t for next intersection with voxel boundary of axis, t increment for axis
238 | **/
239 | int32_t Step[3], Out[3], Pos[3]; /** voxel increment for axis, index of of outside voxels, current position **/
240 | float dir[3]; /** ray direction **/
241 |
242 | if (visited != nullptr) visited->clear();
243 |
244 | Pos[0] = i;
245 | Pos[1] = j;
246 | Pos[2] = k;
247 |
248 | /** calculate direction vector assuming sensor at (0,0,_heightOffset) **/
249 | Eigen::Vector3f startpoint = voxel2position(Pos[0], Pos[1], Pos[2]);
250 |
251 | double halfResolution = 0.5 * resolution_;
252 |
253 | dir[0] = endpoint[0] - startpoint[0];
254 | dir[1] = endpoint[1] - startpoint[1];
255 | dir[2] = endpoint[2] - startpoint[2];
256 |
257 | /** initialize variables for traversal **/
258 | for (uint32_t axis = 0; axis < 3; ++axis) {
259 | if (dir[axis] < 0) {
260 | NextCrossingT[axis] = -halfResolution / dir[axis];
261 | DeltaT[axis] = -resolution_ / dir[axis];
262 | Step[axis] = -1;
263 | Out[axis] = 0;
264 | } else {
265 | NextCrossingT[axis] = halfResolution / dir[axis];
266 | DeltaT[axis] = resolution_ / dir[axis];
267 | Step[axis] = 1;
268 | Out[axis] = size(axis);
269 | }
270 | }
271 |
272 | Eigen::Vector3i endindexes = position2voxel(endpoint);
273 | int32_t i_end = endindexes[0];
274 | int32_t j_end = endindexes[1];
275 | int32_t k_end = endindexes[2];
276 |
277 | const int32_t cmpToAxis[8] = {2, 1, 2, 1, 2, 2, 0, 0};
278 | int32_t iteration = 0;
279 |
280 | std::vector traversed;
281 | traversed.reserve(std::max(sizex_, std::max(sizey_, sizez_)));
282 |
283 | for (;;) // loop infinitely...
284 | {
285 | if (Pos[0] < 0 || Pos[1] < 0 || Pos[2] < 0) break;
286 | if (Pos[0] >= int32_t(sizex_) || Pos[1] >= int32_t(sizey_) || Pos[2] >= int32_t(sizez_)) break;
287 |
288 | int32_t idx = index(Pos[0], Pos[1], Pos[2]);
289 | bool occupied = voxels_[idx].count > 0;
290 | if (visited != nullptr) visited->push_back(Eigen::Vector3i(Pos[0], Pos[1], Pos[2]));
291 |
292 | if (occupied) {
293 | for (auto i : traversed) occludedBy_[i] = idx;
294 | return idx;
295 | }
296 |
297 | traversed.push_back(idx);
298 |
299 | int32_t bits = ((NextCrossingT[0] < NextCrossingT[1]) << 2) + ((NextCrossingT[0] < NextCrossingT[2]) << 1) +
300 | ((NextCrossingT[1] < NextCrossingT[2]));
301 | int32_t stepAxis = cmpToAxis[bits]; /* branch-free looping */
302 |
303 | Pos[stepAxis] += Step[stepAxis];
304 | NextCrossingT[stepAxis] += DeltaT[stepAxis];
305 |
306 | /** note the first condition should never happen, since we want to reach a point inside the grid **/
307 | if (Pos[stepAxis] == Out[stepAxis]) break; //... until outside, and leaving the loop here!
308 | if (Pos[0] == i_end && Pos[1] == j_end && Pos[2] == k_end) break; // .. or the sensor origin is reached.
309 |
310 | ++iteration;
311 | }
312 |
313 | for (auto i : traversed) occludedBy_[i] = -1;
314 |
315 | return -1;
316 | }
317 |
--------------------------------------------------------------------------------
/src/data/VoxelGrid.h:
--------------------------------------------------------------------------------
1 | #ifndef SRC_DATA_VOXELGRID_H_
2 | #define SRC_DATA_VOXELGRID_H_
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 |
9 | /** \brief simple Voxelgrid data structure with a label historgram per voxel.
10 | *
11 | * Instead of averaging of points per voxel, we simply record the label of the point.
12 | *
13 | *
14 | * \author behley
15 | */
16 |
17 | class VoxelGrid {
18 | public:
19 | class Voxel {
20 | public:
21 | std::map labels;
22 | uint32_t count{0};
23 | };
24 |
25 | /** \brief set parameters of voxel grid and compute internal offsets, etc. **/
26 | void initialize(float resolution, const Eigen::Vector4f& min, const Eigen::Vector4f& max);
27 |
28 | Voxel& operator()(uint32_t i, uint32_t j, uint32_t k) { return voxels_[index(i, j, k)]; }
29 | const Voxel& operator()(uint32_t i, uint32_t j, uint32_t k) const { return voxels_[index(i, j, k)]; }
30 |
31 | /** \brief cleanup voxelgrid. **/
32 | void clear();
33 |
34 | /** \brief add label for specific point to voxel grid **/
35 | void insert(const Eigen::Vector4f& p, uint32_t label);
36 | /** \brief get all voxels. **/
37 | const std::vector& voxels() const { return voxels_; }
38 |
39 | const Eigen::Vector4f& offset() const { return offset_; }
40 |
41 | uint32_t num_elements() const { return sizex_ * sizey_ * sizez_; }
42 |
43 | /** \brief get size in specific dimension **/
44 | uint32_t size(uint32_t dim) const { return (&sizex_)[std::max(std::min(dim, 2), 0)]; }
45 |
46 | /** \brief resolutions aka sidelength of a voxel **/
47 | float resolution() const { return resolution_; }
48 |
49 | /** \brief check for each voxel if there is voxel occluding the voxel. **/
50 | void updateOcclusions();
51 |
52 | /** \brief update invalid flags with given position to cast rays for occlusion check. **/
53 | void updateInvalid(const Eigen::Vector3f& position);
54 |
55 | /** \brief fill occluded areas with labels. **/
56 | void insertOcclusionLabels();
57 |
58 | /** \brief get if voxel at (i,j,k) is occluded.
59 | * Assumes that updateOcclusions has been called before,
60 | *
61 | * \see updateOcclusions
62 | **/
63 | bool isOccluded(int32_t i, int32_t j, int32_t k) const;
64 |
65 | /** \brief get if voxel at (i,j,k) is free.
66 | * Assumes that updateOcclusions has been called before,
67 | *
68 | * \see updateOcclusions
69 | **/
70 | bool isFree(int32_t i, int32_t j, int32_t k) const;
71 |
72 | /** \brief get if voxel at (i,j,k) is invalid.
73 | * Assumes that updateInvalid was called with all observation positions.
74 | *
75 | * \see updateInvalid.
76 | **/
77 | bool isInvalid(int32_t i, int32_t j, int32_t k) const;
78 |
79 | /** \brief check if given voxel is occluded.
80 | *
81 | * \param i,j,k voxel indexes
82 | * \param endpoint end point of ray (optional)
83 | * \param visited visited voxel indexes.
84 | *
85 | * traces a ray from the voxel to the given endpoint.
86 | *
87 | * \return index of voxel that occludes given voxel, -1 if voxel is not occluded.
88 | **/
89 | int32_t occludedBy(int32_t i, int32_t j, int32_t k, const Eigen::Vector3f& endpoint = Eigen::Vector3f::Zero(),
90 | std::vector* visited = nullptr);
91 |
92 | /** \brief get position of voxel center. **/
93 | inline Eigen::Vector3f voxel2position(int32_t i, int32_t j, int32_t k) const {
94 | return Eigen::Vector3f(offset_[0] + i * resolution_ + 0.5 * resolution_,
95 | offset_[1] + j * resolution_ + 0.5 * resolution_,
96 | offset_[2] + k * resolution_ + 0.5 * resolution_);
97 | }
98 |
99 | inline Eigen::Vector3i position2voxel(const Eigen::Vector3f& pos) const {
100 | return Eigen::Vector3i((pos[0] - offset_[0]) / resolution_, (pos[1] - offset_[1]) / resolution_,
101 | (pos[2] - offset_[2]) / resolution_);
102 | }
103 |
104 | inline int32_t index(int32_t i, int32_t j, int32_t k) const {
105 | // if (i >= sizex_ || j >= sizey_ || k >= sizez_) {
106 | // std::cout << sizex_ << ", " << sizey_ << ", " << sizez_ << std::endl;
107 | // std::cout << i << ", " << j << ", " << k << std::endl;
108 | //
109 | // std::cout << "indexes to large." << std::endl;
110 | // }
111 | // if (i < 0 || j < 0 || k < 0) {
112 | //
113 | // std::cout << i << ", " << j << ", " << k << std::endl;
114 | //
115 | // std::cout << "indexes too small" << std::endl;
116 | // }
117 | return i + j * sizex_ + k * sizex_ * sizey_;
118 | }
119 |
120 | protected:
121 | float resolution_;
122 |
123 | uint32_t sizex_, sizey_, sizez_;
124 | std::vector voxels_;
125 | std::vector occupied_;
126 |
127 | Eigen::Vector4f offset_;
128 |
129 | std::vector occlusions_; // filled by updateOcclusions.
130 | std::vector invalid_;
131 | bool occlusionsValid_{false};
132 | // std::vector occluded_; // filled by updateOcclusions.
133 |
134 | std::vector occludedBy_;
135 | };
136 |
137 | #endif /* SRC_DATA_VOXELGRID_H_ */
138 |
--------------------------------------------------------------------------------
/src/data/common.h:
--------------------------------------------------------------------------------
1 | #ifndef SRC_WIDGET_COMMON_H_
2 | #define SRC_WIDGET_COMMON_H_
3 |
4 | #include
5 | #include
6 | #include "data/geometry.h"
7 | #include
8 |
9 | typedef std::shared_ptr PointcloudPtr;
10 | typedef std::shared_ptr> LabelsPtr;
11 | typedef std::shared_ptr> ColorsPtr;
12 |
13 |
14 | struct LabeledVoxel{
15 | public:
16 | glow::vec3 position; // lower left corner
17 | uint32_t label; // majority voted label.
18 | };
19 |
20 | #endif /* SRC_WIDGET_COMMON_H_ */
21 |
--------------------------------------------------------------------------------
/src/data/geometry.h:
--------------------------------------------------------------------------------
1 | /*
2 | pbrt source code Copyright(c) 1998-2010 Matt Pharr and Greg Humphreys.
3 |
4 | This file is part of pbrt.
5 |
6 | pbrt is free software; you can redistribute it and/or modify
7 | it under the terms of the GNU General Public License as published by
8 | the Free Software Foundation; either version 2 of the License, or
9 | (at your option) any later version. Note that the text contents of
10 | the book "Physically Based Rendering" are *not* licensed under the
11 | GNU GPL.
12 |
13 | pbrt is distributed in the hope that it will be useful,
14 | but WITHOUT ANY WARRANTY; without even the implied warranty of
15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 | GNU General Public License for more details.
17 |
18 | You should have received a copy of the GNU General Public License
19 | along with this program. If not, see
37 | #include
38 | #include
39 | #include
40 |
41 | // Global Inline Functions
42 | inline float Lerp(float t, float v1, float v2) {
43 | return (1.f - t) * v1 + t * v2;
44 | }
45 |
46 | inline float Clamp(float val, float low, float high) {
47 | if (val < low)
48 | return low;
49 | else if (val > high)
50 | return high;
51 | else
52 | return val;
53 | }
54 |
55 | inline int Clamp(int val, int low, int high) {
56 | if (val < low)
57 | return low;
58 | else if (val > high)
59 | return high;
60 | else
61 | return val;
62 | }
63 |
64 | inline int Mod(int a, int b) {
65 | int n = int(a / b);
66 | a -= n * b;
67 | if (a < 0) a += b;
68 | return a;
69 | }
70 |
71 | inline float Radians(float deg) {
72 | return ((float)M_PI / 180.f) * deg;
73 | }
74 |
75 | inline float Degrees(float rad) {
76 | return (180.f / (float)M_PI) * rad;
77 | }
78 |
79 | inline float Log2(float x) {
80 | static float invLog2 = 1.f / logf(2.f);
81 | return logf(x) * invLog2;
82 | }
83 |
84 | inline int Floor2Int(float val);
85 | inline int Log2Int(float v) {
86 | return Floor2Int(Log2(v));
87 | }
88 |
89 | inline bool IsPowerOf2(int v) {
90 | return (v & (v - 1)) == 0;
91 | }
92 |
93 | inline uint32_t RoundUpPow2(uint32_t v) {
94 | v--;
95 | v |= v >> 1;
96 | v |= v >> 2;
97 | v |= v >> 4;
98 | v |= v >> 8;
99 | v |= v >> 16;
100 | return v + 1;
101 | }
102 |
103 | inline int Floor2Int(float val) {
104 | return (int)floorf(val);
105 | }
106 |
107 | inline int Round2Int(float val) {
108 | return Floor2Int(val + 0.5f);
109 | }
110 |
111 | inline int Float2Int(float val) {
112 | return (int)val;
113 | }
114 |
115 | inline int Ceil2Int(float val) {
116 | return (int)ceilf(val);
117 | }
118 |
119 | struct Vector3f;
120 | struct Point3f;
121 | struct Normal3f;
122 |
123 | // Geometry Declarations
124 | struct Vector3f {
125 | public:
126 | // Vector3f Public Methods
127 | Vector3f() { x = y = z = 0.f; }
128 | Vector3f(float xx, float yy, float zz) : x(xx), y(yy), z(zz) { assert(!HasNaNs()); }
129 | bool HasNaNs() const { return std::isnan(x) || std::isnan(y) || std::isnan(z); }
130 | explicit Vector3f(const Point3f &p);
131 | #ifndef NDEBUG
132 | // The default versions of these are fine for release builds; for debug
133 | // we define them so that we can add the assert checks.
134 | Vector3f(const Vector3f &v) {
135 | assert(!v.HasNaNs());
136 | x = v.x;
137 | y = v.y;
138 | z = v.z;
139 | }
140 |
141 | Vector3f &operator=(const Vector3f &v) {
142 | assert(!v.HasNaNs());
143 | x = v.x;
144 | y = v.y;
145 | z = v.z;
146 | return *this;
147 | }
148 | #endif // !NDEBUG
149 | Vector3f operator+(const Vector3f &v) const {
150 | assert(!v.HasNaNs());
151 | return Vector3f(x + v.x, y + v.y, z + v.z);
152 | }
153 |
154 | Vector3f &operator+=(const Vector3f &v) {
155 | assert(!v.HasNaNs());
156 | x += v.x;
157 | y += v.y;
158 | z += v.z;
159 | return *this;
160 | }
161 | Vector3f operator-(const Vector3f &v) const {
162 | assert(!v.HasNaNs());
163 | return Vector3f(x - v.x, y - v.y, z - v.z);
164 | }
165 |
166 | Vector3f &operator-=(const Vector3f &v) {
167 | assert(!v.HasNaNs());
168 | x -= v.x;
169 | y -= v.y;
170 | z -= v.z;
171 | return *this;
172 | }
173 | Vector3f operator*(float f) const { return Vector3f(f * x, f * y, f * z); }
174 |
175 | Vector3f &operator*=(float f) {
176 | assert(!std::isnan(f));
177 | x *= f;
178 | y *= f;
179 | z *= f;
180 | return *this;
181 | }
182 | Vector3f operator/(float f) const {
183 | assert(f != 0);
184 | float inv = 1.f / f;
185 | return Vector3f(x * inv, y * inv, z * inv);
186 | }
187 |
188 | Vector3f &operator/=(float f) {
189 | assert(f != 0);
190 | float inv = 1.f / f;
191 | x *= inv;
192 | y *= inv;
193 | z *= inv;
194 | return *this;
195 | }
196 | Vector3f operator-() const { return Vector3f(-x, -y, -z); }
197 | float operator[](int i) const {
198 | assert(i >= 0 && i <= 2);
199 | return (&x)[i];
200 | }
201 |
202 | float &operator[](int i) {
203 | assert(i >= 0 && i <= 2);
204 | return (&x)[i];
205 | }
206 | float LengthSquared() const { return x * x + y * y + z * z; }
207 | float Length() const { return sqrtf(LengthSquared()); }
208 | explicit Vector3f(const Normal3f &n);
209 |
210 | bool operator==(const Vector3f &v) const { return x == v.x && y == v.y && z == v.z; }
211 | bool operator!=(const Vector3f &v) const { return x != v.x || y != v.y || z != v.z; }
212 |
213 | friend std::ostream &operator<<(std::ostream &out, const Vector3f &v) {
214 | out.width(4);
215 | out.precision(3);
216 | out << v.x << ", " << v.y << ", " << v.z;
217 | return out;
218 | }
219 |
220 | // Vector3f Public Data
221 | float x, y, z;
222 | };
223 |
224 | struct Point3f {
225 | public:
226 | // Point3f Public Methods
227 | Point3f() { x = y = z = 0.f; }
228 | Point3f(float xx, float yy, float zz) : x(xx), y(yy), z(zz) {
229 | if (HasNaNs()) std::cerr << *this << std::endl;
230 | assert(!HasNaNs());
231 | }
232 | explicit Point3f(const Vector3f &v) : x(v.x), y(v.y), z(v.z) {}
233 |
234 | #ifndef NDEBUG
235 | Point3f(const Point3f &p) {
236 | if (p.HasNaNs()) std::cerr << p << std::endl;
237 | assert(!p.HasNaNs());
238 | x = p.x;
239 | y = p.y;
240 | z = p.z;
241 | }
242 |
243 | Point3f &operator=(const Point3f &p) {
244 | if (p.HasNaNs()) std::cerr << p << std::endl;
245 | assert(!p.HasNaNs());
246 | x = p.x;
247 | y = p.y;
248 | z = p.z;
249 | return *this;
250 | }
251 | #endif // !NDEBUG
252 | Point3f operator+(const Vector3f &v) const {
253 | assert(!v.HasNaNs());
254 | return Point3f(x + v.x, y + v.y, z + v.z);
255 | }
256 |
257 | Point3f &operator+=(const Vector3f &v) {
258 | assert(!v.HasNaNs());
259 | x += v.x;
260 | y += v.y;
261 | z += v.z;
262 | return *this;
263 | }
264 | Vector3f operator-(const Point3f &p) const {
265 | assert(!p.HasNaNs());
266 | return Vector3f(x - p.x, y - p.y, z - p.z);
267 | }
268 |
269 | Point3f operator-(const Vector3f &v) const {
270 | assert(!v.HasNaNs());
271 | return Point3f(x - v.x, y - v.y, z - v.z);
272 | }
273 |
274 | Point3f &operator-=(const Vector3f &v) {
275 | assert(!v.HasNaNs());
276 | x -= v.x;
277 | y -= v.y;
278 | z -= v.z;
279 | return *this;
280 | }
281 | Point3f &operator+=(const Point3f &p) {
282 | assert(!p.HasNaNs());
283 | x += p.x;
284 | y += p.y;
285 | z += p.z;
286 | return *this;
287 | }
288 | Point3f operator+(const Point3f &p) const {
289 | assert(!p.HasNaNs());
290 | return Point3f(x + p.x, y + p.y, z + p.z);
291 | }
292 | Point3f operator*(float f) const { return Point3f(f * x, f * y, f * z); }
293 | Point3f &operator*=(float f) {
294 | x *= f;
295 | y *= f;
296 | z *= f;
297 | return *this;
298 | }
299 | Point3f operator/(float f) const {
300 | float inv = 1.f / f;
301 | return Point3f(inv * x, inv * y, inv * z);
302 | }
303 | Point3f &operator/=(float f) {
304 | float inv = 1.f / f;
305 | x *= inv;
306 | y *= inv;
307 | z *= inv;
308 | return *this;
309 | }
310 | float operator[](int i) const {
311 | // assert(i >= 0 && i <= 2); ** CUDA doesn't allow assertion in kernels **
312 | return (&x)[i];
313 | }
314 |
315 | float &operator[](int i) {
316 | // assert(i >= 0 && i <= 2); ** CUDA doesn't allow assertion in kernels **
317 | return (&x)[i];
318 | }
319 | bool HasNaNs() const { return std::isnan(x) || std::isnan(y) || std::isnan(z); }
320 |
321 | bool operator==(const Point3f &p) const { /** todo: epsilon tests? **/
322 | return x == p.x && y == p.y && z == p.z;
323 | }
324 |
325 | bool operator!=(const Point3f &p) const { /** todo: epsilon tests? **/
326 | return x != p.x || y != p.y || z != p.z;
327 | }
328 |
329 | friend std::ostream &operator<<(std::ostream &out, const Point3f &p) {
330 | out.width(4);
331 | out.precision(3);
332 | out << p.x << ", " << p.y << ", " << p.z;
333 | return out;
334 | }
335 | // Point3f Public Data
336 | float x, y, z;
337 | };
338 |
339 | struct Normal3f {
340 | public:
341 | // Normal3f Public Methods
342 | Normal3f() { x = y = z = 0.f; }
343 | Normal3f(float xx, float yy, float zz) : x(xx), y(yy), z(zz) { assert(!HasNaNs()); }
344 | Normal3f operator-() const { return Normal3f(-x, -y, -z); }
345 | Normal3f operator+(const Normal3f &n) const {
346 | assert(!n.HasNaNs());
347 | return Normal3f(x + n.x, y + n.y, z + n.z);
348 | }
349 |
350 | Normal3f &operator+=(const Normal3f &n) {
351 | assert(!n.HasNaNs());
352 | x += n.x;
353 | y += n.y;
354 | z += n.z;
355 | return *this;
356 | }
357 | Normal3f operator-(const Normal3f &n) const {
358 | assert(!n.HasNaNs());
359 | return Normal3f(x - n.x, y - n.y, z - n.z);
360 | }
361 |
362 | Normal3f &operator-=(const Normal3f &n) {
363 | assert(!n.HasNaNs());
364 | x -= n.x;
365 | y -= n.y;
366 | z -= n.z;
367 | return *this;
368 | }
369 | bool HasNaNs() const { return std::isnan(x) || std::isnan(y) || std::isnan(z); }
370 | Normal3f operator*(float f) const { return Normal3f(f * x, f * y, f * z); }
371 |
372 | Normal3f &operator*=(float f) {
373 | x *= f;
374 | y *= f;
375 | z *= f;
376 | return *this;
377 | }
378 | Normal3f operator/(float f) const {
379 | assert(f != 0);
380 | float inv = 1.f / f;
381 | return Normal3f(x * inv, y * inv, z * inv);
382 | }
383 |
384 | Normal3f &operator/=(float f) {
385 | assert(f != 0);
386 | float inv = 1.f / f;
387 | x *= inv;
388 | y *= inv;
389 | z *= inv;
390 | return *this;
391 | }
392 | float LengthSquared() const { return x * x + y * y + z * z; }
393 | float Length() const { return sqrtf(LengthSquared()); }
394 |
395 | #ifndef NDEBUG
396 | Normal3f(const Normal3f &n) {
397 | assert(!n.HasNaNs());
398 | x = n.x;
399 | y = n.y;
400 | z = n.z;
401 | }
402 |
403 | Normal3f &operator=(const Normal3f &n) {
404 | assert(!n.HasNaNs());
405 | x = n.x;
406 | y = n.y;
407 | z = n.z;
408 | return *this;
409 | }
410 | #endif // !NDEBUG
411 | explicit Normal3f(const Vector3f &v) : x(v.x), y(v.y), z(v.z) { assert(!v.HasNaNs()); }
412 | float operator[](int i) const {
413 | assert(i >= 0 && i <= 2);
414 | return (&x)[i];
415 | }
416 |
417 | float &operator[](int i) {
418 | assert(i >= 0 && i <= 2);
419 | return (&x)[i];
420 | }
421 |
422 | bool operator==(const Normal3f &n) const { return x == n.x && y == n.y && z == n.z; }
423 | bool operator!=(const Normal3f &n) const { return x != n.x || y != n.y || z != n.z; }
424 |
425 | // Normal3f Public Data
426 | float x, y, z;
427 | };
428 |
429 | struct Ray {
430 | public:
431 | // Ray Public Methods
432 | Ray() : mint(0.f), maxt(INFINITY), time(0.f), depth(0) {}
433 | Ray(const Point3f &origin, const Vector3f &direction, float start, float end = INFINITY, float t = 0.f, int d = 0)
434 | : o(origin), d(direction), mint(start), maxt(end), time(t), depth(d) {}
435 | Ray(const Point3f &origin, const Vector3f &direction, const Ray &parent, float start, float end = INFINITY)
436 | : o(origin), d(direction), mint(start), maxt(end), time(parent.time), depth(parent.depth + 1) {}
437 | Point3f operator()(float t) const { return o + d * t; }
438 | bool HasNaNs() const { return (o.HasNaNs() || d.HasNaNs() || std::isnan(mint) || std::isnan(maxt)); }
439 |
440 | // Ray Public Data
441 | Point3f o;
442 | Vector3f d;
443 | mutable float mint, maxt;
444 | float time;
445 | int depth;
446 | };
447 |
448 | // Geometry Inline Functions
449 | inline Vector3f::Vector3f(const Point3f &p) : x(p.x), y(p.y), z(p.z) {
450 | assert(!HasNaNs());
451 | }
452 |
453 | inline Vector3f operator*(float f, const Vector3f &v) {
454 | return v * f;
455 | }
456 | inline float Dot(const Vector3f &v1, const Vector3f &v2) {
457 | assert(!v1.HasNaNs() && !v2.HasNaNs());
458 | return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
459 | }
460 |
461 | inline float AbsDot(const Vector3f &v1, const Vector3f &v2) {
462 | assert(!v1.HasNaNs() && !v2.HasNaNs());
463 | return fabsf(Dot(v1, v2));
464 | }
465 |
466 | inline Vector3f Cross(const Vector3f &v1, const Vector3f &v2) {
467 | assert(!v1.HasNaNs() && !v2.HasNaNs());
468 | double v1x = v1.x, v1y = v1.y, v1z = v1.z;
469 | double v2x = v2.x, v2y = v2.y, v2z = v2.z;
470 | return Vector3f((v1y * v2z) - (v1z * v2y), (v1z * v2x) - (v1x * v2z), (v1x * v2y) - (v1y * v2x));
471 | }
472 |
473 | inline Vector3f Cross(const Vector3f &v1, const Normal3f &v2) {
474 | assert(!v1.HasNaNs() && !v2.HasNaNs());
475 | double v1x = v1.x, v1y = v1.y, v1z = v1.z;
476 | double v2x = v2.x, v2y = v2.y, v2z = v2.z;
477 | return Vector3f((v1y * v2z) - (v1z * v2y), (v1z * v2x) - (v1x * v2z), (v1x * v2y) - (v1y * v2x));
478 | }
479 |
480 | inline Vector3f Cross(const Normal3f &v1, const Vector3f &v2) {
481 | assert(!v1.HasNaNs() && !v2.HasNaNs());
482 | double v1x = v1.x, v1y = v1.y, v1z = v1.z;
483 | double v2x = v2.x, v2y = v2.y, v2z = v2.z;
484 | return Vector3f((v1y * v2z) - (v1z * v2y), (v1z * v2x) - (v1x * v2z), (v1x * v2y) - (v1y * v2x));
485 | }
486 |
487 | inline Vector3f Normalize(const Vector3f &v) {
488 | return v / v.Length();
489 | }
490 |
491 | // inline void CoordinateSystem(const Vector3f &v1, Vector3f *v2, Vector3f *v3)
492 | //{
493 | // if (fabsf(v1.x) > fabsf(v1.y))
494 | // {
495 | // float invLen = 1.f / sqrtf(v1.x * v1.x + v1.z * v1.z);
496 | // *v2 = Vector3f(-v1.z * invLen, 0.f, v1.x * invLen);
497 | // }
498 | // else
499 | // {
500 | // float invLen = 1.f / sqrtf(v1.y * v1.y + v1.z * v1.z);
501 | // *v2 = Vector3f(0.f, v1.z * invLen, -v1.y * invLen);
502 | // }
503 | // *v3 = Cross(v1, *v2);
504 | //}
505 |
506 | inline float Distance(const Point3f &p1, const Point3f &p2) {
507 | return (p1 - p2).Length();
508 | }
509 |
510 | inline float DistanceSquared(const Point3f &p1, const Point3f &p2) {
511 | return (p1 - p2).LengthSquared();
512 | }
513 |
514 | inline Point3f operator*(float f, const Point3f &p) {
515 | assert(!p.HasNaNs());
516 | return p * f;
517 | }
518 |
519 | inline Normal3f operator*(float f, const Normal3f &n) {
520 | return Normal3f(f * n.x, f * n.y, f * n.z);
521 | }
522 |
523 | inline Normal3f Normalize(const Normal3f &n) {
524 | return n / n.Length();
525 | }
526 |
527 | inline Vector3f::Vector3f(const Normal3f &n) : x(n.x), y(n.y), z(n.z) {
528 | assert(!n.HasNaNs());
529 | }
530 |
531 | inline float Dot(const Normal3f &n1, const Vector3f &v2) {
532 | assert(!n1.HasNaNs() && !v2.HasNaNs());
533 | return n1.x * v2.x + n1.y * v2.y + n1.z * v2.z;
534 | }
535 |
536 | inline float Dot(const Vector3f &v1, const Normal3f &n2) {
537 | assert(!v1.HasNaNs() && !n2.HasNaNs());
538 | return v1.x * n2.x + v1.y * n2.y + v1.z * n2.z;
539 | }
540 |
541 | inline float Dot(const Normal3f &n1, const Normal3f &n2) {
542 | assert(!n1.HasNaNs() && !n2.HasNaNs());
543 | return n1.x * n2.x + n1.y * n2.y + n1.z * n2.z;
544 | }
545 |
546 | inline float AbsDot(const Normal3f &n1, const Vector3f &v2) {
547 | assert(!n1.HasNaNs() && !v2.HasNaNs());
548 | return fabsf(n1.x * v2.x + n1.y * v2.y + n1.z * v2.z);
549 | }
550 |
551 | inline float AbsDot(const Vector3f &v1, const Normal3f &n2) {
552 | assert(!v1.HasNaNs() && !n2.HasNaNs());
553 | return fabsf(v1.x * n2.x + v1.y * n2.y + v1.z * n2.z);
554 | }
555 |
556 | inline float AbsDot(const Normal3f &n1, const Normal3f &n2) {
557 | assert(!n1.HasNaNs() && !n2.HasNaNs());
558 | return fabsf(n1.x * n2.x + n1.y * n2.y + n1.z * n2.z);
559 | }
560 |
561 | inline Normal3f Faceforward(const Normal3f &n, const Vector3f &v) {
562 | return (Dot(n, v) < 0.f) ? -n : n;
563 | }
564 |
565 | inline Normal3f Faceforward(const Normal3f &n, const Normal3f &n2) {
566 | return (Dot(n, n2) < 0.f) ? -n : n;
567 | }
568 |
569 | inline Vector3f Faceforward(const Vector3f &v, const Vector3f &v2) {
570 | return (Dot(v, v2) < 0.f) ? -v : v;
571 | }
572 |
573 | inline Vector3f Faceforward(const Vector3f &v, const Normal3f &n2) {
574 | return (Dot(v, n2) < 0.f) ? -v : v;
575 | }
576 |
577 | inline Vector3f SphericalDirection(float sintheta, float costheta, float phi) {
578 | return Vector3f(sintheta * cosf(phi), sintheta * sinf(phi), costheta);
579 | }
580 |
581 | inline Vector3f SphericalDirection(float sintheta, float costheta, float phi, const Vector3f &x, const Vector3f &y,
582 | const Vector3f &z) {
583 | return sintheta * cosf(phi) * x + sintheta * sinf(phi) * y + costheta * z;
584 | }
585 |
586 | inline float SphericalTheta(const Vector3f &v) {
587 | return acosf(Clamp(v.z, -1.f, 1.f));
588 | }
589 |
590 | inline float SphericalPhi(const Vector3f &v) {
591 | float p = atan2f(v.y, v.x);
592 | return (p < 0.f) ? p + 2.f * M_PI : p;
593 | }
594 | #endif // GEOMETRY_H
595 |
--------------------------------------------------------------------------------
/src/data/kitti_utils.cpp:
--------------------------------------------------------------------------------
1 | #include "kitti_utils.h"
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include "rv/string_utils.h"
10 |
11 | using namespace rv;
12 |
13 | #define SAFE_COMMAND(cmd) \
14 | if (!system(cmd)) { \
15 | std::cerr << cmd << " failed." << std::endl; \
16 | exit(1); \
17 | }
18 |
19 | KITTICalibration::KITTICalibration() {}
20 |
21 | KITTICalibration::KITTICalibration(const std::string& filename) { initialize(filename); }
22 |
23 | const Eigen::Matrix4f& KITTICalibration::operator[](const std::string& name) const {
24 | if (m_.find(name) == m_.end()) throw std::runtime_error("Unknown name for calibration matrix.");
25 |
26 | return m_.find(name)->second;
27 | }
28 |
29 | void KITTICalibration::initialize(const std::string& filename) {
30 | m_.clear();
31 | std::ifstream cin(filename.c_str());
32 | if (!cin.is_open()) {
33 | throw std::runtime_error(std::string("Unable to open calibration file: ") + filename);
34 | }
35 | std::string line;
36 |
37 | cin.peek();
38 | while (!cin.eof()) {
39 | std::getline(cin, line);
40 | std::vector tokens = rv::split(line, ":");
41 |
42 | if (tokens.size() == 2) {
43 | std::string name = rv::trim(tokens[0]);
44 | std::vector entries = rv::split(rv::trim(tokens[1]), " ");
45 | if (entries.size() == 12) {
46 | Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
47 | for (uint32_t i = 0; i < 12; ++i) {
48 | m(i / 4, i - int(i / 4) * 4) = boost::lexical_cast(rv::trim(entries[i]));
49 | }
50 |
51 | m_[name] = m;
52 | }
53 | }
54 | cin.peek();
55 | }
56 |
57 | cin.close();
58 | }
59 |
60 | void KITTICalibration::clear() { m_.clear(); }
61 |
62 | bool KITTICalibration::exists(const std::string& name) const { return (m_.find(name) != m_.end()); }
63 |
64 | namespace KITTI {
65 |
66 | namespace Odometry {
67 |
68 | float lengths[] = {100, 200, 300, 400, 500, 600, 700, 800};
69 | int32_t num_lengths = 8;
70 |
71 | std::vector loadPoses(const std::string& file_name) {
72 | std::vector poses;
73 | std::ifstream fp(file_name.c_str());
74 | std::string line;
75 |
76 | if (!fp.is_open()) throw std::runtime_error("Unable to open pose file :" + file_name);
77 |
78 | fp.peek();
79 |
80 | while (fp.good()) {
81 | Eigen::Matrix4f P = Eigen::Matrix4f::Identity();
82 |
83 | std::getline(fp, line);
84 | std::vector entries = rv::split(line, " ");
85 |
86 | if (entries.size() < 12) {
87 | fp.peek();
88 | continue;
89 | }
90 |
91 | for (uint32_t i = 0; i < 12; ++i) {
92 | std::string txt = rv::trim(entries[i]);
93 | float value = boost::lexical_cast(txt);
94 | P(i / 4, i - int(i / 4) * 4) = value;
95 | }
96 |
97 | poses.push_back(P);
98 |
99 | fp.peek();
100 | }
101 |
102 | fp.close();
103 |
104 | return poses;
105 | }
106 |
107 | std::vector trajectoryDistances(const std::vector& poses) {
108 | std::vector dist;
109 | dist.push_back(0);
110 | for (uint32_t i = 1; i < poses.size(); i++) {
111 | const Eigen::Matrix4f& P1 = poses[i - 1];
112 | const Eigen::Matrix4f& P2 = poses[i];
113 |
114 | float dx = P1(0, 3) - P2(0, 3);
115 | float dy = P1(1, 3) - P2(1, 3);
116 | float dz = P1(2, 3) - P2(2, 3);
117 |
118 | dist.push_back(dist[i - 1] + sqrt(dx * dx + dy * dy + dz * dz));
119 | }
120 |
121 | return dist;
122 | }
123 |
124 | int32_t lastFrameFromSegmentLength(const std::vector& dist, int32_t first_frame, float len) {
125 | for (uint32_t i = first_frame; i < dist.size(); i++)
126 | if (dist[i] > dist[first_frame] + len) return i;
127 | return -1;
128 | }
129 |
130 | float rotationError(const Eigen::Matrix4f& pose_error) {
131 | float a = pose_error(0, 0);
132 | float b = pose_error(1, 1);
133 | float c = pose_error(2, 2);
134 | float d = 0.5 * (a + b + c - 1.0);
135 | return std::acos(std::max(std::min(d, 1.0f), -1.0f));
136 | }
137 |
138 | float translationError(const Eigen::Matrix4f& pose_error) {
139 | float dx = pose_error(0, 3);
140 | float dy = pose_error(1, 3);
141 | float dz = pose_error(2, 3);
142 | return std::sqrt(dx * dx + dy * dy + dz * dz);
143 | }
144 |
145 | std::vector calcSequenceErrors(const std::vector& poses_gt,
146 | const std::vector& poses_result) {
147 | // error vector
148 | std::vector err;
149 |
150 | // parameters
151 | int32_t step_size = 10; // every second
152 |
153 | // pre-compute distances (from ground truth as reference)
154 | std::vector dist = trajectoryDistances(poses_gt);
155 |
156 | // for all start positions do
157 | for (uint32_t first_frame = 0; first_frame < poses_gt.size(); first_frame += step_size) {
158 | // for all segment lengths do
159 | for (int32_t i = 0; i < num_lengths; i++) {
160 | // current length
161 | float len = lengths[i];
162 |
163 | // compute last frame
164 | int32_t last_frame = lastFrameFromSegmentLength(dist, first_frame, len);
165 |
166 | // continue, if sequence not long enough
167 | if (last_frame == -1) continue;
168 |
169 | // compute rotational and translational errors
170 | Eigen::Matrix4f pose_delta_gt = poses_gt[first_frame].inverse() * poses_gt[last_frame];
171 | Eigen::Matrix4f pose_delta_result = poses_result[first_frame].inverse() * poses_result[last_frame];
172 | Eigen::Matrix4f pose_error = pose_delta_result.inverse() * pose_delta_gt;
173 | float r_err = rotationError(pose_error);
174 | float t_err = translationError(pose_error);
175 |
176 | // compute speed
177 | float num_frames = (float)(last_frame - first_frame + 1);
178 | float speed = len / (0.1 * num_frames);
179 |
180 | // write to file
181 | err.push_back(errors(first_frame, r_err / len, t_err / len, len, speed));
182 | }
183 | }
184 |
185 | // return error vector
186 | return err;
187 | }
188 |
189 | void saveSequenceErrors(const std::vector& err, const std::string& file_name) {
190 | // open file
191 | // FILE *fp;
192 | // fp = fopen(file_name.c_str(), "w");
193 | std::ofstream out(file_name.c_str());
194 |
195 | // write to file
196 | for (std::vector::const_iterator it = err.begin(); it != err.end(); it++) {
197 | // fprintf(fp, "%d %f %f %f %f\n", it->first_frame, it->r_err, it->t_err, it->len, it->speed);
198 | out << it->first_frame << " " << it->r_err << " " << it->t_err << " " << it->len << " " << it->speed << std::endl;
199 | }
200 |
201 | // close file
202 | // fclose (fp);
203 | out.close();
204 | }
205 |
206 | void savePathPlot(const std::vector& poses_gt, const std::vector& poses_result,
207 | const std::string& file_name) {
208 | // parameters
209 | int32_t step_size = 3;
210 |
211 | // open file
212 | // FILE *fp = fopen(file_name.c_str(), "w");
213 | std::ofstream out(file_name.c_str());
214 |
215 | // save x/z coordinates of all frames to file
216 | for (uint32_t i = 0; i < poses_gt.size(); i += step_size) {
217 | out << poses_gt[i](0, 3) << " " << poses_gt[i](2, 3) << " ";
218 | out << poses_result[i](0, 3) << " " << poses_result[i](2, 3) << std::endl;
219 | }
220 | // close file
221 | // fclose(fp);
222 | out.close();
223 | }
224 |
225 | std::vector computeRoi(const std::vector& poses_gt,
226 | const std::vector& poses_result) {
227 | float x_min = std::numeric_limits::max();
228 | float x_max = std::numeric_limits::min();
229 | float z_min = std::numeric_limits::max();
230 | float z_max = std::numeric_limits::min();
231 |
232 | for (std::vector::const_iterator it = poses_gt.begin(); it != poses_gt.end(); it++) {
233 | float x = (*it)(0, 3);
234 | float z = (*it)(2, 3);
235 | if (x < x_min) x_min = x;
236 | if (x > x_max) x_max = x;
237 | if (z < z_min) z_min = z;
238 | if (z > z_max) z_max = z;
239 | }
240 |
241 | for (std::vector::const_iterator it = poses_result.begin(); it != poses_result.end(); it++) {
242 | float x = (*it)(0, 3);
243 | float z = (*it)(2, 3);
244 | if (x < x_min) x_min = x;
245 | if (x > x_max) x_max = x;
246 | if (z < z_min) z_min = z;
247 | if (z > z_max) z_max = z;
248 | }
249 |
250 | float dx = 1.1 * (x_max - x_min);
251 | float dz = 1.1 * (z_max - z_min);
252 | float mx = 0.5 * (x_max + x_min);
253 | float mz = 0.5 * (z_max + z_min);
254 | float r = 0.5 * std::max(dx, dz);
255 |
256 | std::vector roi;
257 | roi.push_back((int32_t)(mx - r));
258 | roi.push_back((int32_t)(mx + r));
259 | roi.push_back((int32_t)(mz - r));
260 | roi.push_back((int32_t)(mz + r));
261 |
262 | return roi;
263 | }
264 |
265 | void plotPathPlot(const std::string& dir, const std::vector& roi, int32_t idx) {
266 | // gnuplot file name
267 | char command[1024];
268 | char file_name[256];
269 | sprintf(file_name, "%02d.gp", idx);
270 | std::string full_name = dir + "/" + file_name;
271 |
272 | // create png + eps
273 | for (int32_t i = 0; i < 2; i++) {
274 | // open file
275 | FILE* fp = fopen(full_name.c_str(), "w");
276 |
277 | // save gnuplot instructions
278 | if (i == 0) {
279 | fprintf(fp, "set term png size 900,900\n");
280 | fprintf(fp, "set output \"%02d.png\"\n", idx);
281 | } else {
282 | fprintf(fp, "set term postscript eps enhanced color\n");
283 | fprintf(fp, "set output \"%02d.eps\"\n", idx);
284 | }
285 |
286 | fprintf(fp, "set size ratio -1\n");
287 | fprintf(fp, "set xrange [%d:%d]\n", roi[0], roi[1]);
288 | fprintf(fp, "set yrange [%d:%d]\n", roi[2], roi[3]);
289 | fprintf(fp, "set xlabel \"x [m]\"\n");
290 | fprintf(fp, "set ylabel \"z [m]\"\n");
291 | fprintf(fp, "plot \"%02d.txt\" using 1:2 lc rgb \"#FF0000\" title 'Ground Truth' w lines,", idx);
292 | fprintf(fp, "\"%02d.txt\" using 3:4 lc rgb \"#0000FF\" title 'Visual Odometry' w lines,", idx);
293 | fprintf(fp, "\"< head -1 %02d.txt\" using 1:2 lc rgb \"#000000\" pt 4 ps 1 lw 2 title 'Sequence Start' w points\n",
294 | idx);
295 |
296 | // close file
297 | fclose(fp);
298 |
299 | // run gnuplot => create png + eps
300 | sprintf(command, "cd %s; gnuplot %s", dir.c_str(), file_name);
301 | if (!system(command)) exit(1);
302 | }
303 |
304 | // create pdf and crop
305 | sprintf(command, "cd %s; ps2pdf %02d.eps %02d_large.pdf", dir.c_str(), idx, idx);
306 | SAFE_COMMAND(command);
307 | sprintf(command, "cd %s; pdfcrop %02d_large.pdf %02d.pdf", dir.c_str(), idx, idx);
308 | SAFE_COMMAND(command);
309 | sprintf(command, "cd %s; rm %02d_large.pdf", dir.c_str(), idx);
310 | SAFE_COMMAND(command);
311 | }
312 |
313 | void saveErrorPlots(const std::vector& seq_err, const std::string& plot_error_dir, const std::string& prefix) {
314 | // file names
315 | char file_name_tl[1024];
316 | sprintf(file_name_tl, "%s/%s_tl.txt", plot_error_dir.c_str(), prefix.c_str());
317 | char file_name_rl[1024];
318 | sprintf(file_name_rl, "%s/%s_rl.txt", plot_error_dir.c_str(), prefix.c_str());
319 | char file_name_ts[1024];
320 | sprintf(file_name_ts, "%s/%s_ts.txt", plot_error_dir.c_str(), prefix.c_str());
321 | char file_name_rs[1024];
322 | sprintf(file_name_rs, "%s/%s_rs.txt", plot_error_dir.c_str(), prefix.c_str());
323 |
324 | // open files
325 | FILE* fp_tl = fopen(file_name_tl, "w");
326 | FILE* fp_rl = fopen(file_name_rl, "w");
327 | FILE* fp_ts = fopen(file_name_ts, "w");
328 | FILE* fp_rs = fopen(file_name_rs, "w");
329 |
330 | // for each segment length do
331 | for (int32_t i = 0; i < num_lengths; i++) {
332 | float t_err = 0;
333 | float r_err = 0;
334 | float num = 0;
335 |
336 | // for all errors do
337 | for (std::vector::const_iterator it = seq_err.begin(); it != seq_err.end(); it++) {
338 | if (fabs(it->len - lengths[i]) < 1.0) {
339 | t_err += it->t_err;
340 | r_err += it->r_err;
341 | num++;
342 | }
343 | }
344 |
345 | // we require at least 3 values
346 | if (num > 2.5) {
347 | fprintf(fp_tl, "%f %f\n", lengths[i], t_err / num);
348 | fprintf(fp_rl, "%f %f\n", lengths[i], r_err / num);
349 | }
350 | }
351 |
352 | // for each driving speed do (in m/s)
353 | for (float speed = 2; speed < 25; speed += 2) {
354 | float t_err = 0;
355 | float r_err = 0;
356 | float num = 0;
357 |
358 | // for all errors do
359 | for (std::vector::const_iterator it = seq_err.begin(); it != seq_err.end(); it++) {
360 | if (fabs(it->speed - speed) < 2.0) {
361 | t_err += it->t_err;
362 | r_err += it->r_err;
363 | num++;
364 | }
365 | }
366 |
367 | // we require at least 3 values
368 | if (num > 2.5) {
369 | fprintf(fp_ts, "%f %f\n", speed, t_err / num);
370 | fprintf(fp_rs, "%f %f\n", speed, r_err / num);
371 | }
372 | }
373 |
374 | // close files
375 | fclose(fp_tl);
376 | fclose(fp_rl);
377 | fclose(fp_ts);
378 | fclose(fp_rs);
379 | }
380 |
381 | void plotErrorPlots(const std::string& dir, char* prefix) {
382 | char command[1024];
383 |
384 | // for all four error plots do
385 | for (int32_t i = 0; i < 4; i++) {
386 | // create suffix
387 | char suffix[16];
388 | switch (i) {
389 | case 0:
390 | sprintf(suffix, "tl");
391 | break;
392 | case 1:
393 | sprintf(suffix, "rl");
394 | break;
395 | case 2:
396 | sprintf(suffix, "ts");
397 | break;
398 | case 3:
399 | sprintf(suffix, "rs");
400 | break;
401 | }
402 |
403 | // gnuplot file name
404 | char file_name[1024];
405 | char full_name[1024];
406 | sprintf(file_name, "%s_%s.gp", prefix, suffix);
407 | sprintf(full_name, "%s/%s", dir.c_str(), file_name);
408 |
409 | // create png + eps
410 | for (int32_t j = 0; j < 2; j++) {
411 | // open file
412 | FILE* fp = fopen(full_name, "w");
413 |
414 | // save gnuplot instructions
415 | if (j == 0) {
416 | fprintf(fp, "set term png size 500,250 font \"Helvetica\" 11\n");
417 | fprintf(fp, "set output \"%s_%s.png\"\n", prefix, suffix);
418 | } else {
419 | fprintf(fp, "set term postscript eps enhanced color\n");
420 | fprintf(fp, "set output \"%s_%s.eps\"\n", prefix, suffix);
421 | }
422 |
423 | // start plot at 0
424 | fprintf(fp, "set size ratio 0.5\n");
425 | fprintf(fp, "set yrange [0:*]\n");
426 |
427 | // x label
428 | if (i <= 1)
429 | fprintf(fp, "set xlabel \"Path Length [m]\"\n");
430 | else
431 | fprintf(fp, "set xlabel \"Speed [km/h]\"\n");
432 |
433 | // y label
434 | if (i == 0 || i == 2)
435 | fprintf(fp, "set ylabel \"Translation Error [%%]\"\n");
436 | else
437 | fprintf(fp, "set ylabel \"Rotation Error [deg/m]\"\n");
438 |
439 | // plot error curve
440 | fprintf(fp, "plot \"%s_%s.txt\" using ", prefix, suffix);
441 | switch (i) {
442 | case 0:
443 | fprintf(fp, "1:($2*100) title 'Translation Error'");
444 | break;
445 | case 1:
446 | fprintf(fp, "1:($2*57.3) title 'Rotation Error'");
447 | break;
448 | case 2:
449 | fprintf(fp, "($1*3.6):($2*100) title 'Translation Error'");
450 | break;
451 | case 3:
452 | fprintf(fp, "($1*3.6):($2*57.3) title 'Rotation Error'");
453 | break;
454 | }
455 | fprintf(fp, " lc rgb \"#0000FF\" pt 4 w linespoints\n");
456 |
457 | // close file
458 | fclose(fp);
459 |
460 | // run gnuplot => create png + eps
461 | sprintf(command, "cd %s; gnuplot %s", dir.c_str(), file_name);
462 | SAFE_COMMAND(command);
463 | }
464 |
465 | // create pdf and crop
466 | sprintf(command, "cd %s; ps2pdf %s_%s.eps %s_%s_large.pdf", dir.c_str(), prefix, suffix, prefix, suffix);
467 | SAFE_COMMAND(command);
468 | sprintf(command, "cd %s; pdfcrop %s_%s_large.pdf %s_%s.pdf", dir.c_str(), prefix, suffix, prefix, suffix);
469 | SAFE_COMMAND(command);
470 | sprintf(command, "cd %s; rm %s_%s_large.pdf", dir.c_str(), prefix, suffix);
471 | SAFE_COMMAND(command);
472 | }
473 | }
474 |
475 | void saveStats(const std::vector& err, const std::string& dir) {
476 | float t_err = 0;
477 | float r_err = 0;
478 |
479 | // for all errors do => compute sum of t_err, r_err
480 | for (std::vector::const_iterator it = err.begin(); it != err.end(); it++) {
481 | t_err += it->t_err;
482 | r_err += it->r_err;
483 | }
484 |
485 | // open file
486 | FILE* fp = fopen((dir + "/stats.txt").c_str(), "w");
487 |
488 | // save errors
489 | float num = err.size();
490 | fprintf(fp, "%f %f\n", t_err / num, r_err / num);
491 |
492 | // close file
493 | fclose(fp);
494 | }
495 |
496 | /** \brief evaluating the odometry results for given result_dir and gt_dir.
497 | *
498 | * \param gt_dir groundtruth directory containing XX.txt files.
499 | * \param result_dir directory where the estimated trajectory is stored. This is also the directory
500 | * where all files are written.
501 | **/
502 | bool eval(const std::string& gt_dir, const std::string& result_dir) {
503 | // ground truth and result directories
504 | // string gt_dir = "data/odometry/poses";
505 | // string result_dir = "results/" + result_sha;
506 | std::string error_dir = result_dir + "/errors";
507 | std::string plot_path_dir = result_dir + "/plot_path";
508 | std::string plot_error_dir = result_dir + "/plot_error";
509 |
510 | // create output directories
511 | SAFE_COMMAND(("mkdir " + error_dir).c_str());
512 | SAFE_COMMAND(("mkdir " + plot_path_dir).c_str());
513 | SAFE_COMMAND(("mkdir " + plot_error_dir).c_str());
514 |
515 | // total errors
516 | std::vector total_err;
517 |
518 | // for all sequences do
519 | for (int32_t i = 11; i < 22; i++) {
520 | // file name
521 | char file_name[256];
522 | sprintf(file_name, "%02d.txt", i);
523 |
524 | // read ground truth and result poses
525 | std::vector poses_gt = loadPoses(gt_dir + "/" + file_name);
526 | std::vector poses_result = loadPoses(result_dir + "/data/" + file_name);
527 |
528 | // plot status
529 | std::cout << "Processing: " << file_name << ", poses: " << poses_result.size() << "/" << poses_gt.size()
530 | << std::endl;
531 |
532 | // check for errors
533 | if (poses_gt.size() == 0 || poses_result.size() != poses_gt.size()) {
534 | std::cout << "ERROR: Couldn't read (all) poses of: " << file_name << std::endl;
535 | return false;
536 | }
537 |
538 | // compute sequence errors
539 | std::vector seq_err = calcSequenceErrors(poses_gt, poses_result);
540 | saveSequenceErrors(seq_err, error_dir + "/" + file_name);
541 |
542 | // add to total errors
543 | total_err.insert(total_err.end(), seq_err.begin(), seq_err.end());
544 |
545 | // for first half => plot trajectory and compute individual stats
546 | if (i <= 15) {
547 | // save + plot bird's eye view trajectories
548 | savePathPlot(poses_gt, poses_result, plot_path_dir + "/" + file_name);
549 | std::vector roi = computeRoi(poses_gt, poses_result);
550 | plotPathPlot(plot_path_dir, roi, i);
551 |
552 | // save + plot individual errors
553 | char prefix[16];
554 | sprintf(prefix, "%02d", i);
555 | saveErrorPlots(seq_err, plot_error_dir, prefix);
556 | plotErrorPlots(plot_error_dir, prefix);
557 | }
558 | }
559 |
560 | // save + plot total errors + summary statistics
561 | if (total_err.size() > 0) {
562 | char prefix[16];
563 | sprintf(prefix, "avg");
564 | saveErrorPlots(total_err, plot_error_dir, prefix);
565 | plotErrorPlots(plot_error_dir, prefix);
566 | saveStats(total_err, result_dir);
567 | }
568 |
569 | // success
570 | return true;
571 | }
572 | }
573 | }
574 |
--------------------------------------------------------------------------------
/src/data/kitti_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef INCLUDE_CORE_KITTI_UTILS_H_
2 | #define INCLUDE_CORE_KITTI_UTILS_H_
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 |
9 | /** \brief parse calibration file given in KITTI file format
10 | * \author behley
11 | */
12 | class KITTICalibration {
13 | public:
14 | KITTICalibration();
15 | KITTICalibration(const std::string& filename);
16 |
17 | /** \brief initialize calibration matrices from given file in KITTI format. **/
18 | void initialize(const std::string& filename);
19 |
20 | /** \brief remove all matrices. **/
21 | void clear();
22 |
23 | /** \brief get calibration matrix with given name. **/
24 | const Eigen::Matrix4f& operator[](const std::string& name) const;
25 |
26 | /** \brief does the calibration matrix with given name exists? **/
27 | bool exists(const std::string& name) const;
28 |
29 | protected:
30 | std::map m_;
31 | };
32 |
33 | namespace KITTI {
34 |
35 | /** \brief helper functions from devkit for visual odometry evaluation.
36 | *
37 | * Uses Eigen:Matrix4f instead of Matrix.
38 | */
39 | namespace Odometry {
40 |
41 | struct errors {
42 | int32_t first_frame;
43 | float r_err;
44 | float t_err;
45 | float len;
46 | float speed;
47 | errors(int32_t first_frame, float r_err, float t_err, float len, float speed)
48 | : first_frame(first_frame), r_err(r_err), t_err(t_err), len(len), speed(speed) {}
49 | };
50 |
51 | /** \brief load poses from text file. **/
52 | std::vector loadPoses(const std::string& file_name);
53 |
54 | std::vector trajectoryDistances(const std::vector& poses);
55 |
56 | int32_t lastFrameFromSegmentLength(const std::vector& dist, int32_t first_frame, float len);
57 |
58 | float rotationError(const Eigen::Matrix4f& pose_error);
59 |
60 | float translationError(const Eigen::Matrix4f& pose_error);
61 |
62 | std::vector calcSequenceErrors(const std::vector& poses_gt,
63 | const std::vector& poses_result);
64 |
65 | void saveSequenceErrors(const std::vector& err, const std::string& file_name);
66 |
67 | void savePathPlot(const std::vector& poses_gt, const std::vector& poses_result,
68 | const std::string& file_name);
69 |
70 | std::vector computeRoi(const std::vector& poses_gt,
71 | const std::vector& poses_result);
72 |
73 | void plotPathPlot(const std::string& dir, const std::vector& roi, int32_t idx);
74 |
75 | void saveErrorPlots(const std::vector& seq_err, const std::string& plot_error_dir, const std::string& prefix);
76 |
77 | void plotErrorPlots(const std::string& dir, char* prefix);
78 |
79 | void saveStats(const std::vector& err, const std::string& dir);
80 |
81 | /** \brief evaluating the odometry results for given result_dir and gt_dir.
82 | *
83 | * \param gt_dir groundtruth directory containing XX.txt files.
84 | * \param result_dir directory where the estimated trajectory is stored. This is also the directory
85 | * where all files are written.
86 | **/
87 | bool eval(const std::string& gt_dir, const std::string& result_dir);
88 | }
89 | }
90 |
91 | #endif /* INCLUDE_CORE_KITTI_UTILS_H_ */
92 |
--------------------------------------------------------------------------------
/src/data/label_utils.cpp:
--------------------------------------------------------------------------------
1 | #include "label_utils.h"
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 |
8 | void getLabelNames(const std::string& filename,
9 | std::map& label_names)
10 | {
11 | QDomDocument doc("mydocument");
12 | QFile file(QString::fromStdString(filename));
13 | if (!file.open(QIODevice::ReadOnly)) return;
14 |
15 | if (!doc.setContent(&file))
16 | {
17 | file.close();
18 | return;
19 | }
20 |
21 | file.close();
22 |
23 | QDomElement docElem = doc.documentElement();
24 | QDomElement rootNode = doc.firstChildElement("config");
25 |
26 | QDomElement n = rootNode.firstChildElement("label");
27 | for (; !n.isNull(); n = n.nextSiblingElement("label"))
28 | {
29 | std::string name = n.firstChildElement("name").text().toStdString();
30 | uint32_t id = n.firstChildElement("id").text().toInt();
31 |
32 | label_names[id] = name;
33 | }
34 | }
35 |
36 | void getLabelColors(const std::string& filename,
37 | std::map& label_colors)
38 | {
39 | QDomDocument doc("mydocument");
40 | QFile file(QString::fromStdString(filename));
41 | if (!file.open(QIODevice::ReadOnly)) return;
42 |
43 | if (!doc.setContent(&file))
44 | {
45 | file.close();
46 | return;
47 | }
48 |
49 | file.close();
50 |
51 | QDomElement docElem = doc.documentElement();
52 | QDomElement rootNode = doc.firstChildElement("config");
53 |
54 | QDomElement n = rootNode.firstChildElement("label");
55 | for (; !n.isNull(); n = n.nextSiblingElement("label"))
56 | {
57 | std::string name = n.firstChildElement("name").text().toStdString();
58 | uint32_t id = n.firstChildElement("id").text().toInt();
59 | QString color_string = n.firstChildElement("color").text();
60 | QStringList tokens = color_string.split(" ");
61 |
62 | int32_t R = tokens.at(0).toInt();
63 | int32_t G = tokens.at(1).toInt();
64 | int32_t B = tokens.at(2).toInt();
65 |
66 | label_colors[id] = glow::GlColor::FromRGB(R, G, B);
67 | }
68 | }
69 |
--------------------------------------------------------------------------------
/src/data/label_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef LABEL_UTILS_H_
2 | #define LABEL_UTILS_H_
3 |
4 | /**
5 | * \brief Tools for processing of label description files.
6 | *
7 | * A label description file consists of names and colors for labels represented by a numerical id. The xml
8 | * structure is defined as follows:
9 | *
10 | *
11 | *
12 | * 0
13 | * no label
14 | * all points with no special label.
15 | * 255 255 255
16 | *
17 | *
18 | * [...]
19 | *
20 | *
21 | */
22 |
23 | #include
24 | #include
25 |
26 | #include
27 |
28 | /** \brief retrieve label names from given xml file.
29 | * \brief param[in] filename of the description file
30 | * \brief param[out] map of label ids to names.
31 | */
32 | void getLabelNames(const std::string& filename, std::map& label_names);
33 |
34 | /** \brief retrieve label colors from given xml file.
35 | * \brief param[in] filename of the description file.
36 | * \brief param[out] map of labels to colors.
37 | */
38 | void getLabelColors(const std::string& filename, std::map& label_colors);
39 |
40 | #endif /* LABEL_UTILS_H_ */
41 |
--------------------------------------------------------------------------------
/src/data/misc.cpp:
--------------------------------------------------------------------------------
1 | #include "misc.h"
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 |
8 | std::string trim(const std::string& str, const std::string& whitespaces) {
9 | int32_t beg = 0;
10 | int32_t end = 0;
11 |
12 | /** find the beginning **/
13 | for (beg = 0; beg < (int32_t)str.size(); ++beg) {
14 | bool found = false;
15 | for (uint32_t i = 0; i < whitespaces.size(); ++i) {
16 | if (str[beg] == whitespaces[i]) {
17 | found = true;
18 | break;
19 | }
20 | }
21 | if (!found) break;
22 | }
23 |
24 | /** find the end **/
25 | for (end = int32_t(str.size()) - 1; end > beg; --end) {
26 | bool found = false;
27 | for (uint32_t i = 0; i < whitespaces.size(); ++i) {
28 | if (str[end] == whitespaces[i]) {
29 | found = true;
30 | break;
31 | }
32 | }
33 | if (!found) break;
34 | }
35 |
36 | return str.substr(beg, end - beg + 1);
37 | }
38 |
39 | std::vector split(const std::string& line, const std::string& delim) {
40 | QString string = QString::fromStdString(line);
41 |
42 | QStringList list = string.split(QString::fromStdString(delim));
43 | std::vector tokens;
44 |
45 | QStringListIterator it(list);
46 |
47 | while (it.hasNext()) tokens.push_back(it.next().toStdString());
48 |
49 | return tokens;
50 | }
51 |
52 | bool insideTriangle(const glow::vec2& p, const glow::vec2& v1, const glow::vec2& v2, const glow::vec2& v3) {
53 | float b0 = ((v2.x - v1.x) * (v3.y - v1.y) - (v3.x - v1.x) * (v2.y - v1.y));
54 |
55 | if (std::abs(b0) > 0) {
56 | // compute barycentric coordinates.
57 | float b1 = (((v2.x - p.x) * (v3.y - p.y) - (v3.x - p.x) * (v2.y - p.y)) / b0);
58 |
59 | float b2 = (((v3.x - p.x) * (v1.y - p.y) - (v1.x - p.x) * (v3.y - p.y)) / b0);
60 | float b3 = 1.0f - b1 - b2;
61 |
62 | // only if all are greater equal 0, the point can be inside.
63 | return (b1 > 0) && (b2 > 0) && (b3 > 0);
64 | }
65 |
66 | return false;
67 | }
68 |
69 | bool triangulate(const std::vector& points, std::vector& triangles) {
70 | int32_t i = 0;
71 | int32_t lastear = -1;
72 | std::vector lst = points;
73 |
74 | glow::vec2 p1, p, p2;
75 | do {
76 | lastear = lastear + 1;
77 |
78 | // check next ear.
79 | p1 = lst[((i - 1) + lst.size()) % lst.size()];
80 | p = lst[(i + lst.size()) % lst.size()];
81 | p2 = lst[((i + 1) + lst.size()) % lst.size()];
82 |
83 | // is corner convex or concave?
84 | // float l = ((p1.x - p.x) * (p2.y - p.y) - (p1.y - p.y) * (p2.x - p.x));
85 |
86 | float g = (p.y - p1.y) * (p2.x - p1.x) + (p1.x - p.x) * (p2.y - p1.y);
87 | // signed triangle area:
88 | // float c = (p2.x - p1.x) * (p2.y + p1.y) + (p.x - p2.x) * (p.y + p2.y) + (p1.x - p.x) * (p1.y + p.y);
89 |
90 | if (g < 0) {
91 | // check for other point inside triangle.
92 | bool intriangle = false;
93 | for (int32_t j = 2; j < int32_t(lst.size()) - 2; ++j) {
94 | glow::vec2 pt = lst[(i + j + lst.size()) % lst.size()];
95 |
96 | if (insideTriangle(pt, p1, p, p2)) {
97 | intriangle = true;
98 | break;
99 | }
100 | }
101 |
102 | // found an ear, remove vertex.
103 | if (!intriangle) {
104 | Triangle tri;
105 | tri.i = p1;
106 | tri.j = p;
107 | tri.k = p2;
108 |
109 | triangles.push_back(tri);
110 | lst.erase(lst.begin() + i);
111 |
112 | lastear = 0;
113 |
114 | i = i - 1;
115 | }
116 | }
117 |
118 | i = (i + 1) % lst.size();
119 | } while ((lastear < int32_t(lst.size()) * 2) && (int32_t(lst.size()) != 3));
120 |
121 | if (lst.size() == 3) {
122 | Triangle tri;
123 | tri.i = lst[0];
124 | tri.j = lst[1];
125 | tri.k = lst[2];
126 |
127 | triangles.push_back(tri);
128 | }
129 |
130 | return (lst.size() == 3);
131 | }
132 |
--------------------------------------------------------------------------------
/src/data/misc.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \headerfile misc.h
3 | *
4 | * \brief some functions.
5 | *
6 | * \author behley
7 | */
8 |
9 | #ifndef MISC_H_
10 | #define MISC_H_
11 |
12 | #include
13 | #include
14 | #include
15 |
16 | #include
17 |
18 | /** \brief remove whitespaces at the beginning and end of a string. **/
19 | std::string trim(const std::string& str, const std::string& whitespaces = " \0\t\n\r\x0B");
20 |
21 | /** \brief splits a string in tokens, which are seperated by delim. **/
22 | std::vector split(const std::string& line, const std::string& delim = " ");
23 |
24 | template
25 | class IndexValue {
26 | public:
27 | IndexValue() : index(-1), value(nullptr) {}
28 | IndexValue(int32_t i, const T* v) : index(i), value(v) {}
29 |
30 | int32_t index;
31 | const T* value;
32 |
33 | friend bool operator<(const IndexValue& l, const IndexValue& r) { return (*l.value < *r.value); }
34 | friend bool operator==(const IndexValue& l, const IndexValue& r) { return (*l.value == *r.value); }
35 | };
36 |
37 | /** \brief determine indexes of set_difference between a and b. **/
38 | template
39 | void index_difference(const std::vector& a, const std::vector& b, std::vector& result) {
40 | std::vector> proxy_a;
41 | std::vector> proxy_b;
42 | std::vector> proxy_result(a.size());
43 |
44 | for (int32_t i = 0; i < int32_t(a.size()); ++i) proxy_a.push_back(IndexValue(i, &a[i]));
45 | for (int32_t i = 0; i < int32_t(b.size()); ++i) proxy_b.push_back(IndexValue(i, &b[i]));
46 |
47 | std::sort(proxy_a.begin(), proxy_a.end());
48 | std::sort(proxy_b.begin(), proxy_b.end());
49 |
50 | auto end = std::set_difference(proxy_a.begin(), proxy_a.end(), proxy_b.begin(), proxy_b.end(), proxy_result.begin());
51 |
52 | result.clear();
53 | for (auto it = proxy_result.begin(); it != end; ++it) result.push_back(it->index);
54 | }
55 |
56 | struct Triangle {
57 | glow::vec2 i, j, k;
58 | };
59 |
60 | bool insideTriangle(const glow::vec2& p, const glow::vec2& v1, const glow::vec2& v2, const glow::vec2& v3);
61 |
62 | bool triangulate(const std::vector& points, std::vector& triangles);
63 |
64 | #endif /* MISC_H_ */
65 |
--------------------------------------------------------------------------------
/src/data/voxelize_utils.cpp:
--------------------------------------------------------------------------------
1 | #include "voxelize_utils.h"
2 |
3 | #include
4 | #include
5 | #include
6 |
7 | using namespace rv;
8 |
9 | std::vector parseDictionary(std::string str) {
10 | std::vector tokens;
11 | str = trim(str);
12 | if (str[0] != '{' || str[str.size() - 1] != '}') {
13 | std::cerr << "Parser Error: " << str << " is not a valid dictionary token!" << std::endl;
14 | return tokens;
15 | }
16 |
17 | tokens = split(str.substr(1, str.size() - 2), ":");
18 |
19 | return tokens;
20 | }
21 |
22 | template
23 | std::vector parseList(std::string str) {
24 | str = trim(str);
25 | std::vector list;
26 |
27 | str = trim(str);
28 | if (str[0] != '[' || str[str.size() - 1] != ']') {
29 | std::cerr << "Parser Error: " << str << " is not a valid list token!" << std::endl;
30 | return list;
31 | }
32 |
33 | auto entry_tokens = split(str.substr(1, str.size() - 2), ",");
34 |
35 | for (const auto& token : entry_tokens) {
36 | T value = boost::lexical_cast(trim(token));
37 | list.push_back(value);
38 | }
39 |
40 | return list;
41 | }
42 |
43 | template <>
44 | std::vector parseList(std::string str) {
45 | str = trim(str);
46 | if (str[0] != '[' || str[str.size() - 1] != ']') {
47 | std::cerr << "Parser Error: " << str << " is not a valid list token!" << std::endl;
48 | }
49 |
50 | std::vector list;
51 | auto entry_tokens = split(str.substr(1, str.size() - 2), ",");
52 |
53 | for (uint32_t i = 0; i < entry_tokens.size(); ++i) {
54 | std::string token = entry_tokens[i];
55 |
56 | if (token.find("[") != std::string::npos) {
57 | if (token.find("]", token.find("[")) == std::string::npos) {
58 | // found a nested unterminated list token
59 | std::string next_token;
60 | do {
61 | if (i >= entry_tokens.size()) break;
62 | next_token = entry_tokens[i + 1];
63 | token += "," + next_token;
64 | ++i;
65 | } while (next_token.find("]") == std::string::npos);
66 | }
67 | }
68 | list.push_back(token);
69 | }
70 |
71 | return list;
72 | }
73 |
74 | Config parseConfiguration(const std::string& filename) {
75 | Config config;
76 | std::ifstream in(filename);
77 |
78 | if (!in.is_open()) return config;
79 |
80 | std::string line;
81 | in.peek();
82 | while (in.good() && !in.eof()) {
83 | std::getline(in, line);
84 |
85 | if (trim(line)[0] == '#') continue; // ignore comments.
86 |
87 | auto tokens = split(line, ":");
88 | if (tokens.size() < 2) continue;
89 | if (tokens.size() > 2) {
90 | for (uint32_t i = 2; i < tokens.size(); ++i) {
91 | tokens[1] += ":" + tokens[i];
92 | }
93 | tokens.resize(2);
94 | }
95 |
96 | if (tokens[0] == "max scans") {
97 | config.maxNumScans = boost::lexical_cast(trim(tokens[1]));
98 | continue;
99 | }
100 | if (tokens[0] == "max range") {
101 | config.maxRange = boost::lexical_cast(trim(tokens[1]));
102 | continue;
103 | }
104 | if (tokens[0] == "voxel size") {
105 | config.voxelSize = boost::lexical_cast(trim(tokens[1]));
106 | continue;
107 | }
108 | if (tokens[0] == "min range") {
109 | config.minRange = boost::lexical_cast(trim(tokens[1]));
110 | continue;
111 | }
112 | if (tokens[0] == "prior scans") {
113 | config.priorScans = boost::lexical_cast(trim(tokens[1]));
114 | continue;
115 | }
116 | if (tokens[0] == "past scans") {
117 | config.pastScans = boost::lexical_cast(trim(tokens[1]));
118 | continue;
119 | }
120 | if (tokens[0] == "past distance") {
121 | config.pastScans = boost::lexical_cast(trim(tokens[1]));
122 | continue;
123 | }
124 |
125 | if (tokens[0] == "stride num") {
126 | config.stride_num = boost::lexical_cast(trim(tokens[1]));
127 | continue;
128 | }
129 | if (tokens[0] == "stride distance") {
130 | config.stride_distance = boost::lexical_cast(trim(tokens[1]));
131 | continue;
132 | }
133 |
134 | if (tokens[0] == "min extent") {
135 | auto coords = parseList(tokens[1]);
136 | config.minExtent = Eigen::Vector4f(coords[0], coords[1], coords[2], 1.0f);
137 | continue;
138 | }
139 |
140 | if (tokens[0] == "max extent") {
141 | auto coords = parseList(tokens[1]);
142 | config.maxExtent = Eigen::Vector4f(coords[0], coords[1], coords[2], 1.0f);
143 |
144 | continue;
145 | }
146 |
147 | if (tokens[0] == "ignore") {
148 | config.filteredLabels = parseList(tokens[1]);
149 |
150 | continue;
151 | }
152 |
153 | if (tokens[0] == "join") {
154 | auto join_tokens = parseList(trim(tokens[1]));
155 |
156 | for (const auto& token : join_tokens) {
157 | auto mapping = parseDictionary(token);
158 | uint32_t label = boost::lexical_cast(trim(mapping[0]));
159 | config.joinedLabels[label] = parseList(trim(mapping[1]));
160 | }
161 |
162 | continue;
163 | }
164 |
165 | std::cout << "unknown parameter: " << tokens[0] << std::endl;
166 | }
167 |
168 | in.close();
169 |
170 | return config;
171 | }
172 |
173 | void fillVoxelGrid(const Eigen::Matrix4f& anchor_pose, const std::vector& points,
174 | const std::vector& labels, VoxelGrid& grid, const Config& config) {
175 | std::map mappedLabels; // replace key with value.
176 | for (auto joins : config.joinedLabels) {
177 | for (auto label : joins.second) {
178 | mappedLabels[label] = joins.first;
179 | }
180 | }
181 |
182 | for (uint32_t t = 0; t < points.size(); ++t) {
183 | Eigen::Matrix4f ap = anchor_pose.inverse() * points[t]->pose;
184 |
185 | for (uint32_t i = 0; i < points[t]->points.size(); ++i) {
186 | const Point3f& pp = points[t]->points[i];
187 |
188 | float range = Eigen::Vector3f(pp.x, pp.y, pp.z).norm();
189 | if (range < config.minRange || range > config.maxRange) continue;
190 | bool is_car_point = (config.hidecar && pp.x < 3.0 && pp.x > -2.0 && std::abs(pp.y) < 2.0);
191 | if (is_car_point) continue;
192 |
193 | Eigen::Vector4f p = ap * Eigen::Vector4f(pp.x, pp.y, pp.z, 1);
194 |
195 | uint32_t label = (*labels[t])[i];
196 | if (mappedLabels.find(label) != mappedLabels.end()) label = mappedLabels[label];
197 |
198 | if (std::find(config.filteredLabels.begin(), config.filteredLabels.end(), label) == config.filteredLabels.end()) {
199 | grid.insert(p, (*labels[t])[i]);
200 | }
201 | }
202 | }
203 | }
204 |
205 | template
206 | std::vector pack(const std::vector& vec) {
207 | std::vector packed(vec.size() / 8);
208 |
209 | for (uint32_t i = 0; i < vec.size(); i += 8) {
210 | packed[i / 8] = (vec[i] > 0) << 7 | (vec[i + 1] > 0) << 6 | (vec[i + 2] > 0) << 5 | (vec[i + 3] > 0) << 4 |
211 | (vec[i + 4] > 0) << 3 | (vec[i + 5] > 0) << 2 | (vec[i + 6] > 0) << 1 | (vec[i + 7] > 0);
212 | ;
213 | }
214 |
215 | return packed;
216 | }
217 |
218 | void saveVoxelGrid(const VoxelGrid& grid, const std::string& directory, const std::string& basename,
219 | const std::string& mode) {
220 | uint32_t Nx = grid.size(0);
221 | uint32_t Ny = grid.size(1);
222 | uint32_t Nz = grid.size(2);
223 |
224 | size_t numElements = grid.num_elements();
225 | std::vector outputLabels(numElements, 0);
226 | std::vector outputTensorOccluded(numElements, 0);
227 | std::vector outputTensorInvalid(numElements, 0);
228 |
229 | int32_t counter = 0;
230 | for (uint32_t x = 0; x < Nx; ++x) {
231 | for (uint32_t y = 0; y < Ny; ++y) {
232 | for (uint32_t z = 0; z < Nz; ++z) {
233 | const VoxelGrid::Voxel& v = grid(x, y, z);
234 |
235 | uint32_t isOccluded = (uint32_t)grid.isOccluded(x, y, z);
236 |
237 | uint32_t maxCount = 0;
238 | uint32_t maxLabel = 0;
239 |
240 | for (auto it = v.labels.begin(); it != v.labels.end(); ++it) {
241 | if (it->second > maxCount) {
242 | maxCount = it->second;
243 | maxLabel = it->first;
244 | }
245 | }
246 |
247 | // Write maxLabel appropriately to file.
248 | assert(counter < numElements);
249 | outputLabels[counter] = maxLabel;
250 | outputTensorOccluded[counter] = isOccluded;
251 | outputTensorInvalid[counter] = (uint32_t)grid.isInvalid(x, y, z);
252 | counter = counter + 1;
253 | }
254 | }
255 | }
256 |
257 | if (mode == "target") {
258 | // for target we just generate label, invalid, occluded.
259 | {
260 | std::string output_filename = directory + "/" + basename + ".label";
261 |
262 | std::ofstream out(output_filename.c_str());
263 | out.write((const char*)&outputLabels[0], outputLabels.size() * sizeof(uint16_t));
264 | out.close();
265 | }
266 |
267 | {
268 | std::string output_filename = directory + "/" + basename + ".occluded";
269 |
270 | std::ofstream out(output_filename.c_str());
271 | std::vector packed = pack(outputTensorOccluded);
272 | out.write((const char*)&packed[0], packed.size() * sizeof(uint8_t));
273 | out.close();
274 | }
275 |
276 | {
277 | std::string output_filename = directory + "/" + basename + ".invalid";
278 |
279 | std::ofstream out(output_filename.c_str());
280 | std::vector packed = pack(outputTensorInvalid);
281 | out.write((const char*)&packed[0], packed.size() * sizeof(uint8_t));
282 | out.close();
283 | }
284 |
285 | } else {
286 | // for input we just generate the ".bin" file.
287 | {
288 | std::string output_filename = directory + "/" + basename + ".bin";
289 |
290 | std::ofstream out(output_filename.c_str());
291 | std::vector packed = pack(outputLabels);
292 | out.write((const char*)&packed[0], packed.size() * sizeof(uint8_t));
293 | out.close();
294 | }
295 | }
296 | }
297 |
--------------------------------------------------------------------------------
/src/data/voxelize_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef SRC_DATA_VOXELIZE_UTILS_H_
2 | #define SRC_DATA_VOXELIZE_UTILS_H_
3 |
4 | #include "Pointcloud.h"
5 | #include "VoxelGrid.h"
6 | #include "common.h"
7 |
8 | class Config {
9 | public:
10 | float voxelSize{0.5}; // size of a voxel
11 | Eigen::Vector4f minExtent{Eigen::Vector4f(0, -20, -2, 1)}; // minimum coordinate to consider for voxelgrid creation
12 | Eigen::Vector4f maxExtent{Eigen::Vector4f(40, 20, 1, 1)}; // maximum coordinate to consider for voxelgrid creation
13 |
14 | uint32_t maxNumScans{100};
15 | uint32_t priorScans{0}; // number of scans before the current scan to consider.
16 | uint32_t pastScans{10}; // number of scans after the current scan to consider.
17 | float pastDistance{0.0f}; // ensure this distance of past scans to the current scan. (might imply that more then past
18 | // scans are used.)
19 | float minRange{2.5f}; // minimum distance of laser points to consider.
20 | float maxRange{25.0f}; // maximum distance of laser points to consider.
21 | std::vector filteredLabels; // ignored labels
22 | std::map> joinedLabels; // labels that get joined into a specific class.
23 |
24 | // gen_data specific values.
25 | uint32_t stride_num{0}; // number of scans between generated voxel grids
26 | float stride_distance{0.0f}; // trajectory distance between generated voxel grids.
27 | bool hidecar{true}; // remove scan points which are probably generated by the scanning car itself
28 | };
29 |
30 | /** \brief parse a given filename and fill configuration. **/
31 | Config parseConfiguration(const std::string& filename);
32 |
33 | /** \brief use given anchor_pose to insert all point clouds into the given VoxelGrid
34 | *
35 | * Iterates over all point clouds and inserts label into the label histogram of the respective voxel.
36 | * However, only points inside max_range/min_range and outside the bounding box of the car are inserted.
37 | *
38 | * \author behley
39 | **/
40 | void fillVoxelGrid(const Eigen::Matrix4f& anchor_pose, const std::vector& points,
41 | const std::vector& labels, VoxelGrid& grid, const Config& config);
42 |
43 | /** \brief save given voxelgrid as .mat file.
44 | *
45 | * \author mgarbade
46 | **/
47 | void saveVoxelGrid(const VoxelGrid& grid, const std::string& directory, const std::string& basename,
48 | const std::string& mode = "input");
49 |
50 | #endif /* SRC_DATA_VOXELIZE_UTILS_H_ */
51 |
--------------------------------------------------------------------------------
/src/gen_data.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include
5 | #include
6 |
7 | #include
8 | #include "data/voxelize_utils.h"
9 | #include "rv/Stopwatch.h"
10 | #include "rv/string_utils.h"
11 | #include "widget/KittiReader.h"
12 |
13 | using namespace rv;
14 |
15 | float poseDistance(const Eigen::Matrix4f& A, const Eigen::Matrix4f& B) {
16 | return (A.col(3).head(3) - B.col(3).head(3)).norm();
17 | }
18 |
19 | int32_t main(int32_t argc, char** argv) {
20 | if (argc < 4) {
21 | std::cout << "Too few arguments" << std::endl;
22 | std::cout << "Usage: ./gen_data "
23 | << std::endl;
24 | std::cout << " : config file, containing spatial extent of volume and resolution" << std::endl;
25 | std::cout << " : folder containing velodyne, labels, calib.txt, poses.txt, e.g. path/to/00 or "
26 | "path/to/01"
27 | << std::endl;
28 | std::cout << " : output folder containing voxelized input and target volumes" << std::endl;
29 | return 1;
30 | }
31 |
32 | Config config = parseConfiguration(argv[1]);
33 | std::string sequences_dirname = argv[2];
34 | std::string output_voxel_dirname = argv[3];
35 |
36 | QDir output_voxel_dir(QString::fromStdString(output_voxel_dirname));
37 | if (!output_voxel_dir.exists()) {
38 | std::cout << "Creating output directory: " << output_voxel_dir.absolutePath().toStdString() << std::endl;
39 | if (!output_voxel_dir.mkpath(output_voxel_dir.absolutePath())) {
40 | throw std::runtime_error("Unable to create output directory.");
41 | }
42 | }
43 |
44 | QDir sequences_dir(QString::fromStdString(sequences_dirname));
45 |
46 | KittiReader reader;
47 | reader.initialize(QString::fromStdString(sequences_dirname));
48 |
49 | reader.setNumPriorScans(config.priorScans);
50 | reader.setNumPastScans(config.pastScans);
51 |
52 | VoxelGrid priorGrid;
53 | VoxelGrid pastGrid;
54 |
55 | priorGrid.initialize(config.voxelSize, config.minExtent, config.maxExtent);
56 | pastGrid.initialize(config.voxelSize, config.minExtent, config.maxExtent);
57 |
58 | std::vector poses = reader.poses();
59 |
60 | uint32_t current = 0;
61 |
62 | while (current < reader.count()) {
63 | // std::cout << current << std::endl;
64 | std::stringstream outname;
65 | outname << std::setfill('0') << std::setw(6) << current;
66 |
67 | std::vector priorPoints;
68 | std::vector priorLabels;
69 | std::vector pastPoints;
70 | std::vector pastLabels;
71 |
72 | reader.retrieve(current, priorPoints, priorLabels, pastPoints, pastLabels);
73 |
74 | // ensure that labels are present, only then store data.
75 | uint32_t labelCount = 0;
76 | uint32_t pointCount = 0;
77 |
78 | for (uint32_t i = 0; i < pastLabels.size(); ++i) {
79 | pointCount += pastLabels[i]->size();
80 | for (uint32_t j = 0; j < pastLabels[i]->size(); ++j) {
81 | uint32_t label = (*pastLabels[i])[j];
82 | if (label > 0) labelCount += 1;
83 | }
84 | }
85 |
86 | float percentageLabeled = 100.0f * labelCount / pointCount;
87 | // std::cout << percentageLabeled << "% points labeled." << std::endl;
88 |
89 | priorGrid.clear();
90 | pastGrid.clear();
91 |
92 | if (percentageLabeled > 0.0f) {
93 | Eigen::Matrix4f anchor_pose = priorPoints.back()->pose;
94 |
95 | // Stopwatch::tic();
96 | fillVoxelGrid(anchor_pose, priorPoints, priorLabels, priorGrid, config);
97 | fillVoxelGrid(anchor_pose, priorPoints, priorLabels, pastGrid, config);
98 | fillVoxelGrid(anchor_pose, pastPoints, pastLabels, pastGrid, config);
99 | // std::cout << "fill voxelgrid took " << Stopwatch::toc() << std::endl;
100 |
101 | // Stopwatch::tic();
102 | priorGrid.updateOcclusions();
103 | pastGrid.updateOcclusions();
104 | // std::cout << "update occlusions took " << Stopwatch::toc() << std::endl;
105 |
106 | //# Fill voxels from ground
107 | // Stopwatch::tic();
108 | // priorGrid.insertOcclusionLabels();
109 | // pastGrid.insertOcclusionLabels();
110 | // std::cout << "occlusion labels took " << Stopwatch::toc() << std::endl;
111 |
112 | // Stopwatch::tic();
113 | for (uint32_t i = 0; i < pastPoints.size(); ++i) {
114 | Eigen::Vector3f endpoint = (anchor_pose.inverse() * pastPoints[i]->pose).col(3).head(3);
115 | pastGrid.updateInvalid(endpoint);
116 | }
117 | // std::cout << "update invalid took " << Stopwatch::toc() << std::endl;
118 |
119 | // store grid in mat file.
120 | saveVoxelGrid(priorGrid, output_voxel_dirname, outname.str(), "input");
121 | saveVoxelGrid(pastGrid, output_voxel_dirname, outname.str(), "target");
122 |
123 |
124 | } else {
125 | std::cout << "skipped." << std::endl;
126 | }
127 |
128 | // get index of next scan.
129 | float distance = 0.0f;
130 | uint32_t count = 0;
131 | while ((count < config.stride_num || distance < config.stride_distance || count == 0) &&
132 | current + 1 < poses.size()) {
133 | distance += poseDistance(poses[current], poses[current + 1]);
134 | count += 1;
135 | current += 1;
136 | }
137 |
138 | if ((count < config.stride_num || distance < config.stride_distance || count == 0) && current + 1 >= poses.size()) {
139 | // no further scan can be extracted possible.
140 | break;
141 | }
142 | }
143 |
144 | return 0;
145 | }
146 |
--------------------------------------------------------------------------------
/src/rv/Stopwatch.cpp:
--------------------------------------------------------------------------------
1 | #include "rv/Stopwatch.h"
2 | #include
3 |
4 | namespace rv
5 | {
6 |
7 | std::vector Stopwatch::stimes =
8 | std::vector();
9 |
10 | void Stopwatch::tic()
11 | {
12 | stimes.push_back(std::chrono::high_resolution_clock::now());
13 | }
14 |
15 | /** \brief stops the last timer started and outputs \a msg, if given.
16 | *
17 | * \return elapsed time in seconds.
18 | **/
19 | double Stopwatch::toc()
20 | {
21 | assert(stimes.begin() != stimes.end());
22 |
23 | std::chrono::system_clock::time_point endtime = std::chrono::high_resolution_clock::now();
24 | std::chrono::system_clock::time_point starttime = stimes.back();
25 | stimes.pop_back();
26 |
27 | std::chrono::duration elapsed_seconds = endtime - starttime;
28 |
29 | return elapsed_seconds.count();
30 | }
31 |
32 | }
33 |
--------------------------------------------------------------------------------
/src/rv/Stopwatch.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \headerfile Stopwatch.h
3 | *
4 | * \brief simple timing of function calls.
5 | *
6 | * Mimics the behavior of tic/toc of Matlab and throws an error if toc() is called without starting a timer before.
7 | *
8 | * Example:
9 | * Stopwatch::tic(); starts timer A
10 | *
11 | * Stopwatch::tic(); starts timer B
12 | * method1();
13 | * double time1 = Stopwatch::toc(); stops timer B and returns time elapsed since timer B started
14 | *
15 | * Stopwatch::tic(); starts timer C
16 | * method2();
17 | * double time2 = Stopwatch::toc(); stops timer C
18 | *
19 | * Stopwatch::toc("finished"); stops timer A, thus this is approx. time1 + time2. and outputs a message.
20 | *
21 | * \author behley
22 | */
23 |
24 | #ifndef STOPWATCH_H_
25 | #define STOPWATCH_H_
26 |
27 | #include
28 | #include
29 | #include
30 |
31 | namespace rv {
32 |
33 | class Stopwatch {
34 | public:
35 | /** \brief starts a timer. **/
36 | static void tic();
37 | /** \brief stops the last timer started and outputs \a msg **/
38 | static double toc();
39 | /** \brief number of active stopwatches. **/
40 | static size_t active() { return stimes.size(); }
41 |
42 | protected:
43 | static std::vector stimes;
44 | };
45 | }
46 |
47 | #endif /* STOPWATCH_H_ */
48 |
--------------------------------------------------------------------------------
/src/rv/string_utils.cpp:
--------------------------------------------------------------------------------
1 | #include "rv/string_utils.h"
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 |
8 | namespace rv {
9 |
10 | std::string trim(const std::string& str, const std::string& whitespaces) {
11 | int32_t beg = 0;
12 | int32_t end = 0;
13 |
14 | /** find the beginning **/
15 | for (beg = 0; beg < (int32_t)str.size(); ++beg) {
16 | bool found = false;
17 | for (uint32_t i = 0; i < whitespaces.size(); ++i) {
18 | if (str[beg] == whitespaces[i]) {
19 | found = true;
20 | break;
21 | }
22 | }
23 | if (!found) break;
24 | }
25 |
26 | /** find the end **/
27 | for (end = int32_t(str.size()) - 1; end > beg; --end) {
28 | bool found = false;
29 | for (uint32_t i = 0; i < whitespaces.size(); ++i) {
30 | if (str[end] == whitespaces[i]) {
31 | found = true;
32 | break;
33 | }
34 | }
35 | if (!found) break;
36 | }
37 |
38 | return str.substr(beg, end - beg + 1);
39 | }
40 |
41 | std::vector split(const std::string& line, const std::string& delim, bool skipEmpty) {
42 | std::vector tokens;
43 |
44 | boost::char_separator sep(delim.c_str(), "", (skipEmpty ? boost::drop_empty_tokens : boost::keep_empty_tokens));
45 | boost::tokenizer> tokenizer(line, sep);
46 |
47 | for (auto it = tokenizer.begin(); it != tokenizer.end(); ++it) {
48 | tokens.push_back(*it);
49 | }
50 | return tokens;
51 | }
52 | }
53 |
54 | // std::string operator+(const std::string& string, const uint32_t& other)
55 | //{
56 | // std::stringstream stream;
57 | // stream << string << other;
58 | // return stream.str();
59 | //}
60 | //
61 | // std::string operator+(const std::string& string, const int32_t& other)
62 | //{
63 | // std::stringstream stream;
64 | // stream << string << other;
65 | // return stream.str();
66 | //}
67 | //
68 | // std::string operator+(const std::string& string, const float& other)
69 | //{
70 | // std::stringstream stream;
71 | // stream << string << other;
72 | // return stream.str();
73 | //}
74 | //
75 | // std::string operator+(const std::string& string, const double& other)
76 | //{
77 | // std::stringstream stream;
78 | // stream << string << other;
79 | // return stream.str();
80 | //}
81 |
--------------------------------------------------------------------------------
/src/rv/string_utils.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \headerfile misc.h
3 | *
4 | * \brief some utility functions for std::strings.
5 | *
6 | * \author behley
7 | */
8 |
9 | #ifndef MISC_H_
10 | #define MISC_H_
11 |
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 |
19 | namespace rv
20 | {
21 |
22 | /** \brief remove whitespaces at the beginning and end of a string. **/
23 | std::string trim(const std::string& str, const std::string& whitespaces =
24 | " \0\t\n\r\x0B");
25 |
26 | /** \brief splits a string in tokens, which are seperated by delim. **/
27 | std::vector split(const std::string& line,
28 | const std::string& delim = " ", bool skipEmpty = false);
29 |
30 | /** \brief generate a string from a map.
31 | * \return stringified version of a map. **/
32 | template
33 | std::string stringify(const std::map& map)
34 | {
35 | std::stringstream str;
36 |
37 | typename std::map::const_iterator it;
38 | str << "{";
39 | for (it = map.begin(); it != map.end(); ++it)
40 | str << (it == map.begin() ? "" : ", ") << it->first << ": " << it->second;
41 | str << "}";
42 |
43 | return str.str();
44 | }
45 |
46 | /** \brief generate a string from a vector
47 | * \return stringified version of a vector. **/
48 | template
49 | std::string stringify(const std::vector& vec)
50 | {
51 | std::stringstream str;
52 |
53 | str << "[";
54 | for (uint32_t i = 0; i < vec.size(); ++i)
55 | str << (i == 0 ? "" : ", ") << vec[i];
56 | str << "]";
57 |
58 | return str.str();
59 | }
60 |
61 | /** \brief generate a string from a list
62 | * \return stringified version of a list. **/
63 | template
64 | std::string stringify(const std::list& list)
65 | {
66 | std::stringstream str;
67 | typename std::list::const_iterator it;
68 | str << "[";
69 | for (it = list.begin(); it != list.end(); ++it)
70 | str << (it == list.begin() ? "" : ", ") << *it;
71 | str << "]";
72 |
73 | return str.str();
74 | }
75 |
76 | /** \brief generate a string from a value using stringstream
77 | *
78 | * \param value stringifiable value (e.g., int, double, string,)
79 | * \return stringified version of the value
80 | */
81 | template
82 | std::string stringify(const T& value)
83 | {
84 | std::stringstream str;
85 | str << value;
86 | return str.str();
87 | }
88 |
89 | }
90 |
91 | /** some useful concatenation operators for std::strings. **/
92 | //std::string operator+(const std::string& string, const uint32_t& other);
93 | //std::string operator+(const std::string& string, const int32_t& other);
94 | //std::string operator+(const std::string& string, const float& other);
95 | //std::string operator+(const std::string& string, const double& other);
96 |
97 | #endif /* MISC_H_ */
98 |
--------------------------------------------------------------------------------
/src/shaders/blinnphong.frag:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | // simple Blinn-Phong Shading.
4 |
5 | out vec4 out_color;
6 |
7 | in vec4 color;
8 | in vec3 position;
9 | in vec3 normal;
10 |
11 |
12 | uniform vec3 lightPos;
13 | uniform vec3 viewPos;
14 |
15 |
16 | void main()
17 | {
18 | vec3 ambient = 0.05 * color.xyz;
19 |
20 | vec3 lightDir = normalize(lightPos - position);
21 | vec3 normal1 = normalize(normal);
22 | float diff = max(dot(lightDir, normal1), 0.0);
23 | vec3 diffuse = diff * color.xyz;
24 |
25 | vec3 viewDir = normalize(viewPos - position);
26 | vec3 reflectDir = reflect(-lightDir, normal);
27 | vec3 halfwayDir = normalize(lightDir + viewDir);
28 |
29 | float spec = pow(max(dot(normal, halfwayDir), 0.0), 32.0);
30 | vec3 specular = vec3(0.3) * spec;
31 |
32 | out_color = vec4(ambient + diffuse + specular, 1.0);
33 | }
--------------------------------------------------------------------------------
/src/shaders/color.glsl:
--------------------------------------------------------------------------------
1 |
2 | const vec4 RED = vec4(1.0, 0.0, 0.0, 1.0);
3 | const vec4 GREEN = vec4(0.0, 1.0, 0.0, 1.0);
4 | const vec4 BLUE = vec4(0.0, 0.0, 1.0, 1.0);
5 |
6 | // source: http://lolengine.net/blog/2013/07/27/rgb-to-hsv-in-glsl
7 | vec3 hsv2rgb(vec3 c)
8 | {
9 | vec4 K = vec4(1.0, 2.0 / 3.0, 1.0 / 3.0, 3.0);
10 | vec3 p = abs(fract(c.xxx + K.xyz) * 6.0 - K.www);
11 | return c.z * mix(K.xxx, clamp(p - K.xxx, 0.0, 1.0), c.y);
12 | }
13 |
14 | // http://stackoverflow.com/questions/15095909/from-rgb-to-hsv-in-opengl-glsl
15 | vec3 rgb2hsv(vec3 c)
16 | {
17 | vec4 K = vec4(0.0, -1.0 / 3.0, 2.0 / 3.0, -1.0);
18 | vec4 p = mix(vec4(c.bg, K.wz), vec4(c.gb, K.xy), step(c.b, c.g));
19 | vec4 q = mix(vec4(p.xyw, c.r), vec4(c.r, p.yzx), step(p.x, c.r));
20 |
21 | float d = q.x - min(q.w, q.y);
22 | float e = 1.0e-10;
23 | return vec3(abs(q.z + (q.w - q.y) / (6.0 * d + e)), d / (q.x + e), q.x);
24 | }
25 |
26 |
27 | // source: elastic fusion https://github.com/mp3guy/ElasticFusion &
28 | // http://stackoverflow.com/questions/6893302/decode-rgb-value-to-single-float-without-bit-shift-in-glsl
29 | float pack(vec3 c)
30 | {
31 | int rgb = int(round(c.x * 255.0f));
32 | rgb = (rgb << 8) + int(round(c.y * 255.0f));
33 | rgb = (rgb << 8) + int(round(c.z * 255.0f));
34 | return float(rgb);
35 | }
36 |
37 | vec3 unpack(float c)
38 | {
39 | vec3 col;
40 | col.x = float(int(c) >> 16 & 0xFF) / 255.0f;
41 | col.y = float(int(c) >> 8 & 0xFF) / 255.0f;
42 | col.z = float(int(c) & 0xFF) / 255.0f;
43 | return col;
44 | }
45 |
--------------------------------------------------------------------------------
/src/shaders/draw_points.vert:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | layout (location = 0) in vec3 in_vertex;
4 | layout (location = 1) in float in_remission;
5 | layout (location = 2) in uint in_label;
6 | layout (location = 3) in uint in_visible;
7 |
8 | uniform sampler2DRect label_colors;
9 | uniform sampler2D heightMap;
10 |
11 | #include "shaders/color.glsl"
12 |
13 | // materials.
14 | uniform mat4 mvp;
15 | uniform mat4 pose;
16 | uniform bool useRemission;
17 | uniform bool useColor;
18 |
19 | uniform float minRange;
20 | uniform float maxRange;
21 |
22 | uniform bool removeGround;
23 | uniform float groundThreshold;
24 |
25 | uniform vec2 tilePos;
26 | uniform float tileSize;
27 | uniform bool showAllPoints;
28 |
29 | out vec4 color;
30 |
31 | void main()
32 | {
33 | vec4 in_color = texture(label_colors, vec2(in_label, 0));
34 |
35 | float range = length(in_vertex);
36 | gl_Position = mvp * vec4(in_vertex, 1.0);
37 |
38 | vec4 v_global = pose * vec4(in_vertex, 1.0);
39 | vec2 v = (pose * vec4(in_vertex, 1.0)).xy - tilePos;
40 |
41 |
42 | bool visible = (in_visible > uint(0));
43 |
44 | if(!visible || range < minRange || range > maxRange) gl_Position = vec4(-10, -10, -10, 1);
45 |
46 |
47 | if(useRemission)
48 | {
49 | float r = in_remission * 0.7 + 0.3; // ensure r in [0.3, 1.0]
50 | vec3 hsv = rgb2hsv(in_color.rgb);
51 | hsv.b = max(hsv.b, 0.8);
52 |
53 | color = vec4(hsv2rgb(vec3(1, 1, r) * hsv), 1.0);
54 | }
55 | else
56 | {
57 | color = vec4(in_color.rgb, 1.0);
58 | }
59 | }
60 |
--------------------------------------------------------------------------------
/src/shaders/draw_pose.geom:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | layout(points) in;
4 | layout(line_strip, max_vertices = 6) out;
5 |
6 | uniform mat4 mvp;
7 | uniform mat4 pose;
8 | uniform float size;
9 |
10 | out vec4 color;
11 |
12 | void main()
13 | {
14 | color = vec4(1, 0, 0, 1);
15 | gl_Position = mvp * pose * vec4(0, 0, 0, 1);
16 | EmitVertex();
17 | gl_Position = mvp * pose * vec4(size, 0, 0, 1);
18 | EmitVertex();
19 | EndPrimitive();
20 |
21 | color = vec4(0, 1, 0, 1);
22 | gl_Position = mvp * pose * vec4(0, 0, 0, 1);
23 | EmitVertex();
24 | gl_Position = mvp * pose * vec4(0, size, 0, 1);
25 | EmitVertex();
26 | EndPrimitive();
27 |
28 | color = vec4(0, 0, 1, 1);
29 | gl_Position = mvp * pose * vec4(0, 0, 0, 1);
30 | EmitVertex();
31 | gl_Position = mvp * pose * vec4(0, 0, size, 1);
32 | EmitVertex();
33 | EndPrimitive();
34 |
35 | }
36 |
--------------------------------------------------------------------------------
/src/shaders/draw_voxels.geom:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | layout(points) in;
4 | layout(triangle_strip, max_vertices = 36) out;
5 |
6 |
7 | in VOXEL {
8 | vec3 pos;
9 | vec4 color;
10 | } gs_in[];
11 |
12 | uniform mat4 mvp;
13 | uniform vec4 voxelOffset;
14 | uniform float voxelSize;
15 |
16 | out vec4 color;
17 | out vec3 position;
18 | out vec3 normal;
19 |
20 | void main()
21 | {
22 | // see https://stackoverflow.com/questions/28375338/cube-using-single-gl-triangle-strip for order.
23 | // 4 3 7 8 5 3 1 4 2 7 6 5 2 1
24 | // only 14 vertices instead of 36! However, I really don't know how to handle there the normals for the lighting.
25 |
26 |
27 | vec4 p = vec4(gs_in[0].pos.xyz, 1);
28 | color = gs_in[0].color;
29 |
30 | vec4 p1 = p + vec4(voxelSize,0,0,0);
31 | vec4 p2 = p + vec4(0,0,0,0);
32 | vec4 p3 = p + vec4(voxelSize,voxelSize,0,0);
33 | vec4 p4 = p + vec4(0,voxelSize,0,0);
34 | vec4 p5 = p + vec4(voxelSize,0,voxelSize,0);
35 | vec4 p6 = p + vec4(0,0,voxelSize,0);
36 | vec4 p7 = p + vec4(0, voxelSize,voxelSize,0);
37 | vec4 p8 = p + vec4(voxelSize,voxelSize,voxelSize,0);
38 |
39 | normal = vec3(0,1,0);
40 | position = p4.xyz;
41 | gl_Position = mvp * p4;
42 | EmitVertex();
43 | position = p3.xyz;
44 | gl_Position = mvp * p3;
45 | EmitVertex();
46 | position = p7.xyz;
47 | gl_Position = mvp * p7;
48 | EmitVertex();
49 | EndPrimitive();
50 |
51 | position = p3.xyz;
52 | gl_Position = mvp * p3;
53 | EmitVertex();
54 | position = p7.xyz;
55 | gl_Position = mvp * p7;
56 | EmitVertex();
57 | position = p8.xyz;
58 | gl_Position = mvp * p8;
59 | EmitVertex();
60 | EndPrimitive();
61 |
62 |
63 | normal = vec3(0,0,1);
64 | position = p7.xyz;
65 | gl_Position = mvp * p7;
66 | EmitVertex();
67 | position = p8.xyz;
68 | gl_Position = mvp * p8;
69 | EmitVertex();
70 | position = p5.xyz;
71 | gl_Position = mvp * p5;
72 | EmitVertex();
73 | EndPrimitive();
74 |
75 | position = p7.xyz;
76 | gl_Position = mvp * p7;
77 | EmitVertex();
78 | position = p6.xyz;
79 | gl_Position = mvp * p6;
80 | EmitVertex();
81 | position = p5.xyz;
82 | gl_Position = mvp * p5;
83 | EmitVertex();
84 | EndPrimitive();
85 |
86 |
87 | normal = vec3(1,0,0);
88 | position = p8.xyz;
89 | gl_Position = mvp * p8;
90 | EmitVertex();
91 | position = p5.xyz;
92 | gl_Position = mvp * p5;
93 | EmitVertex();
94 | position = p3.xyz;
95 | gl_Position = mvp * p3;
96 | EmitVertex();
97 | EndPrimitive();
98 |
99 | position = p5.xyz;
100 | gl_Position = mvp * p5;
101 | EmitVertex();
102 | position = p3.xyz;
103 | gl_Position = mvp * p3;
104 | EmitVertex();
105 | position = p1.xyz;
106 | gl_Position = mvp * p1;
107 | EmitVertex();
108 | EndPrimitive();
109 |
110 | normal = vec3(0,0,-1);
111 | position = p3.xyz;
112 | gl_Position = mvp * p3;
113 | EmitVertex();
114 | position = p1.xyz;
115 | gl_Position = mvp * p1;
116 | EmitVertex();
117 | position = p4.xyz;
118 | gl_Position = mvp * p4;
119 | EmitVertex();
120 | EndPrimitive();
121 |
122 | position = p1.xyz;
123 | gl_Position = mvp * p1;
124 | EmitVertex();
125 | position = p4.xyz;
126 | gl_Position = mvp * p4;
127 | EmitVertex();
128 | position = p2.xyz;
129 | gl_Position = mvp * p2;
130 | EmitVertex();
131 | EndPrimitive();
132 |
133 | normal = vec3(-1,0,0);
134 | position = p4.xyz;
135 | gl_Position = mvp * p4;
136 | EmitVertex();
137 | position = p2.xyz;
138 | gl_Position = mvp * p2;
139 | EmitVertex();
140 | position = p7.xyz;
141 | gl_Position = mvp * p7;
142 | EmitVertex();
143 | EndPrimitive();
144 |
145 | position = p2.xyz;
146 | gl_Position = mvp * p2;
147 | EmitVertex();
148 | position = p7.xyz;
149 | gl_Position = mvp * p7;
150 | EmitVertex();
151 | position = p6.xyz;
152 | gl_Position = mvp * p6;
153 | EmitVertex();
154 | EndPrimitive();
155 |
156 |
157 | normal = vec3(0,-1,0);
158 | position = p6.xyz;
159 | gl_Position = mvp * p6;
160 | EmitVertex();
161 | position = p5.xyz;
162 | gl_Position = mvp * p5;
163 | EmitVertex();
164 | position = p2.xyz;
165 | gl_Position = mvp * p2;
166 | EmitVertex();
167 | EndPrimitive();
168 |
169 | position = p5.xyz;
170 | gl_Position = mvp * p5;
171 | EmitVertex();
172 | position = p2.xyz;
173 | gl_Position = mvp * p2;
174 | EmitVertex();
175 | position = p1.xyz;
176 | gl_Position = mvp * p1;
177 | EmitVertex();
178 | EndPrimitive();
179 |
180 | }
--------------------------------------------------------------------------------
/src/shaders/draw_voxels.vert:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | layout (location = 0) in vec3 in_position;
4 | layout (location = 1) in uint in_label;
5 |
6 | uniform sampler2DRect label_colors;
7 | uniform vec4 custom_color;
8 | uniform bool use_custom_color;
9 |
10 | #include "shaders/color.glsl"
11 |
12 |
13 | out VOXEL {
14 | vec3 pos;
15 | vec4 color;
16 | } vs_out;
17 |
18 |
19 | void main()
20 | {
21 | vec4 in_color = texture(label_colors, vec2(in_label, 0));
22 | if (use_custom_color) in_color = custom_color;
23 |
24 | vs_out.pos = in_position;
25 | vs_out.color = in_color;
26 | }
27 |
--------------------------------------------------------------------------------
/src/shaders/empty.frag:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 |
4 | void main()
5 | {
6 |
7 | }
--------------------------------------------------------------------------------
/src/shaders/empty.vert:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 |
4 | void main()
5 | {
6 |
7 | }
--------------------------------------------------------------------------------
/src/shaders/passthrough.frag:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | in vec4 color;
4 | out vec4 out_color;
5 |
6 | void main()
7 | {
8 | out_color = color;
9 | }
--------------------------------------------------------------------------------
/src/shaders/quad.geom:
--------------------------------------------------------------------------------
1 | #version 330 core
2 |
3 | layout(points) in;
4 | layout(triangle_strip, max_vertices = 4) out;
5 |
6 | out vec2 texCoords;
7 |
8 | void main()
9 | {
10 | gl_Position = vec4(1.0, 1.0, 0.0, 1.0);
11 | texCoords = vec2(1.0, 1.0);
12 | EmitVertex();
13 |
14 | gl_Position = vec4(-1.0, 1.0, 0.0, 1.0);
15 | texCoords = vec2(0.0, 1.0);
16 | EmitVertex();
17 |
18 | gl_Position = vec4(1.0,-1.0, 0.0, 1.0);
19 | texCoords = vec2(1.0, 0.0);
20 | EmitVertex();
21 |
22 | gl_Position = vec4(-1.0,-1.0, 0.0, 1.0);
23 | texCoords = vec2(0.0, 0.0);
24 | EmitVertex();
25 |
26 | EndPrimitive();
27 | }
28 |
--------------------------------------------------------------------------------
/src/voxelizer.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include "widget/Mainframe.h"
3 |
4 | int main(int argc, char** argv) {
5 | QApplication app(argc, argv);
6 |
7 | Mainframe frame;
8 | frame.show();
9 |
10 | return app.exec();
11 | }
12 |
--------------------------------------------------------------------------------
/src/widget/KittiReader.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include "rv/string_utils.h"
11 |
12 | void KittiReader::initialize(const QString& directory) {
13 | velodyne_filenames_.clear();
14 |
15 | QDir base_dir(directory);
16 | QDir velodyne_dir(base_dir.filePath("velodyne"));
17 | if (!velodyne_dir.exists()) throw std::runtime_error("Missing velodyne files.");
18 | QStringList entries = velodyne_dir.entryList(QDir::Files, QDir::Name);
19 | for (int32_t i = 0; i < entries.size(); ++i) {
20 | velodyne_filenames_.push_back(velodyne_dir.filePath(entries.at(i)).toStdString());
21 | }
22 |
23 | if (!base_dir.exists("calib.txt"))
24 | throw std::runtime_error("Missing calibration file: " + base_dir.filePath("calib.txt").toStdString());
25 |
26 | calib_.initialize(base_dir.filePath("calib.txt").toStdString());
27 |
28 | readPoses(base_dir.filePath("poses.txt").toStdString(), poses_);
29 |
30 | // create label dir, etc.
31 | QDir labels_dir(base_dir.filePath("labels"));
32 |
33 | // find corresponding label files.
34 | if (!labels_dir.exists()) base_dir.mkdir("labels");
35 |
36 | for (uint32_t i = 0; i < velodyne_filenames_.size(); ++i) {
37 | std::ifstream in(velodyne_filenames_[i].c_str());
38 | in.seekg(0, std::ios::end);
39 | uint32_t num_points = in.tellg() / (4 * sizeof(float));
40 | in.close();
41 |
42 | QString filename = QFileInfo(QString::fromStdString(velodyne_filenames_[i])).baseName() + ".label";
43 | if (!labels_dir.exists(filename)) {
44 | std::ofstream out(labels_dir.filePath(filename).toStdString().c_str());
45 |
46 | std::vector labels(num_points, 0);
47 | out.write(reinterpret_cast