├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── assets ├── KIT_passat.mtl ├── KIT_passat.obj ├── KIT_tire.mtl ├── KIT_tire.obj ├── KIT_velodyne.mtl └── KIT_velodyne.obj ├── config ├── default.xml └── vlp16_example.xml ├── external ├── find_dependencies.cmake ├── glow.cmake └── gtsam.cmake ├── src ├── core │ ├── Frame.h │ ├── Frame2Model.cpp │ ├── Frame2Model.h │ ├── ImagePyramidGenerator.cpp │ ├── ImagePyramidGenerator.h │ ├── LieGaussNewton.cpp │ ├── LieGaussNewton.h │ ├── Objective.h │ ├── Posegraph.cpp │ ├── Posegraph.h │ ├── Preprocessing.cpp │ ├── Preprocessing.h │ ├── Surfel.h │ ├── SurfelMap.cpp │ ├── SurfelMap.h │ ├── SurfelMapping.cpp │ ├── SurfelMapping.h │ ├── lie_algebra.cpp │ └── lie_algebra.h ├── genvideo.cpp ├── io │ ├── KITTIReader.cpp │ ├── KITTIReader.h │ ├── LaserscanReader.h │ ├── RobocarReader.cpp │ ├── RobocarReader.h │ ├── SimulationReader.cpp │ └── SimulationReader.h ├── opengl │ ├── MatrixStack.h │ ├── Mesh.cpp │ ├── Mesh.h │ ├── Model.cpp │ ├── Model.h │ ├── ObjReader.cpp │ └── ObjReader.h ├── rv │ ├── CompositeParameter.cpp │ ├── CompositeParameter.h │ ├── Exception.h │ ├── FileUtil.cpp │ ├── FileUtil.h │ ├── IOError.h │ ├── Laserscan.cpp │ ├── Laserscan.h │ ├── LaserscanReader.h │ ├── Math.cpp │ ├── Math.h │ ├── Parameter.cpp │ ├── Parameter.h │ ├── ParameterList.cpp │ ├── ParameterList.h │ ├── ParameterListIterator.cpp │ ├── ParameterListIterator.h │ ├── PrimitiveParameters.cpp │ ├── PrimitiveParameters.h │ ├── Random.cpp │ ├── Random.h │ ├── RangeParameter.cpp │ ├── RangeParameter.h │ ├── RingBuffer.h │ ├── Stopwatch.cpp │ ├── Stopwatch.h │ ├── XmlDocument.cpp │ ├── XmlDocument.h │ ├── XmlError.h │ ├── XmlNode.cpp │ ├── XmlNode.h │ ├── geometry.h │ ├── point_traits.h │ ├── string_utils.cpp │ ├── string_utils.h │ ├── transform.cpp │ ├── transform.h │ ├── yxml.c │ └── yxml.h ├── shader │ ├── Frame2Model_jacobians.frag │ ├── Frame2Model_jacobians.geom │ ├── Frame2Model_jacobians.vert │ ├── avg_vertexmap.frag │ ├── bilateral_filter.frag │ ├── color.glsl │ ├── coloredvertices.frag │ ├── coloredvertices.vert │ ├── copy_surfels.geom │ ├── copy_surfels.vert │ ├── draw_depthimg.frag │ ├── draw_mesh.frag │ ├── draw_mesh.vert │ ├── draw_normalmap.frag │ ├── draw_normalmap3d.geom │ ├── draw_normalmap3d.vert │ ├── draw_posegraph_edge.geom │ ├── draw_residuals.frag │ ├── draw_submaps.geom │ ├── draw_submaps.vert │ ├── draw_surfelPoints.vert │ ├── draw_surfels.frag │ ├── draw_surfels.geom │ ├── draw_surfels.vert │ ├── draw_surfels_coords.geom │ ├── draw_vertexmap3d.vert │ ├── empty.frag │ ├── empty.vert │ ├── extract_surfels.vert │ ├── gen_depthimg.frag │ ├── gen_depthimg.vert │ ├── gen_indexmap.frag │ ├── gen_indexmap.vert │ ├── gen_normalmap.frag │ ├── gen_surfels.frag │ ├── gen_surfels.geom │ ├── gen_surfels.vert │ ├── gen_surfels330.geom │ ├── gen_vertexmap.frag │ ├── gen_vertexmap.vert │ ├── init_depthimg.frag │ ├── init_radiusConf.frag │ ├── init_radiusConf.vert │ ├── laserscan.frag │ ├── laserscan.vert │ ├── passthrough.frag │ ├── quad.geom │ ├── render_compose.frag │ ├── render_surfels.frag │ ├── render_surfels.geom │ ├── render_surfels.vert │ ├── update_surfels.frag │ ├── update_surfels.geom │ └── update_surfels.vert ├── util │ ├── Log.cpp │ ├── Log.h │ ├── ScanAccumulator.cpp │ ├── ScanAccumulator.h │ ├── kitti_utils.cpp │ └── kitti_utils.h └── visualizer │ ├── GraphWidget.cpp │ ├── GraphWidget.h │ ├── ViewportWidget.cpp │ ├── ViewportWidget.h │ ├── VisualizerWindow.cpp │ ├── VisualizerWindow.h │ ├── visualizer.cpp │ └── visualizer.ui └── test ├── CMakeLists.txt ├── calib.txt ├── core ├── CalibTest.cpp ├── EvalTest.cpp ├── PosegraphTest.cpp ├── PyramidTest.cpp ├── lie_test.cpp ├── matrix.cpp └── matrix.h ├── gt_poses.txt ├── opengl ├── depthimg-test.cpp ├── icp-test.cpp ├── jacobian-test.cpp ├── main-opengl.cpp ├── ndc.frag ├── ndc.vert ├── pix2ndc.frag ├── pix2ndc.vert ├── rounding.frag ├── rounding.vert ├── rounding_access.frag ├── rounding_access_rect.frag ├── test.frag └── testNDC.cpp ├── result_poses.txt ├── rv ├── fileutil-test.cpp ├── geometry-test.cpp ├── math-test.cpp ├── parameter-test.cpp ├── parameterlist-test.cpp ├── parameterlistiterator-test.cpp ├── transform-test.cpp └── xml-test.cpp ├── scan0.bin ├── scan1.bin └── test_helpers.h /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | IndentWidth: 2 3 | ColumnLimit: 120 4 | AllowShortFunctionsOnASingleLine: Inline 5 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.gitignore.io/api/c++,cmake,cuda,qt 2 | 3 | ### C++ ### 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | 22 | # Compiled Static libraries 23 | *.lai 24 | *.la 25 | *.a 26 | *.lib 27 | 28 | # Executables 29 | *.exe 30 | *.out 31 | *.app 32 | 33 | 34 | ### CMake ### 35 | CMakeCache.txt 36 | CMakeFiles 37 | CMakeScripts 38 | Makefile 39 | cmake_install.cmake 40 | install_manifest.txt 41 | 42 | 43 | ### CUDA ### 44 | *.i 45 | *.ii 46 | *.gpu 47 | *.ptx 48 | *.cubin 49 | *.fatbin 50 | 51 | 52 | ### Qt ### 53 | # C++ objects and libs 54 | 55 | *.slo 56 | *.lo 57 | *.o 58 | *.a 59 | *.la 60 | *.lai 61 | *.so 62 | *.dll 63 | *.dylib 64 | 65 | # Qt-es 66 | 67 | /.qmake.cache 68 | /.qmake.stash 69 | *.pro.user 70 | *.pro.user.* 71 | *.qbs.user 72 | *.qbs.user.* 73 | *.moc 74 | moc_*.cpp 75 | qrc_*.cpp 76 | ui_*.h 77 | Makefile* 78 | *build-* 79 | 80 | # QtCreator 81 | 82 | *.autosave 83 | 84 | # QtCtreator Qml 85 | *.qmlproject.user 86 | *.qmlproject.user.* 87 | 88 | # QtCtreator CMake 89 | CMakeLists.txt.user 90 | 91 | # project-related files 92 | build 93 | .* 94 | IDEAS.md 95 | *~ 96 | evaluation/result 97 | *.pyc 98 | 99 | # generated folders 100 | bin 101 | lib 102 | 103 | evaluation 104 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016-2018 Jens Behley 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Surfel-based Mapping using 3D Laser Range Data 2 | 3 | Mapping of 3d laser range data from a rotating laser range scanner, e.g., the Velodyne HDL-64E. 4 | For representing the map, we use surfels that enables fast rendering of the map for point-to-plane ICP and loop closure detection. 5 | 6 | ## Publication 7 | 8 | If you use our implementation in your academic work, please cite the corresponding paper: 9 | 10 | J. Behley, C. Stachniss. *Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments*, Proc. of Robotics: Science and Systems (RSS), 2018. 11 | 12 | The BibTeX entry for the paper is: 13 | 14 | @inproceedings{behley2018rss, 15 | author = {Jens Behley and Cyrill Stachniss}, 16 | title = {Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments}, 17 | booktitle = {Proc.~of Robotics: Science and Systems~(RSS)}, 18 | year = {2018} 19 | } 20 | 21 | 22 | ## Dependencies 23 | 24 | * Qt5 >= 5.2.1 25 | * OpenGL >= 3.3 26 | * libEigen >= 3.2 27 | 28 | In Ubuntu 22.04/20.04: Installing all dependencies is accomplished by: 29 | ```bash 30 | $ sudo apt-get install -y build-essential cmake libeigen3-dev libboost-all-dev qtbase5-dev libglew-dev 31 | ``` 32 | 33 | ## Build 34 | 35 | ``` 36 | $ mkdir build && cd build 37 | $ cmake .. -DCMAKE_BUILD_TYPE=Release -DOPENGL_VERSION=430 -DENABLE_NVIDIA_EXT=YES 38 | $ make -j5 39 | ``` 40 | 41 | Where you have to set `OPENGL_VERSION` to the supported OpenGL core profile version of your system, which you can query as follows: 42 | 43 | ```bash 44 | $ glxinfo | grep "version" 45 | server glx version string: 1.4 46 | client glx version string: 1.4 47 | GLX version: 1.4 48 | OpenGL core profile version string: 4.3.0 NVIDIA 367.44 49 | OpenGL core profile shading language version string: 4.30 NVIDIA [...] 50 | OpenGL version string: 4.5.0 NVIDIA 367.44 51 | OpenGL shading language version string: 4.50 NVIDIA 52 | ``` 53 | 54 | Here the line `OpenGL core profile version string: 4.3.0 NVIDIA 367.44` is important and therefore you should use `-DOPENGL_VERSION = 430`. If you are unsure you can also leave it on the default version `330`, which should be supported by all OpenGL-capable devices. 55 | 56 | If you have a NVIDIA device, like a Geforce or Quadro graphics card, you should also activate the NVIDIA extensions using `-DENABLE_NVIDIA_EXT=YES` for info about the current GPU memory usage of the program. 57 | 58 | Now the project root directory should contain a `bin` directory containing the visualizer. 59 | 60 | ## How to run and use it? 61 | 62 | All binaries are copied to the `bin` directory of the source folder of the project. Thus, 63 | 1. run `visualizer` in the `bin` directory, 64 | 2. open a Velodyne directory from the KITTI Visual Odometry Benchmark and select a ".bin" file, 65 | 3. start the processing of the scans via the "play button" in the GUI. 66 | 67 | In the `config` directory, different configuration files are given, which can be used as reference to set parameters for some experiments with other data. Specifying the right "vertical Field-of-View" (`data_fov_up` and `data_fov_down`) and the right number of scan lines (`data_height`) are the most important parameters. 68 | 69 | See also the [project page](http://jbehley.github.io/projects/surfel_mapping/) for configuration files used for the evaluation in the paper. 70 | 71 | ## License 72 | 73 | 74 | Copyright 2018 Jens Behley, University of Bonn. 75 | 76 | This project is free software made available under the MIT License. For details see the LICENSE file. 77 | 78 | 79 | -------------------------------------------------------------------------------- /assets/KIT_passat.mtl: -------------------------------------------------------------------------------- 1 | # inside rear stuff; non visible. 2 | newmtl material0 3 | Ka 0.01 0.01 0.01 4 | Kd 1 1 1 5 | Ks 1 1 1 6 | Ke 0 0 0 7 | Ns 128 8 | d 1 9 | # roof profiles. 10 | newmtl material1 11 | Ka 0.01 0.01 0.01 12 | Kd 0.752941 0.752941 0.752941 13 | Ks 0.376471 0.376471 0.376471 14 | Ke 0 0 0 15 | Ns 8 16 | d 1 17 | # velodyne box 18 | newmtl material2 19 | Ka 0 0 0 20 | Kd 0.607843 0.607843 0.607843 21 | Ks 0.2 0.2 0.2 22 | Ke 0 0 0 23 | Ns 8 24 | d 1 25 | # nozzle on the roof 26 | newmtl material3 27 | Ka 0 0 0 28 | Kd 0 0.0 1.0 29 | Ks 0.290196 0.290196 1 30 | Ke 0 0 0 31 | Ns 8 32 | d 1 33 | # brake light 34 | newmtl material4 35 | Ka 0 0 0 36 | Kd 0.709804 0 0 37 | Ks 1 0 0 38 | Ke 0.0 0.0 0 39 | Ns 128 40 | d 1 41 | # ??? 42 | newmtl material5 43 | Ka 0 0 0 44 | Kd 0.6 0.6 0.6 45 | Ks 1 0 0 46 | Ke 0 0 0 47 | Ns 128 48 | d 0.4 49 | # side and back glass tint 50 | newmtl material6 51 | Ka 0 0 0 52 | Kd 0.003922 0.078431 0 53 | Ks 1 1 1 54 | Ke 0 0 0 55 | Ns 90.5097 56 | d 0.1 57 | # brake lights glass 58 | newmtl material7 59 | Ka 0 0 0 60 | Kd 1 0.470588 0 61 | Ks 1 0.470588 0 62 | Ke 0 0 0 63 | Ns 128 64 | d 0.4 65 | # hood at the back // changed to black 66 | newmtl material8 67 | Ka 0.05 0.05 0.05 68 | Kd 0.1 0.1 0.1 69 | # Ks 0.290196 0.290196 1 70 | Ks 0.2 0.2 0.2 71 | Ke 0 0 0 72 | #Ns 64 73 | Ns 32 74 | d 1 75 | # blackish window tint 76 | newmtl material9 77 | Ka 0.2 0.2 0.2 78 | Kd 0 0 0 79 | Ks 0.2 0.2 0.2 80 | Ke 0 0 0 81 | Ns 4 82 | d 1 83 | # inside cover material 84 | newmtl material10 85 | Ka 0 0 0 86 | Kd 0.498039 0.498039 0.498039 87 | Ks 0.5 0.5 0.5 88 | Ke 0 0 0 89 | Ns 32 90 | d 1 91 | # rear windshield wiper 92 | newmtl material11 93 | Ka 0 0 0 94 | #Kd 0.74902 0.74902 0.74902 95 | Kd 0.04902 0.04902 0.04902 96 | Ks 0.2 0.2 0.2 97 | Ke 0 0 0 98 | Ns 2 99 | d 1 100 | # cockpit 101 | newmtl material12 102 | Ka 0 0 0 103 | Kd 0.498039 0.498039 0.498039 104 | Ks 0.2 0.2 0.2 105 | Ke 0 0 0 106 | Ns 2 107 | d 1 108 | # CAR PAINT 109 | newmtl material13 110 | Ka 0 0 0 111 | Kd 0 0 1 112 | Ks 0.423529 0.423529 1 113 | Ke 0 0 0 114 | # Ns 8 115 | Ns 128 116 | d 1 117 | # front window tint 118 | newmtl material14 119 | Ka 0 0 0 120 | Kd 1 1 1 121 | Ks 1 1 1 122 | Ke 0 0 0 123 | Ns 90.5097 124 | d 0.25 125 | # inside material of seats 126 | newmtl material15 127 | Ka 0 0 0 128 | Kd 0.139516 0.139516 0.139516 129 | Ks 0.033146 0.033146 0.033146 130 | Ke 0 0 0 131 | Ns 8 132 | d 1 133 | # turn signals front 134 | newmtl material16 135 | Ka 0 0 0 136 | Kd 1 0.470588 0 137 | Ks 1 0.470588 0 138 | Ke 0.1 0.02 0 139 | Ns 128 140 | d 1 141 | # plastic on car 142 | newmtl material17 143 | Ka 0 0 0 144 | Kd 0.164491 0.164491 0.164491 145 | Kd 0.05 0.05 0.05 146 | Ks 0.2 0.2 0.2 147 | Ke 0 0 0 148 | Ns 2 149 | d 1 150 | # grill, VW logo, material, etc. 151 | newmtl material18 152 | Ka 0 0 0 153 | Kd 0.5 0.5 0.5 154 | Ks 1 1 1 155 | Ke 0 0 0 156 | Ns 128 157 | d 1 158 | # licence plate 159 | newmtl material19 160 | Ka 0.2 0.2 0.2 161 | Kd 1 1 1 162 | Ks 0.2 0.2 0.2 163 | Ke 0 0 0 164 | Ns 128 165 | d 1 166 | -------------------------------------------------------------------------------- /assets/KIT_tire.mtl: -------------------------------------------------------------------------------- 1 | newmtl material0 2 | Ka 0 0 0 3 | Kd 0.5 0.5 0.5 4 | Ks 1 1 1 5 | Ke 0 0 0 6 | Ns 32 7 | d 1 8 | newmtl material1 9 | Ka 0 0 0 10 | Kd 1 1 1 11 | Ks 1 1 1 12 | Ke 0 0 0 13 | Ns 128 14 | d 1 15 | newmtl material2 16 | Ka 0 0 0 17 | Kd 0.164491 0.164491 0.164491 18 | Ks 0.2 0.2 0.2 19 | Ke 0 0 0 20 | Ns 2 21 | d 1 22 | -------------------------------------------------------------------------------- /assets/KIT_velodyne.mtl: -------------------------------------------------------------------------------- 1 | newmtl material0 2 | Ka 0.2 0.2 0.2 3 | Kd 0 0 0 4 | Ks 0.2 0.2 0.2 5 | Ke 0 0 0 6 | Ns 8 7 | d 0.85 8 | newmtl material1 9 | Ka 0 0 0 10 | Kd 0.784314 0.784314 0.784314 11 | Ks 0.2 0.2 0.2 12 | Ke 0 0 0 13 | Ns 8 14 | d 1 15 | newmtl material2 16 | Ka 0 0 0 17 | Kd 0.752941 0.752941 0.752941 18 | Ks 0.2 0.2 0.2 19 | Ke 0 0 0 20 | Ns 8 21 | d 1 22 | -------------------------------------------------------------------------------- /config/default.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 900 4 | 64 5 | 3.0 6 | -25.0 7 | 75.0 8 | 2.0 9 | 0.0 10 | 0.0 11 | 12 | 13 | 10 14 | 0.0001 15 | 0.0001 16 | 2.0 17 | 30.0 18 | huber 19 | 0.5 20 | false 21 | true 22 | 10.0 23 | true 24 | 25 | 26 | 900 27 | 64 28 | 3.0 29 | -25.0 30 | 100.0 31 | 0.0 32 | 33 | 34 | true 35 | 8.0 36 | 37 | 38 | true 39 | 0.5 40 | 30.0 41 | false 42 | 43 | 44 | frame-to-model 45 | 46 | 0.05 47 | 1.0 48 | 90.0 49 | 0.2 50 | 45.0 51 | 3 52 | 3 53 | 0.0 54 | 55 | 0.6 56 | 0.5 57 | 58 | 1 59 | 1 60 | 61 | true 62 | 63 | 1 64 | 65 | 66 | 4 67 | 10.0 68 | true 69 | 70 | 200 71 | 5 72 | 73 | 74 | true 75 | 1.15 76 | 0.95 77 | 1.1 78 | 50 79 | 5 80 | 50 81 | 82 | -------------------------------------------------------------------------------- /config/vlp16_example.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 360 15 | 16 16 | 15 17 | -15.0 18 | 10 19 | 0.0001 20 | 0.0001 21 | 3.0 22 | 30.0 23 | 75.0 24 | 0.5 25 | 26 | 0.0 27 | 0.0 28 | 29 | huber 30 | 0.5 31 | true 32 | 33 | false 34 | 2.5 35 | 2.5 36 | false 37 | 38 | 20.0 39 | false 40 | 0.05 41 | 0.5 42 | 43 | frame-to-model 44 | 360 45 | 16 46 | 15.0 47 | -15.0 48 | 0.0 49 | 100.0 50 | 0.2 51 | 60.0 52 | 53 | 3 54 | 3 55 | 0.0 56 | 0.6 57 | 0.4 58 | 0.5 59 | 60 | 1.0 61 | 1.0 62 | 63 | false 64 | 1.0 65 | 1.0 66 | 67 | true 68 | 2.0 69 | 70 | true 71 | 1.0 72 | 45.0 73 | 74 | 50 75 | 2 76 | 77 | 78 | 100 79 | 20 80 | 150000 81 | 82 | 83 | -------------------------------------------------------------------------------- /external/find_dependencies.cmake: -------------------------------------------------------------------------------- 1 | # not sure if we need to check if other system versions are available. 2 | 3 | include("${CMAKE_CURRENT_LIST_DIR}/glow.cmake") 4 | include("${CMAKE_CURRENT_LIST_DIR}/gtsam.cmake") 5 | -------------------------------------------------------------------------------- /external/glow.cmake: -------------------------------------------------------------------------------- 1 | message(STATUS "[SuMa] 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) 11 | -------------------------------------------------------------------------------- /external/gtsam.cmake: -------------------------------------------------------------------------------- 1 | message(STATUS "[SuMa] Fetching GTSAM v4.2.0.") 2 | 3 | include(FetchContent) 4 | FetchContent_Declare(gtsam GIT_REPOSITORY https://github.com/borglab/gtsam.git GIT_TAG 4.2.0) 5 | 6 | # some flags for building only necessary stuff from GTSAM. 7 | set(GTSAM_BUILD_TESTS Off CACHE BOOL "") 8 | set(GTSAM_BUILD_EXAMPLES_ALWAYS Off CACHE BOOL "") 9 | set(GTSAM_BUILD_EXAMPLES Off CACHE BOOL "") 10 | set(GTSAM_BUILD_UNSTABLE Off CACHE BOOL "") 11 | set(GTSAM_UNSTABLE_BUILD_PYTHON Off CACHE BOOL "") 12 | set(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX Off CACHE BOOL "") 13 | set(GTSAM_BUILD_PYTHON Off CACHE BOOL "") 14 | set(GTSAM_INSTALL_MATLAB_TOOLBOX Off CACHE BOOL "") 15 | 16 | set(GTSAM_USE_SYSTEM_EIGEN On CACHE BOOL "") # only relevant option! 17 | 18 | FetchContent_MakeAvailable(gtsam) -------------------------------------------------------------------------------- /src/core/Frame.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_FRAME_H_ 2 | #define INCLUDE_CORE_FRAME_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | class SurfelMap; 14 | 15 | /** \brief data class holding all information for a specific timestep. 16 | * 17 | * \author behley 18 | */ 19 | 20 | class Frame { 21 | public: 22 | typedef std::shared_ptr Ptr; 23 | 24 | Frame(uint32_t w, uint32_t h) 25 | : valid(false), 26 | width(w), 27 | height(h), 28 | vertex_map(width, height, glow::TextureFormat::RGBA_FLOAT), 29 | normal_map(width, height, glow::TextureFormat::RGBA_FLOAT), 30 | residual_map(width, height, glow::TextureFormat::RGBA_FLOAT) { 31 | points.reserve(150000); 32 | 33 | // interpolation parameters. 34 | vertex_map.setMinifyingOperation(glow::TexRectMinOp::NEAREST); 35 | vertex_map.setMagnifyingOperation(glow::TexRectMagOp::NEAREST); 36 | vertex_map.setWrapOperation(glow::TexRectWrapOp::CLAMP_TO_BORDER, glow::TexRectWrapOp::CLAMP_TO_BORDER); 37 | normal_map.setMinifyingOperation(glow::TexRectMinOp::NEAREST); 38 | normal_map.setMagnifyingOperation(glow::TexRectMagOp::NEAREST); 39 | normal_map.setWrapOperation(glow::TexRectWrapOp::CLAMP_TO_BORDER, glow::TexRectWrapOp::CLAMP_TO_BORDER); 40 | 41 | CheckGlError(); 42 | } 43 | 44 | void copy(const Frame& other) { 45 | assert(width == other.width && height == other.height && "Frame dimensions must match."); 46 | 47 | valid = other.valid; 48 | vertex_map.copy(other.vertex_map); 49 | normal_map.copy(other.normal_map); 50 | points.assign(other.points); 51 | residual_map.copy(other.residual_map); 52 | map = other.map; 53 | 54 | pose = other.pose; 55 | } 56 | 57 | bool valid; 58 | uint32_t width, height; 59 | 60 | glow::GlTextureRectangle vertex_map; // (x,y,z) & w encodes validity 61 | glow::GlTextureRectangle normal_map; // (x,y,z) & w encodes validity 62 | 63 | glow::GlBuffer points{glow::BufferTarget::ARRAY_BUFFER, glow::BufferUsage::DYNAMIC_DRAW}; 64 | 65 | Eigen::Matrix4f pose{Eigen::Matrix4f::Identity()}; // estimated pose of that frame. 66 | 67 | std::shared_ptr map; 68 | 69 | glow::GlTextureRectangle residual_map; 70 | }; 71 | 72 | #endif /* INCLUDE_CORE_FRAME_H_ */ 73 | -------------------------------------------------------------------------------- /src/core/Frame2Model.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_FRAME2MODEL2_H_ 2 | #define INCLUDE_CORE_FRAME2MODEL2_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "Frame.h" 13 | #include "core/Objective.h" 14 | 15 | /** \brief Project point cloud, use projections to associate with model vertex, normal map. 16 | * 17 | * This is essentially ProjectedCloud to frame, but with computation of weights for 18 | * Iterative Re-weighted (Non-linear) Least Squares enabling to use different M-Estimators, like 19 | * Huber weights and Turkey Bi-square weights. 20 | * 21 | * This variant transforms the point cloud and tries to match this to the model frame rendered 22 | * at the last position. 23 | * 24 | * \author behley 25 | */ 26 | class Frame2Model : public Objective { 27 | public: 28 | /** \brief setup objective. **/ 29 | Frame2Model(const rv::ParameterList& params); 30 | 31 | /** \brief set single parameter to specific value. **/ 32 | void setParameter(const rv::Parameter& param) override; 33 | 34 | void setData(const std::shared_ptr& current, const std::shared_ptr& last); 35 | 36 | /** \brief specify at which level the redisual/jacobian is computed. **/ 37 | void setLevel(uint32_t lvl) override; 38 | 39 | /** \brief get maximum level (coarse-to-fine). **/ 40 | uint32_t getMaxLevel() const override; 41 | 42 | uint32_t num_parameters() const; 43 | 44 | /** \brief compute residual for given increment. **/ 45 | double residual(const Eigen::VectorXd& delta); 46 | 47 | /** \brief compute weighted JtJ and Jtf exploiting intermediate computations, return weighted objective F(x). */ 48 | double jacobianProducts(Eigen::MatrixXd& JtJ, Eigen::MatrixXd& Jtf); 49 | 50 | protected: 51 | /** \brief updatable parameters, i.e., params that can be changed at runtime. **/ 52 | void updateParameters(); 53 | 54 | rv::ParameterList params_; 55 | 56 | std::shared_ptr current_; 57 | std::shared_ptr last_; // model frame. 58 | 59 | uint32_t num_residuals_{0}; 60 | 61 | glow::GlSampler sampler_; 62 | 63 | glow::GlVertexArray vao_img_coords_; 64 | glow::GlBuffer vbo_img_coords_{glow::BufferTarget::ARRAY_BUFFER, glow::BufferUsage::STATIC_DRAW}; 65 | 66 | glow::GlTexture JtJJtf_blend_; 67 | glow::GlFramebuffer fbo_blend_; 68 | glow::GlProgram program_blend_; 69 | 70 | uint32_t entriesPerKernel_{64}; 71 | }; 72 | 73 | #endif /* INCLUDE_CORE_FRAME2MODEL_H_ */ 74 | -------------------------------------------------------------------------------- /src/core/ImagePyramidGenerator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void ImagePyramidGenerator::makePyramid(uint32_t max_levels, uint32_t width, uint32_t height, 4 | std::vector& img_coords, std::vector& sizes) { 5 | sizes.resize(max_levels); 6 | std::fill(sizes.begin(), sizes.end(), glow::vec2(-1, -1)); 7 | 8 | sizes[0] = glow::vec2(width, height); 9 | 10 | uint32_t new_max_level = shuffle(1, img_coords, sizes); 11 | 12 | // reorder and shrink. 13 | std::reverse(&sizes[0], &sizes[new_max_level]); 14 | sizes.resize(new_max_level); 15 | } 16 | 17 | int32_t ImagePyramidGenerator::shuffle(uint32_t level, std::vector& img_coords, 18 | std::vector& sizes) { 19 | if (level >= sizes.size()) return level; // maximum level reached. 20 | 21 | uint32_t w_old = sizes[level - 1].x, h_old = sizes[level - 1].y; 22 | if (w_old == 1 || h_old == 1) return level; // not sensible to devide further. 23 | 24 | std::vector temp(w_old * h_old); 25 | 26 | uint32_t w_new = w_old / 2, h_new = h_old; 27 | 28 | uint32_t stride = 2 << (level - 1); 29 | 30 | uint32_t next_level = 0; 31 | uint32_t prev_level = w_old * h_old - 1; 32 | for (uint32_t i = 0; i < w_old * h_old; ++i) { 33 | uint32_t x = img_coords[i].x; 34 | // uint32_t y = img_coords[i].y; 35 | if (x % stride == 0) { 36 | temp[next_level++] = img_coords[i]; 37 | } else { 38 | temp[prev_level--] = img_coords[i]; 39 | } 40 | } 41 | 42 | // assert(next_level == w_new * h_new); 43 | // assert(prev_level == w_old * h_old); 44 | 45 | sizes[level] = glow::vec2(w_new, h_new); 46 | // memcpy(img_coords.data(), temp.data(), temp.size() * sizeof(vec2)); 47 | for (uint32_t i = 0; i < temp.size(); ++i) { 48 | img_coords[i] = temp[i]; 49 | } 50 | 51 | return shuffle(level + 1, img_coords, sizes); 52 | } 53 | -------------------------------------------------------------------------------- /src/core/ImagePyramidGenerator.h: -------------------------------------------------------------------------------- 1 | #ifndef SRC_CORE_IMAGEPYRAMIDGENERATOR_H_ 2 | #define SRC_CORE_IMAGEPYRAMIDGENERATOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | /** \brief generate a image pyramid by reordering image coordinates. 8 | * 9 | * To get a simple way of addressing the different layers of a image pyramid, 10 | * we want to reorder the given image coordinates, such that the coordinates are 11 | * ordered according to the level from coarse-to-fine. This means the array of 12 | * image coordinates contains in the beginning all points shared by all levels. 13 | * Directly following are the coordinates of the remaining letters. 14 | * 15 | * Output is the reordered array of image coordinates and a sizes array. You can 16 | * now access the resulting array like img_coords[0:sizes[level]] and get 17 | * the corresponding image coordinate at the specific level from coarse to fine. 18 | * 19 | * This particular order should make it easy to evaluate the residual at different 20 | * levels using a vertex shader. Another advantage is that we only need a single 21 | * array of image coordinates. 22 | * 23 | * In contrast to the usual downsampling, we use always the exact same image 24 | * coordinates and do not apply something like a Gaussian blur. 25 | * 26 | * \author behley 27 | */ 28 | class ImagePyramidGenerator { 29 | public: 30 | /** \brief Generate pyramid from given img_coords, returning reordered img_coords and sizes of each level. 31 | * 32 | * if it is not possible to generate max_levels of subsampled images, we simply resize sizes to max level. 33 | * 34 | * \param max_levels specifies how many levels we want to have in the end. 35 | * \param width, height of the initial image. 36 | * \param imag_coords array of image coords, which will be reordered. 37 | * \param size of resulting images at given level ordered from coarse to fine. 38 | * 39 | **/ 40 | static void makePyramid(uint32_t max_levels, uint32_t width, uint32_t height, std::vector& img_coords, 41 | std::vector& sizes); 42 | 43 | protected: 44 | /** \brief recursively reorder img_coords to generate pyramid of image coordinates. 45 | * 46 | * never called directly. 47 | * 48 | * \param level currently generated level. 49 | * \param img_coords reordered image coordinates. 50 | * \param sizes array of (width, height) of each level of the pyramid. 51 | */ 52 | static int32_t shuffle(uint32_t level, std::vector& img_coords, std::vector& sizes); 53 | }; 54 | 55 | #endif /* SRC_CORE_IMAGEPYRAMIDGENERATOR_H_ */ 56 | -------------------------------------------------------------------------------- /src/core/LieGaussNewton.cpp: -------------------------------------------------------------------------------- 1 | #include "core/LieGaussNewton.h" 2 | #include 3 | #include 4 | 5 | using namespace rv; 6 | 7 | LieGaussNewton::LieGaussNewton() { 8 | params_.insert(IntegerParameter("max iterations", maxIter)); 9 | params_.insert(FloatParameter("stopping threshold", epsilon)); 10 | params_.insert(FloatParameter("delta", delta)); 11 | } 12 | 13 | int32_t LieGaussNewton::minimize(Objective& F, const Eigen::Matrix4d& T0) { 14 | history_.clear(); 15 | 16 | initialize(F, T0); 17 | 18 | std::vector maxIterations({maxIter, maxIter, maxIter, 3, 3, 3}); 19 | 20 | for (uint32_t i = 0; i <= F.getMaxLevel(); ++i) { 21 | F.setLevel(i); 22 | k_ = 0; 23 | // std::cout << " ==== LEVEL " << i << " ==== " << std::endl; 24 | for (;;) { 25 | history_.push_back(Tk_); 26 | /** check if we can stop here. **/ 27 | 28 | if (maxIterations[i] > 0 && k_ >= maxIterations[i]) break; // max iterations reached. :/ 29 | 30 | int32_t result = step(); 31 | // std::cout << "error = " << last_error << std::endl; 32 | if (result == 0) break; // converged. 33 | 34 | ++k_; 35 | } 36 | } 37 | 38 | return 0; // converged, hurray! 39 | } 40 | 41 | void LieGaussNewton::initialize(Objective& F, const Eigen::Matrix4d& T0) { 42 | const uint32_t N = F.num_parameters(); 43 | 44 | JtJ = Eigen::MatrixXd(N, N); 45 | Jtf = Eigen::MatrixXd(N, 1); 46 | 47 | Tk_ = T0; 48 | objective_ = &F; 49 | objective_->initialize(T0); 50 | last_error = std::numeric_limits::max(); // objective_->jacobianProducts(JtJ, Jtf); // get the jacobians. 51 | 52 | if (callback_ != 0) (*callback_)(last_error, Eigen::VectorXd::Zero(N), Tk_); // t = 0 53 | } 54 | 55 | int32_t LieGaussNewton::step() { 56 | assert(objective_ != nullptr); 57 | 58 | int32_t result = 1; 59 | 60 | double current_error = objective_->jacobianProducts(JtJ, Jtf); 61 | 62 | Eigen::VectorXd deltax = JtJ.ldlt().solve(-Jtf); // new direction. 63 | 64 | assert(!std::isnan(deltax[0])); 65 | 66 | if (deltax.lpNorm() < delta) result = 0; // minimal change. 67 | if (std::abs(Jtf.maxCoeff()) < epsilon) result = 0; // gradient. 68 | if (current_error < last_error && std::abs(current_error - last_error) < epsilon) result = 0; // converged. 69 | 70 | objective_->increment(deltax); 71 | 72 | Tk_ = objective_->pose(); 73 | last_error = current_error; 74 | covariance_ = JtJ; 75 | information_ = JtJ; 76 | covDirty_ = true; 77 | 78 | if (callback_ != nullptr) (*callback_)(current_error, deltax, Tk_); 79 | 80 | return result; 81 | } 82 | 83 | void LieGaussNewton::setParameters(const ParameterList& params) { 84 | for (ParameterList::const_iterator it = params.begin(); it != params.end(); ++it) { 85 | if (params_.hasParam(it->name())) { 86 | params_.insert(*it); 87 | } 88 | } 89 | 90 | maxIter = params_["max iterations"]; 91 | epsilon = params_["stopping threshold"]; 92 | delta = params_["delta"]; 93 | } 94 | 95 | const Eigen::MatrixXd& LieGaussNewton::covariance() { 96 | if (covDirty_) { 97 | covariance_ = covariance_.lu().inverse(); 98 | covDirty_ = false; 99 | } 100 | 101 | return covariance_; 102 | } 103 | 104 | const Eigen::MatrixXd& LieGaussNewton::information() { 105 | return information_; 106 | } 107 | 108 | double LieGaussNewton::residual() const { 109 | return last_error; 110 | } 111 | 112 | std::string LieGaussNewton::reason(int32_t errorno) const { 113 | if (errorno == -1) return "Maximum number of iterations reached."; 114 | if (errorno == -2) return "Diverging."; 115 | 116 | return "no error"; 117 | } 118 | 119 | const Eigen::Matrix4d& LieGaussNewton::pose() const { 120 | return Tk_; 121 | } 122 | 123 | uint32_t LieGaussNewton::iterationCount() const { 124 | return k_; 125 | } 126 | -------------------------------------------------------------------------------- /src/core/LieGaussNewton.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_LIEGAUSSNEWTON_H_ 2 | #define INCLUDE_CORE_LIEGAUSSNEWTON_H_ 3 | 4 | #include 5 | #include "Objective.h" 6 | 7 | #include 8 | 9 | /** \brief Gauss-Newton method on Lie Groups, which performs the update using matrix exponentials. 10 | * 11 | * Based on Gauss Newton method, but using poses and pose increments for representation of state. 12 | * The actual implementation of the pose increments in handled by the objective. Therefore the 13 | * optimizer only produces increments, which are then integrated by the objective. 14 | * 15 | * \author behley 16 | **/ 17 | 18 | class OptimizerCallback { 19 | public: 20 | virtual ~OptimizerCallback() {} 21 | 22 | virtual void operator()(float residual, const Eigen::VectorXd& x, const Eigen::Matrix4d& pose) = 0; 23 | }; 24 | 25 | class LieGaussNewton { 26 | public: 27 | LieGaussNewton(); 28 | 29 | void setParameters(const rv::ParameterList& params); 30 | 31 | /** \brief optimize F starting with given pose T0. **/ 32 | int32_t minimize(Objective& F, const Eigen::Matrix4d& T0); 33 | 34 | /** \brief initialize given objective and pose... **/ 35 | void initialize(Objective& F, const Eigen::Matrix4d& T0); 36 | 37 | /** \brief perform a single step with the initialized objective. **/ 38 | int32_t step(); 39 | 40 | /** \brief get last residual. **/ 41 | double residual() const; 42 | 43 | /** \brief get best pose. **/ 44 | const Eigen::Matrix4d& pose() const; 45 | 46 | std::string reason(int32_t errorno) const; 47 | 48 | const Eigen::MatrixXd& covariance(); 49 | 50 | const Eigen::MatrixXd& information(); 51 | 52 | uint32_t iterationCount() const; 53 | 54 | // some constants. 55 | static const int32_t CONVERGED{0}; 56 | 57 | const std::vector& history() const { return history_; } 58 | 59 | protected: 60 | rv::ParameterList params_; 61 | double last_error{134567.00}; 62 | uint32_t k_{0}; 63 | Eigen::Matrix4d Tk_; 64 | Eigen::MatrixXd information_; 65 | Eigen::MatrixXd covariance_; 66 | bool covDirty_{true}; 67 | Objective* objective_{nullptr}; 68 | 69 | Eigen::MatrixXd JtJ, Jtf; 70 | 71 | uint32_t maxIter{200}; 72 | double epsilon{1e-10}, delta{1e-10}; 73 | 74 | std::vector history_; 75 | std::shared_ptr callback_; 76 | }; 77 | 78 | #endif /* INCLUDE_CORE_LIEGAUSSNEWTON_H_ */ 79 | -------------------------------------------------------------------------------- /src/core/Objective.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_PROJECTIVEICP_H_ 2 | #define INCLUDE_CORE_PROJECTIVEICP_H_ 3 | 4 | #include "Frame.h" 5 | #include "lie_algebra.h" 6 | 7 | #include 8 | 9 | #include 10 | 11 | class Objective { 12 | public: 13 | virtual ~Objective() {} 14 | 15 | virtual uint32_t num_parameters() const = 0; 16 | 17 | /** \brief set single parameter to specific value. **/ 18 | virtual void setParameter(const rv::Parameter& param) {} 19 | 20 | /** \brief Current and last frame. 21 | * 22 | * \param current data from current point cloud (the so-called "live" frame) 23 | * \param last date from previous point cloud or data generated from model. (the so-called "model" frame) 24 | * 25 | * At least one is implemented by the derived classes. 26 | **/ 27 | virtual void setData(const std::shared_ptr& current, const std::shared_ptr& last) { 28 | throw std::runtime_error("ProjectiveICP::setData not implemented."); 29 | } 30 | 31 | /** \brief compute residual with given increment. 32 | * 33 | * Beware: naive implementation using jacobianProducts; should be overwritten by derived 34 | * classes for maximal performance 35 | */ 36 | virtual double residual(const Eigen::VectorXd& delta) = 0; 37 | 38 | /** \brief compute JtJ and Jtf exploiting intermediate computations at current pose, return F(x). */ 39 | virtual double jacobianProducts(Eigen::MatrixXd& JtJ, Eigen::MatrixXd& Jtf) = 0; 40 | 41 | /** \brief increment by given delta. **/ 42 | void increment(const Eigen::VectorXd& delta) { 43 | pose_ = SE3::exp(delta) * pose_; 44 | iteration_ += 1; 45 | } 46 | 47 | uint32_t inlier() const { return inlier_; } 48 | uint32_t outlier() const { return outlier_; } 49 | uint32_t valid() const { return (inlier_ + outlier_); } 50 | uint32_t invalid() const { return invalid_; } 51 | 52 | float inlier_residual() const { return inlier_residual_; } 53 | 54 | /** \brief initialize the pose estimate. **/ 55 | void initialize(const Eigen::Matrix4d& T0) { pose_ = T0; } 56 | 57 | const Eigen::Matrix4d& pose() const { return pose_; } 58 | 59 | /** \brief specify at which level the redisual/jacobian is computed. **/ 60 | virtual void setLevel(uint32_t lvl) { // noop 61 | } 62 | 63 | /** \brief get maximum level (coarse-to-fine). **/ 64 | virtual uint32_t getMaxLevel() const { return 0; } 65 | 66 | virtual void reset() { /* noop */ 67 | } 68 | 69 | protected: 70 | // can only be instanced by derived classes. 71 | Objective() {} 72 | 73 | Eigen::Matrix4d pose_{Eigen::Matrix4d::Identity()}; 74 | uint32_t iteration_{0}; 75 | uint32_t inlier_{0}; 76 | float inlier_residual_{0.0f}; 77 | uint32_t outlier_{0}; 78 | uint32_t invalid_{0}; 79 | }; 80 | 81 | #endif /* INCLUDE_CORE_PROJECTIVEICP_H_ */ 82 | -------------------------------------------------------------------------------- /src/core/Posegraph.cpp: -------------------------------------------------------------------------------- 1 | #include "Posegraph.h" 2 | 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace gtsam; 12 | 13 | Posegraph::Posegraph() {} 14 | 15 | void Posegraph::clear() { 16 | graph_ = NonlinearFactorGraph(); 17 | initial_.clear(); 18 | edges_.clear(); 19 | result_.clear(); 20 | } 21 | 22 | Posegraph::Ptr Posegraph::clone() const { 23 | return Posegraph::Ptr(new Posegraph(*this)); 24 | } 25 | 26 | void Posegraph::setInitial(int32_t id, const Eigen::Matrix4d& initial_estimate) { 27 | bool addPrior = initial_.empty(); 28 | 29 | // std::cout << "Initializing " << id << std::endl; 30 | 31 | if (!initial_.exists(id)) 32 | initial_.insert(id, Pose3(initial_estimate)); 33 | else 34 | initial_.update(id, Pose3(initial_estimate)); 35 | 36 | // also update intermediate result. 37 | if (result_.exists(id)) 38 | result_.update(id, Pose3(initial_estimate)); 39 | else 40 | result_.insert(id, Pose3(initial_estimate)); 41 | 42 | if (addPrior) { 43 | Vector6 diagonal; 44 | diagonal << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6; 45 | noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(diagonal); 46 | graph_.add(PriorFactor(id, Pose3(), priorModel)); 47 | } 48 | } 49 | 50 | void Posegraph::addEdge(int32_t from, int32_t to, const Eigen::Matrix4d& measurement, 51 | const Matrix6d& information) { 52 | SharedNoiseModel model = noiseModel::Gaussian::Information(information); 53 | if (robustify_) { 54 | model = noiseModel::Robust::Create(robustifier_, model); 55 | } 56 | 57 | NonlinearFactor::shared_ptr factor(new BetweenFactor(from, to, Pose3(measurement), model)); 58 | graph_.add(factor); 59 | 60 | edges_.push_back({from, to, measurement}); 61 | } 62 | 63 | Eigen::Matrix4d Posegraph::pose(int32_t id) const { 64 | return result_.at(id).matrix(); 65 | } 66 | 67 | std::vector Posegraph::poses() const { 68 | std::vector poses(result_.size()); 69 | 70 | for (const auto& pair : result_) { 71 | if (pair.key >= poses.size()) continue; 72 | 73 | poses[pair.key] = pair.value.cast().matrix(); 74 | } 75 | 76 | return poses; 77 | } 78 | 79 | int32_t Posegraph::size() const { 80 | return result_.size(); 81 | } 82 | 83 | void Posegraph::reinitialize() { 84 | result_ = initial_; 85 | } 86 | 87 | double Posegraph::error() const { 88 | return graph_.error(result_); 89 | } 90 | 91 | bool Posegraph::optimize(uint32_t num_iters) { 92 | // todo: check if every value has been initialized. 93 | 94 | LevenbergMarquardtParams params; 95 | params.maxIterations = num_iters; 96 | params.setVerbosity("TERMINATION"); // this will show info about stopping conditions 97 | params.setVerbosity("SILENT"); 98 | LevenbergMarquardtOptimizer optimizer(graph_, result_, params); 99 | 100 | // std::cout << ">> initial error = " << graph_.error(result_) << std::endl; 101 | result_ = optimizer.optimize(); 102 | // std::cout << ">> final error = " << graph_.error(result_) << std::endl; 103 | 104 | return true; 105 | } 106 | 107 | const gtsam::NonlinearFactorGraph& Posegraph::graph() const { 108 | return graph_; 109 | } 110 | 111 | const gtsam::Values& Posegraph::initial() const { 112 | return initial_; 113 | } 114 | 115 | const gtsam::Values& Posegraph::result() const { 116 | return result_; 117 | } 118 | 119 | void Posegraph::save(const std::string& filename) const {} 120 | void Posegraph::load(const std::string& filename) {} 121 | 122 | void Posegraph::setMEstimator(const gtsam::noiseModel::mEstimator::Base::shared_ptr& m_estimator) { 123 | robustify_ = (m_estimator != nullptr); 124 | robustifier_ = m_estimator; 125 | } 126 | -------------------------------------------------------------------------------- /src/core/Posegraph.h: -------------------------------------------------------------------------------- 1 | #ifndef SRC_CORE_POSEGRAPH_H_ 2 | #define SRC_CORE_POSEGRAPH_H_ 3 | 4 | #include 5 | #include 6 | 7 | class Posegraph { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | typedef std::shared_ptr ConstPtr; 11 | typedef Eigen::Matrix Matrix6d; 12 | typedef Eigen::Matrix Matrix6f; 13 | 14 | class Edge { 15 | public: 16 | Edge(int32_t from_, int32_t to_, const Eigen::Matrix4d& measurement_) 17 | : from(from_), to(to_), measurement(measurement_) {} 18 | int32_t from, to; 19 | Eigen::Matrix4d measurement; 20 | }; 21 | 22 | Posegraph(); 23 | 24 | Posegraph::Ptr clone() const; 25 | 26 | double error() const; 27 | 28 | void clear(); 29 | 30 | /** \brief add estimate for a given node \id. **/ 31 | void setInitial(int32_t id, const Eigen::Matrix4d& initial_estimate); 32 | 33 | /** \brief insert edge from node \from to node \to with given measurement (SE3 transform) and information matrix. 34 | * 35 | * The information matrix is the inverse of the covariance, where the 36 | **/ 37 | void addEdge(int32_t from, int32_t to, const Eigen::Matrix4d& measurement, const Matrix6d& information); 38 | 39 | /** \brief get pose for given id. **/ 40 | Eigen::Matrix4d pose(int32_t id) const; 41 | 42 | /** \brief get (optimized) poses sorted by id of the vertexes. **/ 43 | std::vector poses() const; 44 | 45 | /** \brief size of graph, i.e., number of vertices **/ 46 | int32_t size() const; 47 | 48 | /** \brief reinitialize model with initial estimates. **/ 49 | void reinitialize(); 50 | 51 | /** \brief start optimization for given number of iterations. **/ 52 | bool optimize(uint32_t num_iters); 53 | 54 | void save(const std::string& filename) const; 55 | void load(const std::string& filename); 56 | 57 | /** \brief set m estimator. **/ 58 | void setMEstimator(const gtsam::noiseModel::mEstimator::Base::shared_ptr& m_estimator); 59 | 60 | const gtsam::NonlinearFactorGraph& graph() const; 61 | const gtsam::Values& initial() const; 62 | const gtsam::Values& result() const; 63 | 64 | const std::vector& getEdges() const { return edges_; } 65 | 66 | protected: 67 | std::vector edges_; 68 | gtsam::Values initial_; 69 | gtsam::Values result_; 70 | gtsam::NonlinearFactorGraph graph_; 71 | 72 | bool robustify_{false}; 73 | gtsam::noiseModel::mEstimator::Base::shared_ptr robustifier_; 74 | }; 75 | 76 | #endif /* SRC_CORE_POSEGRAPH_H_ */ 77 | -------------------------------------------------------------------------------- /src/core/Preprocessing.h: -------------------------------------------------------------------------------- 1 | #ifndef SRC_CORE_PREPROCESSING_H_ 2 | #define SRC_CORE_PREPROCESSING_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "core/Frame.h" 15 | 16 | /** \brief Preprocessing of the data. 17 | * 18 | * We essentially take the point cloud, convert it to polar coordinates and generate from 19 | * this a vertex map. We furthermore 20 | * 21 | * Point cloud's coordinate system should be right-handed coordinate system: x - forward, y - left, z - up. 22 | * The generated vertex map and normal map has normalized yaw/azimuth angles on the x-axis(right) and the normalized 23 | * pitch angles on the y-axis (up). Yaw angles ranges from [-pi, pi], where 0 is in the middle of the image. 24 | * The pitch angle ranges from [0, |fov_up| + |fov_down|] and starts at the upper border of the depth image. 25 | * 26 | * the coordinate system therefore looks something like this: 27 | * --------------------------- 28 | * |<--- -yaw- (0,0) -+yaw-->| 29 | * y | | 30 | * | v | 31 | * O--x-->-------------------- 32 | * 33 | * The vertex map and normal map are represented by an OpenGl texture and therefore have their origin in the lower 34 | *left. 35 | * 36 | * The vertex map and normal map store in the fourth coordinate if the point is valid. If the vertex or 37 | * normal is valid, the fourth coordinate is 1; otherwise 0. 38 | * 39 | * \author behley 40 | **/ 41 | 42 | class Preprocessing { 43 | public: 44 | Preprocessing(const rv::ParameterList& params); 45 | 46 | void setParameters(const rv::ParameterList& params); 47 | 48 | /** \brief pre-process the point cloud (vertex map, normal map, ... etc.) and store results in frame. **/ 49 | void process(glow::GlBuffer& points, Frame& frame); 50 | 51 | protected: 52 | uint32_t width_, height_; 53 | glow::GlProgram depth_program_, normal_program_, bilateral_program_, avg_program_; 54 | glow::GlVertexArray vao_points_; // laser points 55 | glow::GlVertexArray vao_no_points_; // no points. 56 | glow::GlVertexArray vao_img_verts_; // for each image coordinate, we have an point. 57 | glow::GlSampler sampler_; 58 | glow::GlFramebuffer framebuffer_; 59 | glow::GlTextureRectangle temp_vertices_; 60 | 61 | bool filterVertexmap_{false}; 62 | bool useFilteredVertexmap_{true}; 63 | bool avgVertexmap_{false}; 64 | }; 65 | 66 | #endif /* SRC_CORE_PREPROCESSING_H_ */ 67 | -------------------------------------------------------------------------------- /src/core/Surfel.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_SURFEL_H_ 2 | #define INCLUDE_CORE_SURFEL_H_ 3 | 4 | /** \brief surfel representation. **/ 5 | struct Surfel { 6 | public: 7 | float x, y, z; 8 | float radius; 9 | float nx, ny, nz; 10 | float confidence; 11 | 12 | int32_t timestamp; 13 | float color, weight, count; 14 | }; 15 | 16 | #endif /* INCLUDE_CORE_SURFEL_H_ */ 17 | -------------------------------------------------------------------------------- /src/core/lie_algebra.cpp: -------------------------------------------------------------------------------- 1 | #include "lie_algebra.h" 2 | #include 3 | 4 | Eigen::Matrix4d SE3::exp(const Eigen::VectorXd& x) { 5 | // std::cout << "exponential of " << x.transpose() << std::endl; 6 | Eigen::Matrix4d result = Eigen::Matrix4d::Identity(); 7 | 8 | // see phd thesis of Hauke Strasdat, 2012 pp 47-53: 9 | Eigen::Vector3d v(x[0], x[1], x[2]); 10 | Eigen::Vector3d omega(x[3], x[4], x[5]); 11 | 12 | double theta = omega.norm(); 13 | if (theta > 1e-10) { 14 | Eigen::Matrix3d omega_skew; 15 | omega_skew << 0, -omega[2], omega[1], omega[2], 0, -omega[0], -omega[1], omega[0], 0; 16 | Eigen::Matrix3d omega_skew_sqr = omega_skew * omega_skew; 17 | 18 | // Rotation: 19 | double alpha = sin(theta) / theta; 20 | double beta = (1 - cos(theta)) / (theta * theta); 21 | result.topLeftCorner(3, 3) = Eigen::Matrix3d::Identity() + alpha * omega_skew + beta * omega_skew_sqr; 22 | 23 | // Translation: 24 | double gamma = (1.0f - cos(theta)) / (theta * theta); 25 | double delta = (theta - sin(theta)) / (theta * theta * theta); 26 | 27 | Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + gamma * omega_skew + delta * omega_skew_sqr; 28 | result.block<3, 1>(0, 3) = V * v; 29 | } else { 30 | result.block<3, 1>(0, 3) = v; // just translation. 31 | } 32 | 33 | return result; 34 | } 35 | 36 | Eigen::VectorXd SE3::log(const Eigen::Matrix4d& M) { 37 | // see PhD thesis of Hauke Strasdat, 2012 pp. 47-53 and Sophus library: 38 | 39 | Eigen::VectorXd x = Eigen::VectorXd::Zero(6); 40 | 41 | Eigen::Matrix3d R = M.topLeftCorner(3, 3); 42 | Eigen::Vector3d t = M.topRightCorner(3, 1); 43 | 44 | double d = 0.5 * (R.trace() - 1.0); 45 | Eigen::Matrix3d omega_skew = Eigen::Matrix3d::Zero(); 46 | 47 | if (d < 1 - 1e-10) { 48 | double theta = std::acos(d); 49 | omega_skew = theta / (2 * std::sin(theta)) * (R - R.transpose()); 50 | 51 | x[3] = omega_skew(2, 1); 52 | x[4] = omega_skew(0, 2); 53 | x[5] = omega_skew(1, 0); 54 | } 55 | 56 | double theta = x.tail(3).norm(); 57 | 58 | x.head(3) = t; 59 | 60 | if (std::abs(theta) > 1e-10) { 61 | double half_theta = 0.5 * theta; 62 | 63 | double alpha = -0.5; 64 | double beta = 1 / (theta * theta) * (1 - theta * std::cos(half_theta) / (2 * std::sin(half_theta))); 65 | Eigen::Matrix3d V_inv = Eigen::Matrix3d::Identity() + alpha * omega_skew + beta * (omega_skew * omega_skew); 66 | 67 | x.head(3) = V_inv * t; 68 | } 69 | 70 | return x; 71 | } 72 | 73 | Eigen::Matrix4d SO3::exp(const Eigen::VectorXd& x) { 74 | Eigen::Matrix4d result = Eigen::Matrix4d::Identity(); 75 | 76 | Eigen::Vector3d omega(x[0], x[1], x[2]); 77 | 78 | double theta = omega.norm(); 79 | if (theta > 1e-10) { 80 | Eigen::Matrix3d omega_skew; 81 | omega_skew << 0, -omega[2], omega[1], omega[2], 0, -omega[0], -omega[1], omega[0], 0; 82 | Eigen::Matrix3d omega_skew_sqr = omega_skew * omega_skew; 83 | result.topLeftCorner(3, 3) = Eigen::Matrix3d::Identity() + sin(theta) / theta * omega_skew + 84 | (1 - cos(theta)) / (theta * theta) * omega_skew_sqr; 85 | } 86 | 87 | return result; 88 | } 89 | -------------------------------------------------------------------------------- /src/core/lie_algebra.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_LIE_ALGEBRA_H_ 2 | #define INCLUDE_CORE_LIE_ALGEBRA_H_ 3 | 4 | #include 5 | 6 | /** \brief some utility functions for Lie groups. 7 | * 8 | * \author behley 9 | **/ 10 | 11 | class SE3 { 12 | public: 13 | SE3() = delete; 14 | 15 | /** \brief get rotation matrix from angle-axis + translation **/ 16 | static Eigen::Matrix4d exp(const Eigen::VectorXd& x); 17 | 18 | /** \brief get angle-axis + translation from rotation matrix **/ 19 | static Eigen::VectorXd log(const Eigen::Matrix4d& x); 20 | }; 21 | 22 | class SO3 { 23 | public: 24 | SO3() = delete; 25 | static Eigen::Matrix4d exp(const Eigen::VectorXd& x); 26 | }; 27 | 28 | #endif /* INCLUDE_CORE_LIE_ALGEBRA_H_ */ 29 | -------------------------------------------------------------------------------- /src/io/KITTIReader.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef KITTILASERSCANREADER_H_ 3 | #define KITTILASERSCANREADER_H_ 4 | 5 | #include "LaserscanReader.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace rv { 12 | 13 | /** \brief a reader for the KITTI datasets provided by the KIT. 14 | * 15 | * \author behley 16 | */ 17 | class KITTIReader : public LaserscanReader { 18 | public: 19 | KITTIReader(const std::string& scan_filename, uint32_t buffer_size = 50); 20 | 21 | void reset() override; 22 | bool read(Laserscan& scan) override; 23 | void seek(uint32_t scan) override; 24 | bool isSeekable() const override; 25 | uint32_t count() const override; 26 | 27 | protected: 28 | void initScanFilenames(const std::string& scan_filename); 29 | 30 | bool read(uint32_t scan_idx, Laserscan& scan); 31 | 32 | int32_t currentScan; 33 | std::vector scan_filenames; 34 | RingBuffer bufferedScans; 35 | uint32_t firstBufferedScan; 36 | 37 | }; 38 | } 39 | #endif /* KITTILASERSCANREADER_H_ */ 40 | -------------------------------------------------------------------------------- /src/io/LaserscanReader.h: -------------------------------------------------------------------------------- 1 | #ifndef LASERSCANREADER_H_ 2 | #define LASERSCANREADER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace rv { 10 | 11 | /** \brief laserscan reader interface 12 | * 13 | * Defines the interface for a (seekable) laserscan reader. 14 | * 15 | * A seekable laser scan should reimplement the seek method. 16 | * 17 | * \author behley 18 | **/ 19 | class LaserscanReader { 20 | public: 21 | virtual ~LaserscanReader() {} 22 | 23 | /** \brief reset laser scan reader. **/ 24 | virtual void reset() = 0; 25 | 26 | /** \brief read next laser range scan **/ 27 | virtual bool read(Laserscan& scan) = 0; 28 | 29 | /** \brief is reader seekable? **/ 30 | virtual bool isSeekable() const { return false; } 31 | 32 | /** \brief seek to a specific scan number. **/ 33 | virtual void seek(uint32_t scan) { throw IOError("Laserscan reader not seekable."); } 34 | 35 | /** \brief number of scans in the logfile **/ 36 | virtual uint32_t count() const = 0; 37 | }; 38 | } 39 | #endif /* LASERSCANREADER_H_ */ 40 | -------------------------------------------------------------------------------- /src/io/RobocarReader.cpp: -------------------------------------------------------------------------------- 1 | #include "RobocarReader.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace rv 7 | { 8 | 9 | RobocarReader::RobocarReader(const std::string& filename) { 10 | std::vector files = FileUtil::getDirectoryListing(FileUtil::dirName(filename)); 11 | 12 | for (uint32_t i = 0; i < files.size(); ++i) { 13 | if (FileUtil::extension(files[i]) == ".bin") { 14 | filenames_.push_back(files[i]); 15 | } 16 | } 17 | std::cout << "Found " << filenames_.size() << " scans." << std::endl; 18 | std::sort(filenames_.begin(), filenames_.end()); 19 | } 20 | 21 | void RobocarReader::reset() { 22 | currentIdx_ = 0; 23 | } 24 | 25 | bool RobocarReader::read(Laserscan& scan) { 26 | if (currentIdx_ >= filenames_.size()) return false; 27 | 28 | std::ifstream in(filenames_[currentIdx_].c_str(), std::ios::binary); 29 | if (!in.is_open()) return false; 30 | 31 | scan.clear(); 32 | 33 | in.seekg(0, std::ios::end); 34 | uint32_t num_points = in.tellg() / (3 * sizeof(double)); 35 | in.seekg(0, std::ios::beg); 36 | 37 | std::vector values(3 * num_points); 38 | in.read((char*)&values[0], 3 * num_points * sizeof(double)); 39 | 40 | in.close(); 41 | std::vector& points = scan.points(); 42 | 43 | points.resize(num_points); 44 | 45 | for (uint32_t i = 0; i < num_points; ++i) { 46 | points[i].x() = values[3 * i]; 47 | points[i].y() = -values[3 * i + 1]; 48 | points[i].z() = -values[3 * i + 2]; 49 | } 50 | 51 | currentIdx_ += 1; 52 | 53 | return true; 54 | } 55 | 56 | bool RobocarReader::isSeekable() const { 57 | return true; 58 | } 59 | 60 | void RobocarReader::seek(uint32_t scan) { 61 | currentIdx_ = scan; 62 | } 63 | 64 | uint32_t RobocarReader::count() const { 65 | return filenames_.size(); 66 | } 67 | 68 | } 69 | -------------------------------------------------------------------------------- /src/io/RobocarReader.h: -------------------------------------------------------------------------------- 1 | #ifndef SRC_IO_ROBOCARREADER_H_ 2 | #define SRC_IO_ROBOCARREADER_H_ 3 | 4 | #include "LaserscanReader.h" 5 | 6 | namespace rv { 7 | 8 | /** \brief Laserscan reader for RoboCar dataset. 9 | * 10 | * \author behley 11 | **/ 12 | class RobocarReader : public LaserscanReader { 13 | public: 14 | RobocarReader(const std::string& filename); 15 | 16 | void reset(); 17 | 18 | bool read(rv::Laserscan& scan); 19 | 20 | bool isSeekable() const; 21 | 22 | void seek(uint32_t scan); 23 | 24 | uint32_t count() const; 25 | 26 | protected: 27 | uint32_t currentIdx_{0}; 28 | std::vector filenames_; 29 | }; 30 | } 31 | 32 | #endif /* SRC_IO_ROBOCARREADER_H_ */ 33 | -------------------------------------------------------------------------------- /src/io/SimulationReader.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_SIMULATIONREADER_H_ 2 | #define INCLUDE_CORE_SIMULATIONREADER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /** \brief simple generator of simulated laser scan data. **/ 9 | class SimulationReader : public rv::LaserscanReader { 10 | public: 11 | SimulationReader(const std::string& filename); 12 | 13 | void reset(); 14 | 15 | bool read(rv::Laserscan& scan); 16 | 17 | bool isSeekable() const; 18 | 19 | void seek(uint32_t scan); 20 | 21 | uint32_t count() const; 22 | 23 | const std::vector& getTrajectory() const; 24 | 25 | void setParameters(const rv::ParameterList& params); 26 | 27 | protected: 28 | class Ray { 29 | public: 30 | Ray(const Eigen::Vector4f& origin, const Eigen::Vector4f& dir); 31 | Eigen::Vector4f operator()(float t) const; 32 | 33 | Eigen::Vector4f o; 34 | Eigen::Vector4f d; 35 | }; 36 | 37 | class Triangle { 38 | public: 39 | Triangle(const Eigen::Vector4f& p1, const Eigen::Vector4f& p2, const Eigen::Vector4f& p3); 40 | 41 | Eigen::Vector4f vertices[3]; // needed to determine if inside plane. 42 | }; 43 | 44 | /** \brief intersect ray with triangle. if true, than t is the parameter needed to calculate the intersection from 45 | * ray. **/ 46 | bool intersect(const Ray& ray, const Triangle& triangle, float& t); 47 | 48 | void addCube(const Eigen::Matrix4f& pose, float size); 49 | void addPlane(const Eigen::Matrix4f& pose, float size); 50 | 51 | uint32_t timestamp_{0}; 52 | std::vector trajectory_; // sensor locations at time t. 53 | std::vector world_; // world is described by triangles. 54 | std::vector buffer_; 55 | 56 | std::vector beams_; 57 | std::vector calibration_; 58 | Eigen::Matrix4f extrinsicPose_; 59 | 60 | rv::Random rng_; 61 | float sigma_noise_{0.0f}; 62 | float sigma_speed_{0.0f}; 63 | }; 64 | 65 | #endif /* INCLUDE_CORE_SIMULATIONREADER_H_ */ 66 | -------------------------------------------------------------------------------- /src/opengl/MatrixStack.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MatrixStack.h 3 | * 4 | * Created on: Jun 28, 2016 5 | * Author: behley 6 | */ 7 | 8 | #ifndef INCLUDE_CORE_MATRIXSTACK_H_ 9 | #define INCLUDE_CORE_MATRIXSTACK_H_ 10 | 11 | #include 12 | 13 | /** \brief All matrices needed for correct rendering and shading. 14 | * 15 | * For correct (phong) shading, we have to determine the angles between model 16 | * normals and the light directions. Therefore, we have to first transform all 17 | * model vertices in world coordinates and also transform the normals. Thus, it 18 | * is handy to not only have the model-view-projection matrix, but also the model 19 | * matrix and the normal matrix to apply local transformations. 20 | * 21 | * \author behley 22 | */ 23 | struct MatrixStack { 24 | public: 25 | Eigen::Matrix4f mvp; // model-view-projection matrix. 26 | Eigen::Matrix4f model; // model matrix for transformation in world coordinates 27 | Eigen::Matrix4f normal; // normal matrix for transformation in world coordinates. 28 | }; 29 | 30 | #endif /* INCLUDE_CORE_MATRIXSTACK_H_ */ 31 | -------------------------------------------------------------------------------- /src/opengl/Mesh.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_MESH_H_ 2 | #define INCLUDE_CORE_MESH_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | class Model; 11 | 12 | /** \brief Representation of a mesh with possibly multiple materials. 13 | * 14 | * A mesh contains all vertex buffers for drawing. 15 | * 16 | * \author behley 17 | */ 18 | class Mesh { 19 | friend class Model; 20 | 21 | public: 22 | struct Material { 23 | public: 24 | Material() : ambient(0.1, 0.1, 0.1), diffuse(0.0, 0.0, 0.9), specular(0, 0, 0), emission(0, 0, 0) {} 25 | glow::vec3 ambient; 26 | glow::vec3 diffuse; 27 | glow::vec3 specular; 28 | glow::vec3 emission; 29 | 30 | float shininess{1.0f}; // specular exponent 31 | float alpha{1.0f}; 32 | }; 33 | 34 | struct Vertex { 35 | public: 36 | glow::vec4 position; 37 | glow::vec4 normal; 38 | glow::vec2 texture; 39 | }; 40 | 41 | struct Triangle { 42 | public: 43 | uint32_t vertices[3]; 44 | uint32_t material{0}; 45 | }; 46 | 47 | Mesh(); 48 | Mesh(const std::vector& vertices, const std::vector& faces); 49 | Mesh(const std::vector& vertices, const std::vector& faces, const std::vector& materials); 50 | 51 | void draw(glow::GlProgram& program) const; 52 | 53 | const Material& material(uint32_t idx) const { 54 | if (idx >= materials_.size()) throw std::runtime_error("non-existent material."); 55 | return materials_[idx].value(); 56 | } 57 | 58 | protected: 59 | void initialize(const std::vector& vertices, const std::vector& faces, 60 | const std::vector& materials); 61 | 62 | float length(const glow::vec4& v); 63 | 64 | glow::GlBuffer vertices_{glow::BufferTarget::ARRAY_BUFFER, glow::BufferUsage::STATIC_DRAW}; 65 | std::vector vaos_; 66 | std::vector > triangles_; // per material triangles. 67 | std::vector > materials_; 68 | 69 | // std::vector vertices_; 70 | // std::vector > ; 71 | }; 72 | 73 | #endif /* INCLUDE_CORE_MESH_H_ */ 74 | -------------------------------------------------------------------------------- /src/opengl/Model.cpp: -------------------------------------------------------------------------------- 1 | #include "Model.h" 2 | 3 | using namespace glow; 4 | 5 | void Model::attach(const Mesh& mesh) { 6 | model_matrices_.push_back(Eigen::Matrix4f::Identity()); 7 | normal_matrices_.push_back(Eigen::Matrix4f::Identity()); 8 | meshes_.push_back(mesh); 9 | } 10 | 11 | void Model::attach(const Eigen::Matrix4f& transform, const Mesh& mesh) { 12 | model_matrices_.push_back(transform); 13 | normal_matrices_.push_back(transform.inverse().transpose()); 14 | meshes_.push_back(mesh); 15 | } 16 | 17 | void Model::draw(glow::GlProgram& program, const MatrixStack& matrices) const { 18 | GlUniform mvp("mvp", matrices.mvp); 19 | GlUniform model_mat("model_mat", matrices.model); 20 | GlUniform normal_mat("normal_mat", matrices.normal); 21 | 22 | // glEnable(GL_BLEND); 23 | // glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 24 | for (uint32_t i = 0; i < meshes_.size(); ++i) { 25 | // apply local transformations to matrix stack. 26 | mvp = matrices.mvp * model_matrices_[i]; // mvp = p * v * m * l 27 | model_mat = matrices.model * model_matrices_[i]; 28 | normal_mat = matrices.normal * normal_matrices_[i]; 29 | 30 | program.setUniform(mvp); 31 | program.setUniform(model_mat); 32 | program.setUniform(normal_mat); 33 | 34 | meshes_[i].draw(program); 35 | } 36 | 37 | // glDisable(GL_BLEND); 38 | } 39 | -------------------------------------------------------------------------------- /src/opengl/Model.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_MODEL_H_ 2 | #define INCLUDE_CORE_MODEL_H_ 3 | 4 | #include "Mesh.h" 5 | #include "MatrixStack.h" 6 | 7 | /** \brief a model represents a collection of meshes. 8 | * 9 | * \author behley 10 | */ 11 | class Model { 12 | public: 13 | /** \brief attach a mesh to the model. **/ 14 | void attach(const Mesh& mesh); 15 | 16 | /** \brief attach a mesh to the model with given local transformation. **/ 17 | void attach(const Eigen::Matrix4f& transform, const Mesh& mesh); 18 | 19 | /** \brief draw meshes with given program... **/ 20 | void draw(glow::GlProgram& program, const MatrixStack& matrices) const; 21 | 22 | protected: 23 | std::vector model_matrices_; 24 | std::vector normal_matrices_; 25 | 26 | std::vector meshes_; 27 | }; 28 | 29 | #endif /* INCLUDE_CORE_MODEL_H_ */ 30 | -------------------------------------------------------------------------------- /src/opengl/ObjReader.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_OBJREADER_H_ 2 | #define INCLUDE_CORE_OBJREADER_H_ 3 | 4 | #include "Mesh.h" 5 | 6 | /** \brief reading and parsing of Wavefront .obj files. 7 | * 8 | * \author behley 9 | */ 10 | class ObjReader { 11 | public: 12 | /** \brief read mesh from file with given filename. **/ 13 | static Mesh fromFile(const std::string& filename); 14 | 15 | protected: 16 | static void parseMaterials(const std::string& filename, std::map& materials); 17 | }; 18 | 19 | #endif /* INCLUDE_CORE_OBJREADER_H_ */ 20 | -------------------------------------------------------------------------------- /src/rv/CompositeParameter.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/CompositeParameter.h" 2 | #include 3 | 4 | namespace rv 5 | { 6 | 7 | CompositeParameter::CompositeParameter(const std::string& name) : 8 | Parameter(name) 9 | { 10 | 11 | } 12 | 13 | CompositeParameter::CompositeParameter(const std::string& name, const ParameterList& values) : 14 | Parameter(name), param_list(values) 15 | { 16 | 17 | } 18 | 19 | CompositeParameter::CompositeParameter(const std::string& name, const XmlNode& node) : 20 | Parameter(name), val(node) 21 | { 22 | const XmlNode::List& children = node.getChildren(); 23 | 24 | for (XmlNode::List::const_iterator it = children.begin(); it != children.end(); ++it) 25 | { 26 | if (it->getName() == "param") 27 | { 28 | Parameter* p = parseParameter(*it); 29 | if (param_list.hasParam(p->name())) std::cout << "WARNING: (" << name << ") parameter with name = '" << p->name() 30 | << "' already inserted." << std::endl; 31 | param_list.insert(*p); 32 | delete p; 33 | } 34 | } 35 | } 36 | 37 | CompositeParameter::~CompositeParameter() 38 | { 39 | 40 | } 41 | 42 | CompositeParameter::CompositeParameter(const CompositeParameter& other) : 43 | Parameter(other), param_list(other.param_list), val(other.val) 44 | { 45 | 46 | } 47 | 48 | CompositeParameter& CompositeParameter::operator=(const CompositeParameter& other) 49 | { 50 | if (&other == this) return *this; 51 | 52 | Parameter::operator=(other); 53 | 54 | param_list = other.param_list; 55 | 56 | return *this; 57 | } 58 | 59 | CompositeParameter* CompositeParameter::clone() const 60 | { 61 | return new CompositeParameter(*this); 62 | } 63 | 64 | std::string CompositeParameter::toString() const 65 | { 66 | std::stringstream out; 67 | 68 | out << "" << std::endl; 69 | for (ParameterList::const_iterator it = param_list.begin(); it != param_list.end(); ++it) 70 | out << " " << *it << std::endl; 71 | out << ""; 72 | 73 | return out.str(); 74 | } 75 | 76 | CompositeParameter::operator ParameterList() const 77 | { 78 | return param_list; 79 | } 80 | 81 | CompositeParameter* parseCompositeParameter(const XmlNode& node) 82 | { 83 | return new CompositeParameter(node.getAttribute("name"), node); 84 | } 85 | 86 | } 87 | -------------------------------------------------------------------------------- /src/rv/CompositeParameter.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************************\ 2 | \brief Parameter as a composition of other Parameters. 3 | 4 | \headerfile CompositeParameter.h 5 | \author behley 6 | 7 | \*****************************************************************************/ 8 | 9 | #ifndef COMPOSITEPARAMETER_H_ 10 | #define COMPOSITEPARAMETER_H_ 11 | 12 | #include "Parameter.h" 13 | #include "ParameterList.h" 14 | 15 | namespace rv 16 | { 17 | 18 | class CompositeParameter: public Parameter 19 | { 20 | public: 21 | CompositeParameter(const std::string& name); 22 | CompositeParameter(const std::string& name, const XmlNode& value); 23 | CompositeParameter(const std::string& name, const ParameterList& values); 24 | 25 | CompositeParameter(const CompositeParameter& other); 26 | virtual CompositeParameter& operator=(const CompositeParameter& other); 27 | 28 | virtual ~CompositeParameter(); 29 | 30 | virtual CompositeParameter* clone() const; 31 | 32 | virtual operator ParameterList() const; 33 | 34 | const ParameterList& getParams() const 35 | { 36 | return param_list; 37 | } 38 | 39 | ParameterList& getParams() 40 | { 41 | return param_list; 42 | } 43 | 44 | virtual std::string toString() const; 45 | 46 | virtual inline XmlNode& value() 47 | { 48 | return val; 49 | } 50 | 51 | std::string type() const 52 | { 53 | return "composite"; 54 | } 55 | 56 | virtual bool operator==(const Parameter& other) const 57 | { 58 | const CompositeParameter* param = dynamic_cast(&other); 59 | if(param == 0) return false; 60 | 61 | return (other.name() == mName && param_list == param->param_list); 62 | } 63 | 64 | protected: 65 | ParameterList param_list; 66 | XmlNode val; 67 | }; 68 | 69 | CompositeParameter* parseCompositeParameter(const XmlNode& node); 70 | 71 | } 72 | #endif /* COMPOSITEPARAMETER_H_ */ 73 | -------------------------------------------------------------------------------- /src/rv/Exception.h: -------------------------------------------------------------------------------- 1 | #ifndef RV_EXCEPTION_H_ 2 | #define RV_EXCEPTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | /** \brief Base class for exceptions 8 | * \author behley 9 | */ 10 | namespace rv 11 | { 12 | 13 | class Exception: public std::exception 14 | { 15 | public: 16 | Exception(const std::string& msg) : 17 | what_(msg) 18 | { 19 | } 20 | 21 | virtual ~Exception() throw () 22 | { 23 | } 24 | 25 | /** \brief Returns associated message. 26 | * 27 | * This method is called by the C++ runtime system even if 28 | * the exception aborts the program. 29 | */ 30 | virtual const char* what() const throw () 31 | { 32 | return what_.c_str(); 33 | } 34 | 35 | private: 36 | std::string what_; 37 | }; 38 | 39 | } 40 | 41 | #endif /* SRC_CORE_RV_EXCEPTION_H_ */ 42 | -------------------------------------------------------------------------------- /src/rv/FileUtil.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/FileUtil.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace rv 7 | { 8 | 9 | FileUtil::FileUtil() 10 | { 11 | 12 | } 13 | 14 | std::string FileUtil::dirName(const std::string& path) 15 | { 16 | boost::filesystem::path p(path); 17 | 18 | return p.parent_path().string(); 19 | } 20 | 21 | std::string FileUtil::baseName(const std::string& path) 22 | { 23 | boost::filesystem::path p(path); 24 | 25 | return p.filename().string(); 26 | } 27 | 28 | std::string FileUtil::extension(const std::string& path, int32_t level) 29 | { 30 | std::string filename = boost::filesystem::path(path).filename().string(); 31 | if (filename == "" || filename == "." || filename == "..") return ""; 32 | 33 | std::string ext; 34 | while (level-- > 0) 35 | { 36 | std::string::size_type idx = filename.rfind("."); 37 | if (idx == std::string::npos) break; 38 | ext.insert(0, filename.substr(idx)); 39 | filename.resize(idx); 40 | } 41 | 42 | return ext; 43 | } 44 | 45 | std::string FileUtil::stripExtension(const std::string& path, int32_t level) 46 | { 47 | std::string filename = boost::filesystem::path(path).filename().string(); 48 | if (filename == "" || filename == "." || filename == "..") return path; 49 | 50 | std::string::size_type path_index = dirName(path).length() + 1; 51 | std::string complete_path = path; 52 | 53 | while (level-- > 0) 54 | { 55 | std::string::size_type idx = complete_path.rfind("."); 56 | if (idx == std::string::npos || idx < path_index) break; 57 | complete_path.resize(idx); 58 | } 59 | 60 | return complete_path; 61 | } 62 | 63 | bool FileUtil::exists(const std::string& filename) 64 | { 65 | return boost::filesystem::exists(filename); 66 | } 67 | 68 | std::vector FileUtil::getDirectoryListing(const std::string& directory) 69 | { 70 | typedef boost::filesystem::directory_iterator diter; 71 | 72 | std::vector listing; 73 | for (diter it = diter(directory); it != diter(); ++it) 74 | listing.push_back(it->path().string()); 75 | 76 | return listing; 77 | } 78 | 79 | } /* namespace rv */ 80 | -------------------------------------------------------------------------------- /src/rv/FileUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_FILEUTIL_H_ 2 | #define CORE_FILEUTIL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /** \brief Decorator for boost::path. 9 | * 10 | * 11 | * \author behley 12 | */ 13 | 14 | namespace rv 15 | { 16 | 17 | class FileUtil 18 | { 19 | public: 20 | 21 | /** \brief Returns the directory components of a file path. */ 22 | static std::string dirName(const std::string& path); 23 | 24 | /** \brief Strips the directory components from a file path. */ 25 | static std::string baseName(const std::string& path); 26 | 27 | /** \brief Returns the file extension. 28 | * \param[in] path the file name 29 | * \param[in] level maximum number of extension levels. For instance, ".tar.gz" has level 2. */ 30 | static std::string extension(const std::string& path, int32_t level = 1); 31 | 32 | /** \brief Removes the file extension. 33 | * \param[in] path the file name 34 | * \param[in] level maximum number of extension levels to be stripped. For instance, ".tar.gz" has level 2. */ 35 | static std::string stripExtension(const std::string& path, int32_t level = 1); 36 | 37 | /** \brief Checks if a file exists. */ 38 | static bool exists(const std::string& filename); 39 | 40 | /** \brief Get directory listing. **/ 41 | static std::vector getDirectoryListing(const std::string& directory); 42 | 43 | protected: 44 | FileUtil(); 45 | }; 46 | 47 | } /* namespace rv */ 48 | #endif /* CORE_FILEUTIL_H_ */ 49 | -------------------------------------------------------------------------------- /src/rv/IOError.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_IOERROR_H_ 2 | #define CORE_IOERROR_H_ 3 | 4 | #include 5 | 6 | /** \brief Exception associated with I/O operations 7 | * 8 | * \author behley 9 | */ 10 | 11 | namespace rv 12 | { 13 | 14 | class IOError: public std::runtime_error 15 | { 16 | public: 17 | IOError(const std::string& reason) 18 | : std::runtime_error(reason) 19 | { 20 | } 21 | }; 22 | 23 | } // namespace rv 24 | 25 | #endif /* SRC_CORE_RV_IOERROR_H_ */ 26 | -------------------------------------------------------------------------------- /src/rv/Laserscan.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/Laserscan.h" 2 | 3 | namespace rv 4 | { 5 | 6 | Laserscan::Laserscan() 7 | { 8 | 9 | } 10 | 11 | Laserscan::Laserscan(const Transform& pose, const std::list& points, const std::list& remissions) : 12 | mPose(pose) 13 | { 14 | points_.assign(points.begin(), points.end()); 15 | remissions_.assign(remissions.begin(), remissions.end()); 16 | } 17 | 18 | /** \brief clear **/ 19 | void Laserscan::clear() 20 | { 21 | points_.clear(); 22 | remissions_.clear(); 23 | normals_.clear(); 24 | } 25 | 26 | /** \brief getter for points/normals/remission **/ 27 | const Point3f& Laserscan::point(uint32_t i) const 28 | { 29 | assert(i < points_.size()); 30 | return points_[i]; 31 | } 32 | 33 | float Laserscan::remission(uint32_t i) const 34 | { 35 | assert(i < remissions_.size()); 36 | return remissions_[i]; 37 | } 38 | 39 | const Normal3f& Laserscan::normal(uint32_t i) const 40 | { 41 | assert(i < normals_.size()); 42 | return normals_[i]; 43 | } 44 | 45 | Transform& Laserscan::pose() 46 | { 47 | return mPose; 48 | } 49 | 50 | const Transform& Laserscan::pose() const 51 | { 52 | return mPose; 53 | } 54 | 55 | uint32_t Laserscan::size() const 56 | { 57 | return points_.size(); 58 | } 59 | 60 | std::vector& Laserscan::points() 61 | { 62 | return points_; 63 | } 64 | 65 | const std::vector& Laserscan::points() const 66 | { 67 | return points_; 68 | } 69 | 70 | std::vector& Laserscan::remissions() 71 | { 72 | return remissions_; 73 | } 74 | 75 | const std::vector& Laserscan::remissions() const 76 | { 77 | return remissions_; 78 | } 79 | 80 | const std::vector& Laserscan::normals() const 81 | { 82 | return normals_; 83 | } 84 | 85 | std::vector& Laserscan::normals() 86 | { 87 | return normals_; 88 | } 89 | 90 | bool Laserscan::hasRemission() const 91 | { 92 | return (points_.size() == remissions_.size()); 93 | } 94 | 95 | bool Laserscan::hasNormals() const 96 | { 97 | return (points_.size() == remissions_.size()); 98 | } 99 | 100 | } 101 | -------------------------------------------------------------------------------- /src/rv/Laserscan.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \brief representation of a laser range scan with points + remission and optional normals. 3 | * 4 | * The remission is assumed to give values in [0.0,1.0] 5 | * 6 | * \author behley 7 | */ 8 | 9 | #ifndef LASERSCAN_H_ 10 | #define LASERSCAN_H_ 11 | 12 | #include "geometry.h" 13 | #include "transform.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace rv 19 | { 20 | 21 | class Laserscan 22 | { 23 | public: 24 | friend class LaserscanReader; 25 | friend class KITTIReader; 26 | friend class PCAPReader; 27 | friend class EZDReader; 28 | friend class BLFReader; 29 | friend class FreiburgReader; 30 | 31 | Laserscan(); 32 | Laserscan(const Transform& pose, const std::list& points, 33 | const std::list& remission); 34 | 35 | /** \brief clear **/ 36 | void clear(); 37 | 38 | /** \brief getter for points/normals/remission **/ 39 | const Point3f& point(uint32_t i) const; 40 | float remission(uint32_t i) const; 41 | const Normal3f& normal(uint32_t) const; 42 | 43 | /** \brief global pose of the sensor. **/ 44 | Transform& pose(); 45 | const Transform& pose() const; 46 | /** \brief number of points. **/ 47 | uint32_t size() const; 48 | 49 | /** access to the raw data. **/ 50 | std::vector& points(); 51 | const std::vector& points() const; 52 | std::vector& remissions(); 53 | const std::vector& remissions() const; 54 | std::vector& normals(); 55 | const std::vector& normals() const; 56 | 57 | bool hasRemission() const; 58 | bool hasNormals() const; 59 | 60 | protected: 61 | Transform mPose; 62 | /** representing points, normals, and remission this way, we can separately initialize points and normals **/ 63 | std::vector points_; 64 | std::vector remissions_; 65 | std::vector normals_; 66 | }; 67 | 68 | } 69 | 70 | #endif /* LASERSCAN_H_ */ 71 | -------------------------------------------------------------------------------- /src/rv/LaserscanReader.h: -------------------------------------------------------------------------------- 1 | #ifndef LASERSCANREADER_H_ 2 | #define LASERSCANREADER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace rv { 10 | 11 | /** \brief laserscan reader interface 12 | * 13 | * Defines the interface for a (seekable) laserscan reader. 14 | * 15 | * A seekable laser scan should reimplement the seek method. 16 | * 17 | * \author behley 18 | **/ 19 | class LaserscanReader { 20 | public: 21 | virtual ~LaserscanReader() {} 22 | 23 | /** \brief reset laser scan reader. **/ 24 | virtual void reset() = 0; 25 | 26 | /** \brief read next laser range scan **/ 27 | virtual bool read(Laserscan& scan) = 0; 28 | 29 | /** \brief is reader seekable? **/ 30 | virtual bool isSeekable() const { return false; } 31 | 32 | /** \brief seek to a specific scan number. **/ 33 | virtual void seek(uint32_t scan) { throw IOError("Laserscan reader not seekable."); } 34 | 35 | /** \brief number of scans in the logfile **/ 36 | virtual uint32_t count() const = 0; 37 | }; 38 | } 39 | #endif /* LASERSCANREADER_H_ */ 40 | -------------------------------------------------------------------------------- /src/rv/Parameter.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/Parameter.h" 2 | #include "rv/PrimitiveParameters.h" 3 | #include "rv/RangeParameter.h" 4 | #include "rv/CompositeParameter.h" 5 | 6 | #include "rv/ParameterList.h" 7 | 8 | namespace rv 9 | { 10 | 11 | Parameter::operator ParameterList() const 12 | { 13 | std::stringstream str; 14 | str << "conversion of Parameter with name '" << mName 15 | << "' to ParameterList not possible."; 16 | throw Exception(str.str()); 17 | } 18 | 19 | Parameter* Parameter::parseParameter(const XmlNode& node) 20 | { 21 | const std::string param_name = node.getAttribute("name"); 22 | const std::string param_type = node.getAttribute("type"); 23 | 24 | if (param_type == "integer") 25 | return parseIntegerParameter(node); 26 | else if (param_type == "float") 27 | return parseFloatParameter(node); 28 | else if (param_type == "string") 29 | return parseStringParameter(node); 30 | else if (param_type == "boolean") 31 | return parseBooleanParameter(node); 32 | // else if (param_type == "vector") 33 | // return parseVectorParameter(node); 34 | // else if (param_type == "matrix") 35 | // return parseMatrixParameter(node); 36 | else if (param_type == "range") 37 | return parseRangeParameter(node); 38 | else if (param_type == "composite") 39 | return parseCompositeParameter(node); 40 | else 41 | { 42 | std::stringstream reason; 43 | reason << "Unknown parameter type in xml file at line " << node.getLineNo() 44 | << std::endl; 45 | throw XmlError(reason.str()); 46 | } 47 | 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /src/rv/ParameterListIterator.h: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETERLISTITERATOR_H_ 2 | #define PARAMETERLISTITERATOR_H_ 3 | 4 | #include "ParameterList.h" 5 | #include "RangeParameter.h" 6 | #include 7 | 8 | namespace rv 9 | { 10 | /** 11 | * \brief iterates over a given ParameterList and replacing all RangeParameters with appropriate values. 12 | * 13 | * Generate instantiated ParameterList where all RangeParameters are replaced by appropriate values. 14 | * If no values are RangeParameters then only the ParameterList is returned, when next is called. 15 | * 16 | * \author: behley 17 | */ 18 | class ParameterListIterator 19 | { 20 | public: 21 | ParameterListIterator(const ParameterList& params); 22 | 23 | /** \brief iterate only partially over some RangeParameters 24 | * 25 | * Only some of the RangeParameters are replaced, the rest are left in their original state. Thus, 26 | * it is possible to iterate in a nested fashion over the Parameters. 27 | */ 28 | ParameterListIterator(const ParameterList& params, 29 | const std::vector& names); 30 | 31 | /** \brief new ParameterList with instantiated range params available? **/ 32 | bool hasNext(); 33 | /** \brief next ParameterList with instantiated Parameters, and advance to next params **/ 34 | const ParameterList& next(); 35 | /** \brief returns the RangeParameter, which are replaced. **/ 36 | const std::vector& getRangeParams() const; 37 | 38 | protected: 39 | /** todo: implement copy constructors. **/ 40 | ParameterListIterator(const ParameterListIterator& other); 41 | ParameterListIterator& operator=(const ParameterListIterator& other); 42 | 43 | /** \brief extract range parameters from ParameterList, and set parents. **/ 44 | void parseParameterList(ParameterList& new_params, 45 | const ParameterList& ranged_params); 46 | 47 | bool mHasNext; 48 | /** idea for copy/assignment: use original parameter list, and iterator. **/ 49 | ParameterList mOriginalParams; 50 | std::vector mFilter; 51 | 52 | /** fully instantiated ParameterList **/ 53 | ParameterList mCurrentParams; 54 | /** original range parameters **/ 55 | std::vector mRangeParams; 56 | /** extracted range parameters **/ 57 | std::vector mRangeIters; 58 | /** pointers to ParameterLists where the i-th RangeParameter occurs. **/ 59 | std::vector mRangeParamParents; 60 | }; 61 | 62 | } 63 | #endif /* PARAMETERLISTITERATOR_H_ */ 64 | -------------------------------------------------------------------------------- /src/rv/Random.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/Random.h" 2 | 3 | namespace rv 4 | { 5 | 6 | Random::Random() : 7 | uniform_(0.0, 1.0), normal_(0.0, 1.0) 8 | { 9 | 10 | } 11 | 12 | Random::Random(uint32_t seed) : 13 | rng_(seed), uniform_(0.0, 1.0), normal_(0.0, 1.0) 14 | { 15 | 16 | } 17 | 18 | int32_t Random::getInt(int32_t n) 19 | { 20 | return static_cast(n * uniform_(rng_)); 21 | } 22 | 23 | int32_t Random::getInt(int32_t min, int32_t max) 24 | { 25 | return static_cast((max - min) * uniform_(rng_) + min); 26 | } 27 | 28 | int32_t Random::operator()(int32_t n) 29 | { 30 | return getInt(n); 31 | } 32 | 33 | float Random::getFloat() 34 | { 35 | return static_cast(uniform_(rng_)); 36 | } 37 | 38 | float Random::getGaussianFloat() 39 | { 40 | return static_cast(uniform_(rng_)); 41 | } 42 | 43 | double Random::getDouble() 44 | { 45 | return uniform_(rng_); 46 | } 47 | 48 | /** \brief Returns a random double from the Gaussian distribution with mean 0 and std. deviation 1 */ 49 | double Random::getGaussianDouble() 50 | { 51 | return normal_(rng_); 52 | } 53 | 54 | } 55 | -------------------------------------------------------------------------------- /src/rv/Random.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_RANDOM_H_ 2 | #define CORE_RANDOM_H_ 3 | 4 | #include 5 | 6 | namespace rv 7 | { 8 | 9 | /** \brief random number generator based on boost::random 10 | * 11 | * The class simply decorates the functionality in a more usable way. 12 | * 13 | * 14 | * \author snej 15 | */ 16 | 17 | class Random 18 | { 19 | public: 20 | Random(); 21 | explicit Random(uint32_t seed); 22 | 23 | /** \brief Returns a random integer value in the range [0,\a n - 1] inclusive. */ 24 | int32_t getInt(int32_t n); 25 | /** \brief Returns a random integer value in the range [\a min,\a max] inclusive. */ 26 | int32_t getInt(int32_t min, int32_t max); 27 | 28 | /** \brief Function operator for use with STL. 29 | * 30 | * Returns a random value between 0 and \a n - 1 inclusive. 31 | */ 32 | int32_t operator()(int32_t n); 33 | 34 | /** \brief Returns a random float in the range [0,1] inclusive. */ 35 | float getFloat(); 36 | /** \brief Returns a random float from the Gaussian distribution with mean 0 and std. deviation 1 */ 37 | float getGaussianFloat(); 38 | /** \brief Returns a random double in the range [0,1] inclusive. */ 39 | double getDouble(); 40 | /** \brief Returns a random double from the Gaussian distribution with mean 0 and std. deviation 1 */ 41 | double getGaussianDouble(); 42 | 43 | protected: 44 | std::default_random_engine rng_; 45 | std::uniform_real_distribution uniform_; 46 | std::normal_distribution normal_; 47 | }; 48 | 49 | } 50 | 51 | #endif /* CORE_RANDOM_H_ */ 52 | -------------------------------------------------------------------------------- /src/rv/RangeParameter.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/RangeParameter.h" 2 | #include 3 | #include 4 | 5 | namespace rv 6 | { 7 | 8 | RangeParameter::RangeParameter(const std::string& name, const Parameter& begin, 9 | const Parameter& end, const Parameter& increment, 10 | const std::string& ) : 11 | CompositeParameter(name) 12 | { 13 | Parameter* param = begin.clone(); 14 | param->name() = name; 15 | Parameter* endRenamed = end.clone(); 16 | endRenamed->name() = name; 17 | 18 | for (; *param <= *endRenamed; *param += increment) 19 | values.push_back(param->clone()); 20 | 21 | // values.push_back(endRenamed->clone()); // needed to ensure <= 22 | 23 | delete endRenamed; 24 | delete param; 25 | 26 | param_list.insert(begin); 27 | param_list.insert(end); 28 | param_list.insert(increment); 29 | } 30 | 31 | RangeParameter::RangeParameter(const std::string& name, 32 | const XmlNode& node) : 33 | CompositeParameter(name, node) 34 | { 35 | // determine if for or sequence 36 | if (param_list.hasParam("begin") && param_list.hasParam("end") 37 | && param_list.hasParam("increment")) 38 | { 39 | Parameter* param = param_list["begin"].clone(); 40 | param->name() = name; 41 | Parameter* endRenamed = param_list["end"].clone(); 42 | endRenamed->name() = name; 43 | 44 | for (; *param <= *endRenamed; *param += param_list["increment"]) 45 | values.push_back(param->clone()); 46 | 47 | // values.push_back(endRenamed->clone()); 48 | delete endRenamed; 49 | delete param; 50 | } 51 | else 52 | { 53 | for (ParameterList::const_iterator it = param_list.begin(); it 54 | != param_list.end(); ++it) 55 | { 56 | Parameter* param = it->clone(); 57 | // rename this. 58 | param->name() = name; 59 | values.push_back(param); 60 | } 61 | } 62 | 63 | if(values.begin() == values.end()) 64 | throw Exception("RangeParameter initialization failed! No child params found."); 65 | 66 | } 67 | 68 | RangeParameter::RangeParameter(const std::string& name, const ParameterList& vals) 69 | : CompositeParameter(name, vals) 70 | { 71 | for (ParameterList::const_iterator it = param_list.begin(); it 72 | != param_list.end(); ++it) 73 | { 74 | Parameter* param = it->clone(); 75 | // rename this. 76 | param->name() = name; 77 | values.push_back(param); 78 | } 79 | } 80 | 81 | RangeParameter::~RangeParameter() 82 | { 83 | for (std::list::const_iterator it = values.begin(); it 84 | != values.end(); ++it) 85 | delete *it; 86 | values.clear(); 87 | } 88 | 89 | RangeParameter::RangeParameter(const RangeParameter& other) 90 | : CompositeParameter(other) 91 | { 92 | for (std::list::const_iterator it = other.values.begin(); it 93 | != other.values.end(); ++it) 94 | values.push_back((*it)->clone()); 95 | } 96 | 97 | RangeParameter& RangeParameter::operator=(const RangeParameter& other) 98 | { 99 | if (&other == this) return *this; 100 | 101 | for (std::list::const_iterator it = values.begin(); it 102 | != values.end(); ++it) 103 | delete *it; 104 | values.clear(); 105 | 106 | for (std::list::const_iterator it = other.values.begin(); it 107 | != other.values.end(); ++it) 108 | values.push_back((*it)->clone()); 109 | 110 | return *this; 111 | } 112 | 113 | RangeParameter* RangeParameter::clone() const 114 | { 115 | return new RangeParameter(*this); 116 | } 117 | 118 | std::string RangeParameter::toString() const 119 | { 120 | std::stringstream out; 121 | 122 | out << "" << std::endl; 123 | for (ParameterList::const_iterator it = param_list.begin(); it 124 | != param_list.end(); ++it) 125 | out << " " << *it << std::endl; 126 | out << ""; 127 | 128 | return out.str(); 129 | } 130 | 131 | RangeParameter* parseRangeParameter(const XmlNode& node) 132 | { 133 | if (node.getAttribute("type") == "range") 134 | return new RangeParameter(node.getAttribute("name"), node); 135 | else 136 | throw XmlError("Error while parameter parsing: 'type' not 'string'"); 137 | } 138 | 139 | } 140 | -------------------------------------------------------------------------------- /src/rv/RingBuffer.h: -------------------------------------------------------------------------------- 1 | /** \brief ring buffer with predefined capacity 2 | * 3 | * a ring buffer offers push_back and push_front mechanism 4 | * to add elements with a predefined capacity, i.e.,the 5 | * buffer size will never be larger than capacity. 6 | * Thus elements at the end or the beginning are dropped, 7 | * if the capacity is reached. The order of elements is 8 | * preserved. 9 | * 10 | * 11 | * \author behley 12 | */ 13 | 14 | #ifndef RINGBUFFER_H_ 15 | #define RINGBUFFER_H_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace rv 23 | { 24 | 25 | template 26 | class RingBuffer 27 | { 28 | public: 29 | /** \brief initialize a buffer of capacity c **/ 30 | RingBuffer(uint32_t c) : 31 | mStart(0), mCapacity(c), mSize(0), mData(c) 32 | { 33 | } 34 | 35 | /** \brief access the i-th element 36 | * \param index of element in the buffer. 37 | **/ 38 | T& operator[](uint32_t i); 39 | const T& operator[](uint32_t i) const; 40 | 41 | /** \brief insert an element at the end, but this might drop an element at the front. **/ 42 | void push_back(const T& value); 43 | 44 | /** \brief insert an element at the front, but this might drop an element at the end. **/ 45 | void push_front(const T& value); 46 | 47 | /** \brief number of items in buffer **/ 48 | uint32_t size() const 49 | { 50 | return mSize; 51 | } 52 | 53 | /** \brief capacity. **/ 54 | uint32_t capacity() const 55 | { 56 | return mCapacity; 57 | } 58 | 59 | /** \brief remove all elements from the buffer. **/ 60 | void clear() 61 | { 62 | mStart = 0; 63 | mSize = 0; 64 | } 65 | 66 | friend std::ostream& operator<<(std::ostream& out, const RingBuffer& buffer) 67 | { 68 | out << "["; 69 | if (buffer.mSize > 0) out << buffer.mData[buffer.mStart]; 70 | for (uint32_t i = 1; i < buffer.mSize; ++i) 71 | { 72 | out << ", " << buffer.mData[(i + buffer.mStart) % buffer.mCapacity]; 73 | } 74 | out << "]"; 75 | 76 | return out; 77 | } 78 | 79 | private: 80 | uint32_t mStart; 81 | uint32_t mCapacity; 82 | uint32_t mSize; 83 | std::vector mData; 84 | }; 85 | 86 | template 87 | T& RingBuffer::operator[](uint32_t i) 88 | { 89 | assert(i < mSize); 90 | return mData[(i + mStart) % mCapacity]; 91 | } 92 | 93 | template 94 | const T& RingBuffer::operator[](uint32_t i) const 95 | { 96 | assert(i < mSize); 97 | return mData[(i + mStart) % mCapacity]; 98 | } 99 | 100 | template 101 | void RingBuffer::push_back(const T& value) 102 | { 103 | mData[(mStart + mSize) % mCapacity] = value; 104 | if (mSize == mCapacity) mStart = (mStart + 1) % mCapacity; 105 | if (mSize < mCapacity) ++mSize; 106 | } 107 | 108 | template 109 | void RingBuffer::push_front(const T& value) 110 | { 111 | mStart = (mStart - 1 + mCapacity) % mCapacity; 112 | mData[mStart] = value; 113 | if (mSize < mCapacity) ++mSize; 114 | } 115 | 116 | } 117 | #endif /* RINGBUFFER_H_ */ 118 | -------------------------------------------------------------------------------- /src/rv/Stopwatch.cpp: -------------------------------------------------------------------------------- 1 | #include "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/XmlDocument.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/XmlDocument.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "yxml.h" 8 | #include "rv/XmlError.h" 9 | 10 | namespace rv 11 | { 12 | 13 | XmlDocument::XmlDocument() 14 | { 15 | 16 | } 17 | 18 | const XmlNode& XmlDocument::root() const 19 | { 20 | return root_; 21 | } 22 | 23 | XmlDocument XmlDocument::fromFile(const std::string& filename, bool partial) 24 | { 25 | std::ifstream in(filename.c_str()); 26 | XmlDocument doc = fromStream(in, partial); 27 | in.close(); 28 | 29 | return doc; 30 | } 31 | 32 | XmlDocument XmlDocument::fromData(const std::string& buffer, bool partial) 33 | { 34 | std::istringstream in(buffer); 35 | 36 | return fromStream(in, partial); 37 | } 38 | 39 | XmlDocument XmlDocument::fromStream(std::istream& stream, bool partial) 40 | { 41 | void* buf = malloc(4096); 42 | yxml_t x; 43 | yxml_init(&x, buf, 4096); 44 | 45 | XmlDocument doc; 46 | std::vector currentNodes; // stack of nodes that are currently initialized. 47 | 48 | yxml_ret_t ret; 49 | 50 | XmlNode* node = 0; 51 | std::string attr_value; 52 | std::string content; 53 | 54 | stream.peek(); 55 | while (!stream.eof()) 56 | { 57 | ret = yxml_parse(&x, stream.get()); 58 | 59 | switch (ret) 60 | { 61 | case YXML_ELEMSTART: 62 | node = 0; 63 | if (currentNodes.empty()) 64 | { 65 | node = &doc.root_; 66 | } 67 | else 68 | { 69 | // Note: this could alter the pointers in currentNodes; 70 | // However, the invariant is that parent.children are already initialized! 71 | XmlNode* parent = currentNodes.back(); 72 | parent->children_.push_back(XmlNode()); 73 | node = &parent->children_.back(); 74 | } 75 | 76 | assert(node != 0); 77 | node->valid_ = partial; 78 | node->lineNumber_ = x.line; 79 | node->tag_name_ = x.elem; 80 | 81 | currentNodes.push_back(node); 82 | content.clear(); 83 | break; 84 | 85 | case YXML_CONTENT: 86 | content += x.data; 87 | break; 88 | 89 | case YXML_ELEMEND: 90 | node = currentNodes.back(); 91 | 92 | if (node->children_.size() == 0) node->content_ = content; 93 | 94 | node->valid_ = true; 95 | currentNodes.pop_back(); 96 | 97 | break; 98 | 99 | case YXML_ATTRSTART: 100 | attr_value.clear(); 101 | break; 102 | 103 | case YXML_ATTRVAL: 104 | attr_value += x.data; 105 | break; 106 | 107 | case YXML_ATTREND: 108 | node = currentNodes.back(); 109 | node->attributes_[x.attr] = attr_value; 110 | break; 111 | 112 | case YXML_OK: 113 | case YXML_PISTART: 114 | case YXML_PICONTENT: 115 | case YXML_PIEND: 116 | // unhandled. 117 | break; 118 | 119 | case YXML_EEOF: 120 | case YXML_EREF: 121 | case YXML_ECLOSE: 122 | case YXML_ESTACK: 123 | case YXML_ESYN: 124 | free(buf); 125 | 126 | std::stringstream reason; 127 | reason << "Error while parsing in line " << x.line; 128 | throw XmlError(reason.str()); 129 | break; 130 | } 131 | 132 | stream.peek(); 133 | } 134 | 135 | free(buf); 136 | 137 | if (!partial && (currentNodes.size() > 0)) throw XmlError("Error while parsing: Unexpected EOF."); 138 | 139 | return doc; 140 | } 141 | 142 | } /* namespace rv */ 143 | -------------------------------------------------------------------------------- /src/rv/XmlDocument.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_XMLDOCUMENT_H_ 2 | #define CORE_XMLDOCUMENT_H_ 3 | 4 | #include "XmlNode.h" 5 | #include "XmlError.h" 6 | 7 | /** \brief 8 | * 9 | * \author behley 10 | */ 11 | 12 | namespace rv 13 | { 14 | 15 | class XmlDocument 16 | { 17 | public: 18 | /** \brief Returns the root node of the XML document. */ 19 | const XmlNode& root() const; 20 | 21 | /** \brief Reads an XML document from a file. 22 | * 23 | * \param[in] fileName the file to be read 24 | * \param[in] partial do we generate document for partial/incomplete XML files? (default: false) 25 | * \throw XmlError if the file could not be read or is malformed 26 | */ 27 | static XmlDocument fromFile(const std::string& fileName, bool partial = false); 28 | 29 | /** \brief Reads an XML document from a string. 30 | * \param[in] buffer 31 | * \param[in] partial do we generate document for partial/incomplete XML files? (default: false) 32 | * \throw XmlError if the document is malformed 33 | */ 34 | static XmlDocument fromData(const std::string& buffer, bool partial = false); 35 | 36 | /** \brief Reads an XML document from a input stream. 37 | * 38 | * \param in input stream 39 | * \param partial do we generate document for partial/incomplete XML files? (default: false) 40 | * \return xml document 41 | * \throw XmlError if the document is malformed 42 | */ 43 | static XmlDocument fromStream(std::istream& in, bool partial = false); 44 | 45 | protected: 46 | XmlDocument(); 47 | 48 | XmlNode root_; 49 | }; 50 | 51 | } /* namespace rv */ 52 | #endif /* SRC_CORE_RV_XMLDOCUMENT_H_ */ 53 | -------------------------------------------------------------------------------- /src/rv/XmlError.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_XMLERROR_H_ 2 | #define CORE_XMLERROR_H_ 3 | 4 | #include 5 | 6 | /** \brief Exception thrown in case of an error while parsing XML files. 7 | * 8 | * \author behley 9 | */ 10 | 11 | namespace rv 12 | { 13 | 14 | class XmlError: public std::runtime_error 15 | { 16 | public: 17 | XmlError(const std::string& reason) : 18 | std::runtime_error(reason) 19 | { 20 | } 21 | }; 22 | 23 | } 24 | #endif /* SRC_CORE_RV_XMLERROR_H_ */ 25 | -------------------------------------------------------------------------------- /src/rv/XmlNode.cpp: -------------------------------------------------------------------------------- 1 | #include "rv/XmlNode.h" 2 | #include "rv/XmlError.h" 3 | 4 | namespace rv 5 | { 6 | 7 | XmlNode::XmlNode() : 8 | valid_(false), lineNumber_(0) 9 | { 10 | 11 | } 12 | 13 | //XmlNode::~XmlNode() 14 | //{ 15 | // for (uint32_t i = 0; i < children_.size(); ++i) 16 | // delete children_[i]; 17 | //} 18 | 19 | //XmlNode::XmlNode(const XmlNode& other) : 20 | // valid_(other.valid_), tag_name_(other.tag_name_), content_(other.content_), attributes_(other.attributes_), lineNumber_( 21 | // other.lineNumber_) 22 | //{ 23 | // for (uint32_t i = 0; i < other.children_.size(); ++i) 24 | // children_.push_back(new XmlNode(*other.children_[i])); 25 | //} 26 | 27 | //XmlNode& XmlNode::operator=(const XmlNode& other) 28 | //{ 29 | // valid_ = other.valid_; 30 | // tag_name_ = other.tag_name_; 31 | // content_ = other.content_; 32 | // attributes_ = other.attributes_; 33 | // lineNumber_ = other.lineNumber_; 34 | // 35 | // for (uint32_t i = 0; i < children_.size(); ++i) 36 | // delete children_[i]; 37 | // children_.clear(); 38 | // 39 | // for (uint32_t i = 0; i < other.children_.size(); ++i) 40 | // children_.push_back(new XmlNode(*other.children_[i])); 41 | // 42 | // return *this; 43 | //} 44 | 45 | bool XmlNode::isValid() const 46 | { 47 | return valid_; 48 | } 49 | 50 | /** \brief Returns the tag name. */ 51 | const std::string& XmlNode::getName() const 52 | { 53 | return tag_name_; 54 | } 55 | 56 | /** \brief Returns the node content, i.e. the data between opening and closing tag */ 57 | const std::string& XmlNode::getContent() const 58 | { 59 | return content_; 60 | } 61 | 62 | uint32_t XmlNode::getLineNo() const 63 | { 64 | 65 | return lineNumber_; 66 | } 67 | 68 | /** \brief Returns true if the node has at least one child node with the given tag name. */ 69 | bool XmlNode::hasChild(const std::string& name) const 70 | { 71 | for (List::const_iterator it = children_.begin(); it != children_.end(); ++it) 72 | if (it->tag_name_ == name) return true; 73 | 74 | return false; 75 | } 76 | 77 | /** \brief Returns true if the node has the given attribute. */ 78 | bool XmlNode::hasAttribute(const std::string& name) const 79 | { 80 | return (attributes_.find(name) != attributes_.end()); 81 | } 82 | 83 | /** \brief Returns the value of the given attribute. 84 | * \throw XmlError if the node has no attribute \a name */ 85 | std::string XmlNode::getAttribute(const std::string& name) const 86 | { 87 | if (attributes_.find(name) == attributes_.end()) throw XmlError("Attribute not found"); 88 | 89 | return attributes_.find(name)->second; 90 | } 91 | 92 | /** \brief Returns a list of all child nodes. */ 93 | const XmlNode::List& XmlNode::getChildren() const 94 | { 95 | return children_; 96 | } 97 | 98 | XmlNode::List XmlNode::getChildren(const std::string& name) const 99 | { 100 | XmlNode::List matches; 101 | for (List::const_iterator it = children_.begin(); it != children_.end(); ++it) 102 | if (it->tag_name_ == name) matches.push_back(*it); 103 | 104 | return matches; 105 | } 106 | 107 | XmlNode XmlNode::getChild(const std::string& name) const 108 | { 109 | for (List::const_iterator it = children_.begin(); it != children_.end(); ++it) 110 | { 111 | if (it->tag_name_ == name) return *it; 112 | } 113 | 114 | return XmlNode(); 115 | } 116 | 117 | XmlNode::List XmlNode::getChildrenWith(const std::string& name, const std::string& value) const 118 | { 119 | XmlNode::List matches; 120 | for (List::const_iterator it = children_.begin(); it != children_.end(); ++it) 121 | { 122 | if (!it->hasAttribute(name)) continue; 123 | if (it->attributes_.find(name)->second == value) matches.push_back(*it); 124 | } 125 | 126 | return matches; 127 | } 128 | 129 | XmlNode XmlNode::getChildWith(const std::string& name, const std::string& value) const 130 | { 131 | for (List::const_iterator it = children_.begin(); it != children_.end(); ++it) 132 | { 133 | if (!it->hasAttribute(name)) continue; 134 | if (it->attributes_.find(name)->second == value) return *it; 135 | } 136 | 137 | return XmlNode(); 138 | } 139 | 140 | } /* namespace rv */ 141 | 142 | -------------------------------------------------------------------------------- /src/rv/XmlNode.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_XMLNODE_H_ 2 | #define CORE_XMLNODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** \brief XmlNode in the DOM representation of an XmlDocument. 10 | * 11 | * 12 | * Disclaimer: class is inspired by RoSe::XmlNode by Timo Röhling. 13 | * 14 | * \author behley 15 | */ 16 | 17 | namespace rv 18 | { 19 | 20 | class XmlNode 21 | { 22 | public: 23 | friend class XmlDocument; 24 | 25 | XmlNode(); 26 | // ~XmlNode(); 27 | // XmlNode(const XmlNode& other); 28 | // XmlNode& operator=(const XmlNode& other); 29 | 30 | /** \brief List of nodes. */ 31 | typedef std::vector List; 32 | 33 | /** \brief Is XmlNode valid? **/ 34 | bool isValid() const; 35 | 36 | /** \brief Returns the tag name. */ 37 | const std::string& getName() const; 38 | 39 | /** \brief Returns the node content, i.e. the data between opening and closing tag */ 40 | const std::string& getContent() const; 41 | 42 | uint32_t getLineNo() const; 43 | 44 | /** \brief Returns true if the node has at least one child node with the given tag name. */ 45 | bool hasChild(const std::string& name) const; 46 | 47 | /** \brief Returns true if the node has the given attribute. */ 48 | bool hasAttribute(const std::string& name) const; 49 | 50 | /** \brief Returns the value of the given attribute. 51 | * \throw XmlError if the node has no attribute \a name */ 52 | std::string getAttribute(const std::string& name) const; 53 | 54 | /** \brief Returns a list of all child nodes. */ 55 | const List& getChildren() const; 56 | 57 | /** \brief Returns a list of all child nodes with the given tag name. */ 58 | List getChildren(const std::string& name) const; 59 | /** \brief Returns the first child node with the given tag name. 60 | * 61 | * If no such node exists, an invalid node is returned. 62 | */ 63 | XmlNode getChild(const std::string& name) const; 64 | /** \brief Returns a list of all child nodes which have the given attribute and value. */ 65 | 66 | List getChildrenWith(const std::string& attribute, const std::string& value) const; 67 | /** \brief Returns the first child node that has the given attribute. 68 | * 69 | * If no such node exists, an invalid node is returned. 70 | */ 71 | XmlNode getChildWith(const std::string& name, const std::string& value) const; 72 | 73 | protected: 74 | bool valid_; 75 | std::string tag_name_; 76 | std::string content_; 77 | std::map attributes_; 78 | uint32_t lineNumber_; 79 | 80 | std::vector children_; 81 | }; 82 | 83 | } /* namespace rv */ 84 | #endif /* CORE_XMLNODE_H_ */ 85 | -------------------------------------------------------------------------------- /src/rv/point_traits.h: -------------------------------------------------------------------------------- 1 | #ifndef CORE_POINT_TRAITS_H_ 2 | #define CORE_POINT_TRAITS_H_ 3 | 4 | /** \brief 5 | * 6 | * Some traits to access coordinates regardless of the specific implementation of point 7 | * inspired by boost.geometry, which needs to be implemented by new points. 8 | * 9 | * \author behley 10 | */ 11 | 12 | namespace rv 13 | { 14 | 15 | namespace traits 16 | { 17 | 18 | template 19 | struct access 20 | { 21 | }; 22 | 23 | template 24 | struct access 25 | { 26 | static float get(const PointT& p) 27 | { 28 | return p.x; 29 | } 30 | 31 | static void set(PointT& p, float value) 32 | { 33 | p.x = value; 34 | } 35 | }; 36 | 37 | template 38 | struct access 39 | { 40 | static float get(const PointT& p) 41 | { 42 | return p.y; 43 | } 44 | 45 | static void set(PointT& p, float value) 46 | { 47 | p.y = value; 48 | } 49 | }; 50 | 51 | template 52 | struct access 53 | { 54 | static float get(const PointT& p) 55 | { 56 | return p.z; 57 | } 58 | 59 | static void set(PointT& p, float value) 60 | { 61 | p.z = value; 62 | } 63 | }; 64 | 65 | } // namespace traits 66 | 67 | /** convenience function for access of point coordinates **/ 68 | template 69 | inline float get(const PointT& p) 70 | { 71 | return traits::access::get(p); 72 | } 73 | 74 | template 75 | inline void set(PointT& p, float value) 76 | { 77 | traits::access::set(p, value); 78 | } 79 | 80 | } // namespace rv 81 | 82 | #endif /* SRC_CORE_RV_POINT_TRAITS_H_ */ 83 | -------------------------------------------------------------------------------- /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 | 11 | std::string trim(const std::string& str, const std::string& whitespaces ) 12 | { 13 | int32_t beg = 0; 14 | int32_t end = 0; 15 | 16 | /** find the beginning **/ 17 | for (beg = 0; beg < (int32_t) str.size(); ++beg) 18 | { 19 | bool found = false; 20 | for (uint32_t i = 0; i < whitespaces.size(); ++i) 21 | { 22 | if (str[beg] == whitespaces[i]) 23 | { 24 | found = true; 25 | break; 26 | } 27 | } 28 | if (!found) break; 29 | } 30 | 31 | /** find the end **/ 32 | for (end = int32_t(str.size()) - 1; end > beg; --end) 33 | { 34 | bool found = false; 35 | for (uint32_t i = 0; i < whitespaces.size(); ++i) 36 | { 37 | if (str[end] == whitespaces[i]) 38 | { 39 | found = true; 40 | break; 41 | } 42 | } 43 | if (!found) break; 44 | } 45 | 46 | return str.substr(beg, end - beg + 1); 47 | } 48 | 49 | std::vector split(const std::string& line, const std::string& delim, bool skipEmpty) 50 | { 51 | std::vector tokens; 52 | 53 | boost::char_separator sep(delim.c_str(), "", (skipEmpty ? boost::drop_empty_tokens : boost::keep_empty_tokens)); 54 | boost::tokenizer > tokenizer(line, sep); 55 | 56 | for (auto it = tokenizer.begin(); it != tokenizer.end(); ++it) 57 | tokens.push_back(*it); 58 | 59 | return tokens; 60 | } 61 | 62 | } 63 | 64 | //std::string operator+(const std::string& string, const uint32_t& other) 65 | //{ 66 | // std::stringstream stream; 67 | // stream << string << other; 68 | // return stream.str(); 69 | //} 70 | // 71 | //std::string operator+(const std::string& string, const int32_t& other) 72 | //{ 73 | // std::stringstream stream; 74 | // stream << string << other; 75 | // return stream.str(); 76 | //} 77 | // 78 | //std::string operator+(const std::string& string, const float& other) 79 | //{ 80 | // std::stringstream stream; 81 | // stream << string << other; 82 | // return stream.str(); 83 | //} 84 | // 85 | //std::string operator+(const std::string& string, const double& other) 86 | //{ 87 | // std::stringstream stream; 88 | // stream << string << other; 89 | // return stream.str(); 90 | //} 91 | 92 | -------------------------------------------------------------------------------- /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/shader/Frame2Model_jacobians.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec3 values; 4 | 5 | // layout (location = 0) 6 | out vec4 result; 7 | 8 | void main() 9 | { 10 | // blending ensures that the sum is computed. 11 | result = vec4(values, 0.0); 12 | } -------------------------------------------------------------------------------- /src/shader/Frame2Model_jacobians.vert: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | /** \brief Computation of residual & Jacobian from given model vertices & data vertices. 4 | * 5 | * Generating everything needed for computation of (weighted) Jacobian products. 6 | * 7 | * Computation happens in the geometry shader, since this turned out to be more productive. 8 | * 9 | * 10 | * \author behley 11 | **/ 12 | 13 | layout (location = 0) in vec2 texCoords; 14 | 15 | out VS_OUT { 16 | vec2 texCoords; 17 | } vs_out; 18 | 19 | void main() 20 | { 21 | gl_Position = vec4(0.5); 22 | vs_out.texCoords = texCoords; 23 | } -------------------------------------------------------------------------------- /src/shader/avg_vertexmap.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | 5 | uniform sampler2DRect in_vertexmap; 6 | 7 | out vec4 out_vertexmap; 8 | 9 | void main() 10 | { 11 | vec2 dim = textureSize(in_vertexmap); 12 | vec2 p = vec2(int(texCoords.x * dim.x), int(texCoords.y * dim.y)); 13 | 14 | out_vertexmap = texture(in_vertexmap, p); 15 | if(out_vertexmap.w > 0.5) out_vertexmap = out_vertexmap / out_vertexmap.w; 16 | } -------------------------------------------------------------------------------- /src/shader/bilateral_filter.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | 5 | uniform sampler2DRect in_vertexmap; 6 | uniform float width; 7 | uniform float height; 8 | uniform float sigma_space; 9 | uniform float sigma_range; 10 | 11 | out vec4 out_vertexmap; 12 | 13 | float wrap(float x, float dim) 14 | { 15 | float value = x; 16 | 17 | while(value >= dim) value = (value - dim); 18 | while(value < 0) value = (value + dim); 19 | 20 | return value; 21 | } 22 | 23 | void main() 24 | { 25 | int x = int(texCoords.x * width); 26 | int y = int(texCoords.y * height); 27 | 28 | vec4 vertex = texture(in_vertexmap, vec2(x, y)); 29 | 30 | out_vertexmap = vertex; 31 | 32 | if(vertex.w > 0.5) 33 | { 34 | float range = length(vertex.xyz); 35 | vec3 ray = vertex.xyz / range; 36 | 37 | 38 | float sigma_space_factor = -0.5 / (sigma_space * sigma_space); // elastic fusion: 0.024691358 => sigma = 4.5 39 | float sigma_range_factor = -0.5 / (sigma_range * sigma_range); // elastic fusion: 0.000555556 => sigma = 30 40 | 41 | const int R = 6; // make R dependent on sigma_space? 42 | const int D = R * 2 + 1; 43 | 44 | int tx = x - D / 2 + D; 45 | int ty = min(y - D / 2 + D, int(height)); 46 | 47 | float sum1 = 0.0f; 48 | float sum2 = 0.0f; 49 | 50 | for(int cy = max(y - D / 2, 0); cy < ty; ++cy) 51 | { 52 | for(int cx = x - D / 2; cx < tx; ++cx) 53 | { 54 | float xx = wrap(cx, width); 55 | 56 | vec4 tmp = texture(in_vertexmap, vec2(xx, cy)); 57 | if(tmp.w < 0.5) continue; 58 | 59 | float tmp_range = length(tmp); 60 | 61 | 62 | float diff_space2 = (x-xx) * (x-xx) + (y-cy) * (y-cy); // dot((vertex.xyz - tmp.xyz) , (vertex.xyz - tmp.xyz)); 63 | float diff_range2 = (range - tmp_range) * (range - tmp_range); 64 | 65 | float weight = exp(diff_space2 * sigma_space_factor + diff_range2 * sigma_range_factor); 66 | 67 | sum1 += tmp_range * weight; 68 | sum2 += weight; 69 | } 70 | } 71 | 72 | float filtered_range = sum1 / sum2; 73 | 74 | out_vertexmap = vec4( filtered_range * ray, 1.0); 75 | } 76 | } -------------------------------------------------------------------------------- /src/shader/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/shader/coloredvertices.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 vertexColor; 4 | out vec4 color; 5 | 6 | 7 | 8 | 9 | 10 | void main() 11 | { 12 | color = vertexColor; 13 | } 14 | -------------------------------------------------------------------------------- /src/shader/coloredvertices.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position_color; 4 | 5 | uniform mat4 mvp; 6 | 7 | out vec4 vertexColor; 8 | 9 | uniform bool useCustomColor; 10 | uniform vec4 customColor; 11 | 12 | vec4 decodeColor(float c) 13 | { 14 | vec4 col; 15 | col.x = float(int(c) >> 16 & 0xFF) / 255.0f; 16 | col.y = float(int(c) >> 8 & 0xFF) / 255.0f; 17 | col.z = float(int(c) & 0xFF) / 255.0f; 18 | col.w = 1.0f; 19 | 20 | return col; 21 | } 22 | 23 | void main() 24 | { 25 | gl_Position = mvp * vec4(position_color.xyz, 1.0); 26 | vertexColor = decodeColor(position_color.w); 27 | if(useCustomColor) vertexColor = customColor; 28 | } -------------------------------------------------------------------------------- /src/shader/copy_surfels.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(points, max_vertices = 1) out; 5 | 6 | in SURFEL 7 | { 8 | bool valid; 9 | vec4 position_radius; 10 | vec4 normal_confidence; 11 | int timestamp; 12 | vec3 color_weight_count; 13 | } gs_in[]; 14 | 15 | out vec4 sfl_position_radius; 16 | out vec4 sfl_normal_confidence; 17 | out int sfl_timestamp; 18 | out vec3 sfl_color_weight_count; 19 | 20 | void main() 21 | { 22 | if(gs_in[0].valid) 23 | { 24 | sfl_position_radius = gs_in[0].position_radius; 25 | sfl_normal_confidence = gs_in[0].normal_confidence; 26 | sfl_timestamp = gs_in[0].timestamp; 27 | sfl_color_weight_count = gs_in[0].color_weight_count; 28 | 29 | EmitVertex(); 30 | EndPrimitive(); 31 | } 32 | } -------------------------------------------------------------------------------- /src/shader/copy_surfels.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position_radius; 4 | layout (location = 1) in vec4 normal_confidence; 5 | layout (location = 2) in int in_timestamp; 6 | layout (location = 3) in vec3 surfel_color_weight_count; 7 | 8 | out SURFEL 9 | { 10 | bool valid; 11 | vec4 position_radius; 12 | vec4 normal_confidence; 13 | int timestamp; 14 | vec3 color_weight_count; 15 | } vs_out; 16 | 17 | // active area 18 | uniform vec2 submap_center; 19 | uniform float submap_extent; 20 | 21 | uniform samplerBuffer poseBuffer; 22 | 23 | mat4 get_pose(int t) 24 | { 25 | int offset = 4 * t; 26 | return mat4(texelFetch(poseBuffer, offset), texelFetch(poseBuffer, offset + 1), 27 | texelFetch(poseBuffer, offset + 2), texelFetch(poseBuffer, offset+3)); 28 | } 29 | 30 | void main() 31 | { 32 | mat4 surfelPose = get_pose(int(surfel_color_weight_count.z)); 33 | vec4 position = surfelPose * vec4(position_radius.xyz, 1.0); 34 | 35 | if(in_timestamp < 0 || abs(position.x - submap_center.x) > submap_extent || abs(position.y - submap_center.y) > submap_extent) 36 | { 37 | vs_out.valid = false; 38 | } 39 | else 40 | { 41 | vs_out.valid = true; 42 | vs_out.position_radius = position_radius; 43 | vs_out.normal_confidence = normal_confidence; 44 | vs_out.timestamp = in_timestamp; 45 | vs_out.color_weight_count = surfel_color_weight_count; 46 | } 47 | } -------------------------------------------------------------------------------- /src/shader/draw_depthimg.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | out vec4 color; 5 | 6 | uniform int colorMode; // 0 - intensity, 1 - hsv, 2 - raw 7 | uniform sampler2DRect depthTexture; 8 | 9 | uniform bool markInvalidDepthValues; 10 | uniform bool filterDepth; 11 | uniform float minDepth; 12 | uniform float maxDepth; 13 | 14 | 15 | 16 | // source: http://lolengine.net/blog/2013/07/27/rgb-to-hsv-in-glsl 17 | vec3 hsv2rgb(vec3 c) 18 | { 19 | vec4 K = vec4(1.0, 2.0 / 3.0, 1.0 / 3.0, 3.0); 20 | vec3 p = abs(fract(c.xxx + K.xyz) * 6.0 - K.www); 21 | 22 | return c.z * mix(K.xxx, clamp(p - K.xxx, 0.0, 1.0), c.y); 23 | } 24 | 25 | const vec4 rviridis = vec4(2.90912735, -2.14404531, 0.04439198, 0.29390206); 26 | const vec4 gviridis = vec4(-0.17293242, -0.16906214, 1.24131122, 0.01871256); 27 | const vec4 bviridis = vec4(0.17848859, -1.72405244, 1.23042564, 0.34479632); 28 | 29 | // approximate version of viridis colormap. 30 | vec3 viridis(float t) 31 | { 32 | vec4 tt = vec4(t*t*t, t*t, t, 1.0); 33 | return vec3(dot(tt, rviridis), dot(tt, gviridis), dot(tt, bviridis)); 34 | } 35 | 36 | 37 | void main() 38 | { 39 | float depth = length(texture(depthTexture, texCoords * textureSize(depthTexture)).xyz); 40 | bool valid = bool(texture(depthTexture, texCoords * textureSize(depthTexture)).w); 41 | 42 | color = vec4(0.0, 0.0, 0.0, 1.0); // default color. 43 | 44 | if(!valid && markInvalidDepthValues) 45 | { 46 | color = vec4(1.0, 0.0, 0.0, 1.0); 47 | } 48 | else 49 | { 50 | if(filterDepth && (depth < minDepth || depth > maxDepth)) 51 | { 52 | color = vec4(0.0, 0.0, 0.0, 1.0); 53 | } 54 | else 55 | { 56 | if(filterDepth && colorMode != 2) depth = (depth - minDepth) / (maxDepth-minDepth); 57 | else if(colorMode != 2) depth = depth/50.0f; 58 | 59 | if(colorMode == 0) color = vec4(viridis(depth), 1.0); 60 | else if(colorMode == 1) color = vec4(hsv2rgb(vec3(depth, 1.0, 1.0)), 1.0); 61 | else if(colorMode == 2) color = vec4(depth, 0, 0, 1.0); 62 | } 63 | } 64 | } -------------------------------------------------------------------------------- /src/shader/draw_mesh.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec4 pos; 4 | in vec4 normal; 5 | 6 | struct Light 7 | { 8 | vec4 position; // directional lights have w == 0. 9 | 10 | vec3 ambient; 11 | vec3 diffuse; 12 | vec3 specular; 13 | }; 14 | 15 | uniform int num_lights; 16 | uniform Light lights[10]; 17 | 18 | uniform vec4 view_pos; 19 | 20 | struct Material 21 | { 22 | vec3 ambient; 23 | vec3 diffuse; 24 | vec3 specular; 25 | vec3 emission; 26 | 27 | float shininess; 28 | float alpha; 29 | }; 30 | 31 | uniform Material material; 32 | 33 | out vec4 color; 34 | 35 | // see also http://learnopengl.com/#!Lighting/Materials 36 | 37 | void main() 38 | { 39 | vec3 norm = normalize(vec3(normal)); 40 | vec3 view_dir = normalize((view_pos - pos).xyz); 41 | 42 | vec3 result = vec3(0); 43 | 44 | for(int i = 0; i < num_lights; ++i) 45 | { 46 | // ambient: 47 | vec3 ambient = lights[i].ambient * material.ambient; 48 | 49 | // diffuse: 50 | vec3 light_dir = normalize(lights[i].position.xyz - pos.xyz); 51 | if(lights[i].position.w < 0.0001) light_dir = normalize(-lights[i].position.xyz); // directional light. 52 | float diff = abs(dot(norm, light_dir)); 53 | vec3 diffuse = lights[i].diffuse * (diff * material.diffuse); 54 | 55 | // specular: 56 | vec3 reflect_dir = reflect(-light_dir, norm); 57 | float spec = pow(max(dot(view_dir, reflect_dir), 0.0), material.shininess); 58 | vec3 specular = lights[i].specular * (spec * material.specular); 59 | 60 | // emission (seems good to apply for each light the emission; with this the emission part doesn't just depend 61 | // on the number of lights: 62 | result += ambient + diffuse + specular + material.emission; 63 | } 64 | 65 | color = vec4(result, material.alpha); 66 | } -------------------------------------------------------------------------------- /src/shader/draw_mesh.vert: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | layout (location = 0) in vec4 position_in; 4 | layout (location = 1) in vec4 normal_in; 5 | 6 | uniform mat4 model_mat; // model matrix. 7 | uniform mat4 normal_mat; // transformation of normals to world coordinates. 8 | uniform mat4 mvp; // model-view-projection matrix. (applied p*v*m) 9 | 10 | out vec4 pos; 11 | out vec4 normal; 12 | 13 | void main() 14 | { 15 | gl_Position = mvp * position_in; 16 | pos = model_mat * position_in; 17 | normal = normal_mat * normal_in; 18 | } -------------------------------------------------------------------------------- /src/shader/draw_normalmap.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | out vec4 color; 5 | 6 | uniform sampler2DRect screenTexture; 7 | const vec4 invalid = vec4(0.0, 0.0, 0.0f, 1.0f); 8 | 9 | void main() 10 | { 11 | color = abs(texture(screenTexture, texCoords * textureSize(screenTexture))); 12 | if(color.w < 0.5f) color = invalid; 13 | } 14 | -------------------------------------------------------------------------------- /src/shader/draw_normalmap3d.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(line_strip, max_vertices = 2) out; 5 | 6 | in VS_OUT { 7 | bool valid; 8 | vec4 normal; 9 | vec4 color; 10 | } gs_in[]; 11 | 12 | out vec4 color; 13 | 14 | void main() 15 | { 16 | if(gs_in[0].valid) 17 | { 18 | color = gs_in[0].color; 19 | gl_Position = gl_in[0].gl_Position; 20 | EmitVertex(); 21 | 22 | gl_Position = gl_in[0].gl_Position + 0.1 * gs_in[0].normal; 23 | EmitVertex(); 24 | 25 | EndPrimitive(); 26 | } 27 | } -------------------------------------------------------------------------------- /src/shader/draw_normalmap3d.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec2 tex_position; 4 | 5 | uniform sampler2DRect texVertexMap; 6 | uniform sampler2DRect texNormalMap; 7 | 8 | uniform mat4 mvp; 9 | uniform mat4 mvp_inv_t; 10 | 11 | out VS_OUT { 12 | bool valid; 13 | vec4 normal; 14 | vec4 color; 15 | } vs_out; 16 | 17 | void main() 18 | { 19 | 20 | gl_Position = mvp * vec4(texture(texVertexMap, tex_position).rgb, 1.0); 21 | 22 | 23 | vs_out.valid = (texture(texNormalMap, tex_position).w > 0.5f); 24 | vs_out.normal = mvp_inv_t * vec4(texture(texNormalMap, tex_position).rgb, 0.0f); 25 | vs_out.color = vec4(abs(texture(texNormalMap, tex_position).rgb), 1.0f); 26 | } -------------------------------------------------------------------------------- /src/shader/draw_posegraph_edge.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(line_strip, max_vertices = 4) out; 5 | 6 | uniform mat4 from; 7 | uniform mat4 to; 8 | uniform mat4 measurement; 9 | 10 | uniform mat4 mvp; 11 | uniform mat4 mvp_inv_t; 12 | 13 | out vec4 color; 14 | 15 | // better solution would be to use instancing, but for now this hack will do it. 16 | 17 | void main() 18 | { 19 | // edge connecting the poses: 20 | vec4 origin = vec4(0,0,0,1); 21 | color = vec4(0.5, 0, 0.95, 1.0); 22 | gl_Position = mvp * from * origin; 23 | EmitVertex(); 24 | 25 | gl_Position = mvp * to * origin; 26 | EmitVertex(); 27 | 28 | EndPrimitive(); 29 | 30 | color = vec4(1.0, 0.0, 0.0, 1.0); 31 | vec4 tip = from * measurement * origin; 32 | 33 | gl_Position = mvp * tip; 34 | EmitVertex(); 35 | 36 | color = vec4(0.0, 1.0, 0.0, 1.0); 37 | 38 | gl_Position = mvp * (from * measurement * vec4(1,0,0,0) + tip); 39 | EmitVertex(); 40 | EndPrimitive(); 41 | 42 | 43 | } -------------------------------------------------------------------------------- /src/shader/draw_residuals.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | out vec4 color; 5 | 6 | 7 | uniform sampler2DRect residualmap; 8 | 9 | // source: http://lolengine.net/blog/2013/07/27/rgb-to-hsv-in-glsl 10 | vec3 hsv2rgb(vec3 c) 11 | { 12 | vec4 K = vec4(1.0, 2.0 / 3.0, 1.0 / 3.0, 3.0); 13 | vec3 p = abs(fract(c.xxx + K.xyz) * 6.0 - K.www); 14 | 15 | return c.z * mix(K.xxx, clamp(p - K.xxx, 0.0, 1.0), c.y); 16 | } 17 | 18 | const vec4 rviridis = vec4(2.90912735, -2.14404531, 0.04439198, 0.29390206); 19 | const vec4 gviridis = vec4(-0.17293242, -0.16906214, 1.24131122, 0.01871256); 20 | const vec4 bviridis = vec4(0.17848859, -1.72405244, 1.23042564, 0.34479632); 21 | 22 | // approximate version of viridis colormap. 23 | vec3 viridis(float t) 24 | { 25 | vec4 tt = vec4(t*t*t, t*t, t, 1.0); 26 | return vec3(dot(tt, rviridis), dot(tt, gviridis), dot(tt, bviridis)); 27 | } 28 | 29 | 30 | void main() 31 | { 32 | vec4 residual = texture(residualmap, texCoords * textureSize(residualmap)); 33 | //color = vec4(viridis(clamp(abs(residual.y) / 2.0, 0.0, 1.0)), 1.0); 34 | if(residual.w > 0.5 && residual.x > 0.5) color = vec4(0,1,0,1); 35 | else if(residual.w < -1.5) color = vec4(1,0,0,1); 36 | else if(residual.w < -0.5) color = vec4(0,0,1,1); 37 | else if(residual.x < 0.5) color = vec4(0,0,0,1); 38 | } -------------------------------------------------------------------------------- /src/shader/draw_submaps.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(line_strip, max_vertices = 5) out; 5 | 6 | in SUBMAP 7 | { 8 | vec2 center; 9 | float extent; 10 | } gs_in[]; 11 | 12 | uniform mat4 mvp; 13 | 14 | out vec4 vertexColor; 15 | uniform float submap_extent; 16 | 17 | void main() 18 | { 19 | float e = submap_extent; 20 | vertexColor = vec4(0,0,0,1); 21 | 22 | gl_Position = mvp * vec4(gs_in[0].center + vec2(e, e), 0, 1); 23 | EmitVertex(); 24 | 25 | gl_Position = mvp * vec4(gs_in[0].center + vec2(e, -e), 0, 1); 26 | EmitVertex(); 27 | 28 | gl_Position = mvp * vec4(gs_in[0].center + vec2(-e, -e), 0, 1); 29 | EmitVertex(); 30 | 31 | gl_Position = mvp * vec4(gs_in[0].center + vec2(-e, e), 0, 1); 32 | EmitVertex(); 33 | 34 | gl_Position = mvp * vec4(gs_in[0].center + vec2(e, e), 0, 1); 35 | EmitVertex(); 36 | 37 | EndPrimitive(); 38 | } -------------------------------------------------------------------------------- /src/shader/draw_submaps.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec2 submap_centers; 4 | 5 | uniform float submap_extent; 6 | 7 | out SUBMAP 8 | { 9 | vec2 center; 10 | float extent; 11 | } vs_out; 12 | 13 | 14 | void main() 15 | { 16 | vs_out.center = submap_centers; 17 | vs_out.extent = submap_extent; 18 | } -------------------------------------------------------------------------------- /src/shader/draw_surfelPoints.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position_radius; 4 | layout (location = 1) in vec4 normal_confidence; 5 | layout (location = 2) in int surfel_timestamp; 6 | layout (location = 3) in vec3 surfel_color_weight_count; 7 | 8 | uniform mat4 mvp; 9 | uniform mat4 mvp_inv_t; 10 | 11 | uniform int colorMode; // 0 - phong, 1 - normal shading, 2 - normal color, 3 - confidence 12 | uniform float conf_threshold; 13 | 14 | uniform vec3 view_pos; 15 | uniform bool drawCurrentSurfelsOnly; 16 | uniform int timestamp; 17 | uniform bool backface_culling; 18 | 19 | uniform samplerBuffer poseBuffer; 20 | 21 | out vec4 color; 22 | 23 | #include "shader/color.glsl" 24 | 25 | const vec4 rviridis = vec4(2.90912735, -2.14404531, 0.04439198, 0.29390206); 26 | const vec4 gviridis = vec4(-0.17293242, -0.16906214, 1.24131122, 0.01871256); 27 | const vec4 bviridis = vec4(0.17848859, -1.72405244, 1.23042564, 0.34479632); 28 | 29 | // approximate version of viridis colormap. 30 | vec3 viridis(float t) 31 | { 32 | vec4 tt = vec4(t*t*t, t*t, t, 1.0); 33 | return vec3(dot(tt, rviridis), dot(tt, gviridis), dot(tt, bviridis)); 34 | } 35 | 36 | mat4 get_pose(int timestamp) 37 | { 38 | int offset = 4 * timestamp; 39 | return mat4(texelFetch(poseBuffer, offset), texelFetch(poseBuffer, offset + 1), 40 | texelFetch(poseBuffer, offset + 2), texelFetch(poseBuffer, offset+3)); 41 | } 42 | 43 | void main() 44 | { 45 | mat4 surfelPose = get_pose(int(surfel_color_weight_count.z)); 46 | 47 | 48 | // projection etc is applied in geometry shader. 49 | gl_Position = mvp * surfelPose * vec4(position_radius.xyz, 1.0f); 50 | vec4 n = surfelPose * vec4(normal_confidence.xyz, 0.0f); 51 | float c = normal_confidence.w; 52 | 53 | vec4 p = vec4(position_radius.xyz, 1.0f); 54 | vec3 unpacked_color = unpack(surfel_color_weight_count.x); 55 | 56 | bool valid = (c > conf_threshold); 57 | vec3 view_dir = normalize((view_pos.xyz - p.xyz)); 58 | 59 | if(colorMode == 1) 60 | { 61 | color = vec4((vec3(.5f, .5f, .5f) * abs(dot(n.xyz, vec3(1.0, 1.0, 1.0)))) + vec3(0.1f, 0.1f, 0.1f), 1.0f); 62 | } 63 | else if(colorMode == 2) 64 | { 65 | color = vec4(abs(n.xyz), 1.0f); 66 | } 67 | else if(colorMode == 3) 68 | { 69 | valid = true; 70 | color = vec4(viridis(1.0-1.0/(1.0 + exp(c))), 1.0); 71 | } 72 | else 73 | { 74 | vec3 surfel_color = vec3(0.8f, 0.75f, 0.65f); 75 | float alpha = 1.0f; 76 | 77 | if (colorMode == 4) 78 | { 79 | surfel_color = unpacked_color; 80 | valid = true; 81 | alpha = 1.0 - clamp(conf_threshold - c, 0.1, 1); 82 | } 83 | 84 | color = vec4(surfel_color, alpha); 85 | } 86 | 87 | valid = valid && (!backface_culling || (dot(view_dir, n.xyz) > 0)); 88 | 89 | if(drawCurrentSurfelsOnly) valid = (timestamp == surfel_timestamp); 90 | 91 | if(!valid) gl_Position = vec4(-10, -10, -10, 1.0f); 92 | } -------------------------------------------------------------------------------- /src/shader/draw_surfels.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | uniform int surfelDrawMethod; // 0 - texCoord-based fragement discard, 2 - hexagon based. 4 | 5 | in vec2 texCoords; 6 | in vec4 gs_color; 7 | out vec4 color; 8 | in float radius; 9 | 10 | // phong shading? 11 | 12 | void main() 13 | { 14 | if(surfelDrawMethod == 0) 15 | { 16 | if(dot(texCoords, texCoords) > 1.0f) discard; 17 | color = gs_color; 18 | } 19 | else 20 | { 21 | color = gs_color; 22 | } 23 | } -------------------------------------------------------------------------------- /src/shader/draw_surfels.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position_radius; 4 | layout (location = 1) in vec4 normal_confidence; 5 | layout (location = 2) in int timestamp; 6 | layout (location = 3) in vec3 surfel_color_weight_count; 7 | 8 | uniform mat4 mvp; 9 | uniform mat4 mvp_inv_t; 10 | 11 | uniform samplerBuffer poseBuffer; 12 | 13 | out SURFEL 14 | { 15 | float radius; 16 | vec4 normal; 17 | float confidence; 18 | int timestamp; 19 | vec3 color; 20 | } vs_out; 21 | 22 | #include "shader/color.glsl" 23 | 24 | mat4 get_pose(int timestamp) 25 | { 26 | int offset = 4 * timestamp; 27 | return mat4(texelFetch(poseBuffer, offset), texelFetch(poseBuffer, offset + 1), 28 | texelFetch(poseBuffer, offset + 2), texelFetch(poseBuffer, offset+3)); 29 | } 30 | 31 | void main() 32 | { 33 | mat4 surfelPose = get_pose(int(surfel_color_weight_count.z)); 34 | 35 | // projection etc is applied in geometry shader. 36 | gl_Position = surfelPose * vec4(position_radius.xyz, 1.0f); 37 | vs_out.radius = position_radius.w; 38 | vs_out.normal = surfelPose * vec4(normal_confidence.xyz, 0.0f); 39 | vs_out.confidence = normal_confidence.w; 40 | vs_out.timestamp = timestamp; 41 | vs_out.color = unpack(surfel_color_weight_count.x); 42 | } -------------------------------------------------------------------------------- /src/shader/draw_surfels_coords.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(line_strip, max_vertices = 6) out; 5 | 6 | in SURFEL 7 | { 8 | float radius; 9 | vec4 normal; 10 | float confidence; 11 | } gs_in[]; 12 | 13 | uniform mat4 mvp; 14 | uniform mat4 mvp_inv_t; 15 | 16 | out vec4 color; 17 | 18 | #include "shader/color.glsl" 19 | 20 | void main() 21 | { 22 | 23 | vec4 p = mvp * gl_in[0].gl_Position; 24 | vec4 n = gs_in[0].normal; 25 | vec4 u = normalize(vec4(n.y - n.z, -n.x, n.x, 0.0f)); 26 | vec4 v = vec4(normalize(cross(n.xyz, u.xyz)), 0.0f); 27 | 28 | p += 0.01 * mvp_inv_t * n; // shift slightly along the normal to avoid "z-fighting" 29 | 30 | color = RED; 31 | gl_Position = p; 32 | EmitVertex(); 33 | 34 | gl_Position = p + 0.05 * mvp_inv_t * u; 35 | EmitVertex(); 36 | 37 | EndPrimitive(); 38 | 39 | color = GREEN; 40 | gl_Position = p ; 41 | EmitVertex(); 42 | 43 | gl_Position = p + 0.05 * mvp_inv_t * v; 44 | EmitVertex(); 45 | 46 | EndPrimitive(); 47 | 48 | 49 | color = BLUE; 50 | gl_Position = p; 51 | EmitVertex(); 52 | 53 | gl_Position = p + 0.05 * mvp_inv_t * n; 54 | EmitVertex(); 55 | 56 | EndPrimitive(); 57 | 58 | 59 | } -------------------------------------------------------------------------------- /src/shader/draw_vertexmap3d.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec2 tex_position; 4 | 5 | uniform sampler2DRect texVertexMap; 6 | uniform sampler2DRect texResidualMap; 7 | uniform mat4 mvp; 8 | 9 | uniform int colorMode; // 0 - depth grey, 1 - hsv, 2 - red/depth, 3 - residual, 4 - outlier 10 | uniform vec4 customColor; 11 | 12 | out vec4 color; 13 | 14 | // source: http://lolengine.net/blog/2013/07/27/rgb-to-hsv-in-glsl 15 | vec3 hsv2rgb(vec3 c) 16 | { 17 | vec4 K = vec4(1.0, 2.0 / 3.0, 1.0 / 3.0, 3.0); 18 | vec3 p = abs(fract(c.xxx + K.xyz) * 6.0 - K.www); 19 | 20 | return c.z * mix(K.xxx, clamp(p - K.xxx, 0.0, 1.0), c.y); 21 | } 22 | 23 | void main() 24 | { 25 | vec4 coords = vec4(texture(texVertexMap, tex_position).rgb, 1.0); 26 | vec4 residuals = texture(texResidualMap, tex_position).rgba; 27 | float depth = length(coords.xyz)/50.0f; 28 | 29 | gl_Position = mvp * coords; 30 | 31 | color = vec4(0.0f, 0.0f, 0.0f, 1.0f); 32 | 33 | if(colorMode == 0) color = vec4(depth, depth, depth, 1.0); 34 | else if(colorMode == 1) color = vec4(hsv2rgb(vec3(depth, 1.0, 1.0)), 1.0); 35 | else if(colorMode == 2) color = vec4(depth, 0, 0, 1.0); 36 | else if(colorMode == 3) color = vec4(hsv2rgb(vec3(abs(residuals.g), 1.0, 1.0)), 1.0); 37 | else if(colorMode == 4) { 38 | if(int(residuals.a) < 0.5) color = vec4(1,0,0,1); 39 | else color = vec4(0,1,0,1); 40 | } 41 | else if(colorMode == 5) 42 | { 43 | color = vec4(hsv2rgb(vec3(residuals.b, 1.0, 1.0)), 1.0); 44 | } 45 | else if(colorMode == 6) 46 | { 47 | if(residuals.a < -2.0) color = vec4(1,0,0,1); 48 | else color = vec4(0,1,0,1); 49 | } 50 | } -------------------------------------------------------------------------------- /src/shader/empty.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | 4 | void main() 5 | { 6 | 7 | } -------------------------------------------------------------------------------- /src/shader/empty.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | 4 | void main() 5 | { 6 | 7 | } -------------------------------------------------------------------------------- /src/shader/extract_surfels.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position_radius; 4 | layout (location = 1) in vec4 normal_confidence; 5 | layout (location = 2) in int in_timestamp; 6 | layout (location = 3) in vec3 surfel_color_weight_count; 7 | 8 | uniform samplerBuffer poseBuffer; 9 | uniform vec2 submap_center; 10 | uniform float submap_extent; 11 | 12 | out SURFEL 13 | { 14 | bool valid; 15 | vec4 position_radius; 16 | vec4 normal_confidence; 17 | int timestamp; 18 | vec3 color_weight_count; 19 | } vs_out; 20 | 21 | mat4 get_pose(int timestamp) 22 | { 23 | int offset = 4 * timestamp; 24 | return mat4(texelFetch(poseBuffer, offset), texelFetch(poseBuffer, offset + 1), 25 | texelFetch(poseBuffer, offset + 2), texelFetch(poseBuffer, offset+3)); 26 | } 27 | 28 | mat4 inverse_pose(mat4 pose) 29 | { 30 | mat3 R = transpose(mat3(pose)); 31 | vec3 t = vec3(pose[3]); 32 | mat4 result = mat4(R); 33 | result[3] = vec4(-R*t, 1); 34 | 35 | return result; 36 | } 37 | 38 | void main() 39 | { 40 | mat4 surfelPose = get_pose(int(surfel_color_weight_count.z)); 41 | vec4 position = surfelPose * vec4(position_radius.xyz, 1.0); 42 | 43 | if(abs(position.x - submap_center.x) > submap_extent || abs(position.y - submap_center.y) > submap_extent) 44 | { 45 | vs_out.valid = false; 46 | } 47 | else 48 | { 49 | vs_out.valid = true; 50 | vs_out.position_radius = position_radius; 51 | vs_out.normal_confidence = normal_confidence; 52 | vs_out.timestamp = in_timestamp; 53 | vs_out.color_weight_count = surfel_color_weight_count; 54 | } 55 | } -------------------------------------------------------------------------------- /src/shader/gen_depthimg.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | flat in vec2 depth_index; 4 | flat in vec4 vertex_coord; 5 | flat in vec4 image_coord; 6 | 7 | layout (location = 0) out float color; // depth image. 8 | layout (location = 1) out int index; // index map. 9 | layout (location = 2) out vec4 vertex; // vertex map. 10 | layout (location = 3) out int valid; // validity map. 11 | layout (location = 4) out vec4 out_img_coord; // image coordinates 12 | 13 | void main() 14 | { 15 | color = depth_index.x; 16 | index = int(depth_index.y); 17 | vertex = vertex_coord; 18 | valid = 1; 19 | out_img_coord = image_coord; 20 | } -------------------------------------------------------------------------------- /src/shader/gen_depthimg.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position; // x, y, z, 1 4 | 5 | flat out vec2 depth_index; // depth, index of point. 6 | flat out vec4 image_coord; // float pixel coords x, y, depth. 7 | flat out vec4 vertex_coord; // coordinate of the vertex producing this pixel. 8 | flat out vec4 ndc_coord; 9 | 10 | const float pi = 3.14159265358979323846f; 11 | const float inv_pi = 0.31830988618379067154f; 12 | const float pi_2 = 1.57079632679; 13 | 14 | // Velodyne HDL-64E: 26.8 vertical fov (+2 <-> -24.8) 15 | uniform float fov_up; 16 | uniform float fov_down; 17 | 18 | // params: min_depth, max_depth? 19 | uniform bool perform_rounding; 20 | uniform int width; 21 | uniform int height; 22 | 23 | uniform float min_depth; 24 | uniform float max_depth; 25 | 26 | void main() 27 | { 28 | float fov = abs(fov_up) + abs(fov_down); 29 | float depth = length(position.xyz); 30 | float yaw = atan(position.y, position.x); 31 | float pitch = -asin(position.z / depth); // angle = acos((0,0,1) * p/||p||) - pi/2 = pi/2 - asin(x) + pi/2 32 | 33 | float x = (-yaw * inv_pi); // in [-1, 1] 34 | float y = (1.0 - 2.0 * (degrees(pitch) + fov_up) / fov); // in [-1, 1] 35 | float z = 2.0f * ((depth - min_depth) / (max_depth - min_depth)) - 1.0f; // in [-1, 1] 36 | 37 | float x_img = 0.5 * ((-yaw * inv_pi) + 1.0); // in [0, 1] 38 | float y_img = (1.0 - ((degrees(pitch) + fov_up) / fov)); // in [0, 1] 39 | 40 | // force that each point lies exactly inside the texel enabling reproducible results. 41 | x = 2.0f * ((floor(0.5f * (x + 1.0f) * width ) + 0.5f ) / width) - 1.0; 42 | y = 2.0f * ((floor(0.5f * (y + 1.0f) * height ) + 0.5f ) / height) - 1.0; 43 | 44 | 45 | gl_Position = vec4(x, y, z, 1.0); 46 | 47 | depth_index = vec2(depth, gl_VertexID); 48 | ndc_coord = vec4(x, y, z, 1.0f); 49 | image_coord = vec4(x_img, y_img, 0, 1.0f); 50 | vertex_coord = vec4(position.xyz, 1.0); 51 | } -------------------------------------------------------------------------------- /src/shader/gen_indexmap.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | flat in float index; 4 | in vec4 vertex; 5 | in vec4 normal; 6 | 7 | layout (location = 0) out float indexmap; 8 | layout (location = 1) out vec4 vertexmap; 9 | layout (location = 2) out vec4 normalmap; 10 | 11 | void main() 12 | { 13 | indexmap = index; 14 | vertexmap = vertex; 15 | normalmap = normal; 16 | } -------------------------------------------------------------------------------- /src/shader/gen_indexmap.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 surfel_position_radius; 4 | layout (location = 1) in vec4 surfel_normal_confidence; 5 | layout (location = 2) in int surfel_timestamp; 6 | layout (location = 3) in vec3 surfel_color_weight_count; 7 | 8 | 9 | flat out float index; 10 | out vec4 vertex; 11 | out vec4 normal; 12 | 13 | const vec2 halfpix = vec2(0.5f, 0.5f); 14 | const float sqrt2 = 1.41421356237f; 15 | const float pi = 3.14159265358979323846f; 16 | const float inv_pi = 0.31830988618379067154f; 17 | const float pi_2 = 1.57079632679; 18 | 19 | uniform mat4 pose; 20 | uniform mat4 inv_pose; 21 | 22 | uniform float fov_up; 23 | uniform float fov_down; 24 | uniform float max_depth; 25 | uniform float min_depth; 26 | uniform float width; 27 | uniform float height; 28 | 29 | uniform samplerBuffer poseBuffer; 30 | 31 | vec3 projectSpherical(vec4 position) 32 | { 33 | float fov = abs(fov_up) + abs(fov_down); 34 | float depth = length(position.xyz); 35 | float yaw = atan(position.y, position.x); 36 | float pitch = -asin(position.z / depth); 37 | 38 | float x = 0.5 * ((-yaw * inv_pi) + 1.0); // in [0, 1] 39 | float y = (1.0 - (degrees(pitch) + fov_up) / fov); // in [0, 1] 40 | float z = (depth - min_depth) / (max_depth - min_depth); // in [0, 1] 41 | 42 | x = (floor(x * width) + 0.5) / width; 43 | y = (floor(y * height) + 0.5) / height; 44 | 45 | return vec3(x, y, z); 46 | } 47 | 48 | mat4 get_pose(int timestamp) 49 | { 50 | int offset = 4 * timestamp; 51 | return mat4(texelFetch(poseBuffer, offset), texelFetch(poseBuffer, offset + 1), 52 | texelFetch(poseBuffer, offset + 2), texelFetch(poseBuffer, offset+3)); 53 | } 54 | 55 | /** \brief depth-buffered index map. **/ 56 | void main() 57 | { 58 | 59 | mat4 surfelPose = get_pose(int(surfel_color_weight_count.z)); 60 | 61 | vertex = inv_pose * surfelPose * vec4(surfel_position_radius.xyz, 1.0); 62 | normal = inv_pose * surfelPose * vec4(surfel_normal_confidence.xyz, 0.0); 63 | 64 | vec4 p = vertex; 65 | vec4 n = normal; 66 | 67 | gl_Position = vec4(-10.0); 68 | 69 | // visible from current position. 70 | if(dot(n.xyz, -p.xyz/length(p.xyz)) > 0.01) 71 | { 72 | gl_Position = vec4(2.0 * projectSpherical(p) - vec3(1.0), 1.0); 73 | index = gl_VertexID + 1; // initially all index pixels are 0. 74 | } 75 | } -------------------------------------------------------------------------------- /src/shader/gen_normalmap.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec2 texCoords; 4 | 5 | uniform int width; 6 | uniform int height; 7 | 8 | uniform sampler2DRect vertex_map; 9 | 10 | layout (location = 0) out vec4 normal; 11 | 12 | uniform int normal_radius; 13 | 14 | float wrap(float x, float dim) 15 | { 16 | float value = x; 17 | 18 | while(value >= dim) value = (value - dim); 19 | while(value < 0) value = (value + dim); 20 | 21 | return value; 22 | } 23 | 24 | bool valid(vec4 normal) 25 | { 26 | return (normal.w > 0.5); 27 | } 28 | 29 | void main() 30 | { 31 | float width = textureSize(vertex_map).x; 32 | vec2 pos = texCoords * textureSize(vertex_map); 33 | normal = vec4(0.0, 0.0, 0.0, 0.0); 34 | 35 | if(texture(vertex_map, pos).w > 0.0f) 36 | { 37 | normal.w = 1.0f; 38 | 39 | vec4 p = texture(vertex_map, pos); 40 | vec4 u = texture(vertex_map, vec2(wrap(pos.x + 1, width), pos.y)); 41 | vec4 v = texture(vertex_map, vec2(pos.x, pos.y + 1)); 42 | vec4 s = texture(vertex_map, vec2(wrap(pos.x - 1, width), pos.y)); 43 | vec4 t = texture(vertex_map, vec2(pos.x, pos.y - 1)); 44 | 45 | u.xyz = normalize(u.xyz - p.xyz); 46 | v.xyz = normalize(v.xyz - p.xyz); 47 | s.xyz = normalize(p.xyz - s.xyz); 48 | t.xyz = normalize(p.xyz - t.xyz); 49 | 50 | if(u.w < 1.0f && v.w < 1.0f) normal.w = 0; 51 | if(s.w < 1.0f && t.w < 1.0f) normal.w = 0; 52 | 53 | 54 | if(!valid(u) || !valid(v)) normal.w = 0; 55 | 56 | 57 | // TODO: check if distances in x/y-direction are similar. 58 | //if(abs(length(u) - length(s)) / max(length(u), length(s)) > 0.5) normal.w = 0; 59 | //if(abs(length(v) - length(t)) / max(length(t), length(v)) > 0.5) normal.w = 0; 60 | 61 | if(normal.w > 0.0f) 62 | { 63 | vec3 w = cross(u.xyz, v.xyz); 64 | float len = length(w); 65 | normal = vec4(w / len, 1.0); 66 | normal.w = int(len > 0.0000001); 67 | } 68 | } 69 | } -------------------------------------------------------------------------------- /src/shader/gen_surfels.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) out vec4 upd_vertex_radius; 4 | layout (location = 1) out vec4 upd_normal_confidence; 5 | 6 | in vec4 sfl_position_radius; 7 | in vec4 sfl_normal_confidence; 8 | in int sfl_timestamp; 9 | 10 | void main() 11 | { 12 | // store information for update. 13 | upd_vertex_radius = sfl_position_radius; 14 | upd_normal_confidence = sfl_normal_confidence; 15 | } -------------------------------------------------------------------------------- /src/shader/gen_surfels.geom: -------------------------------------------------------------------------------- 1 | #version 400 core 2 | 3 | #include "shader/color.glsl" 4 | 5 | layout(points) in; 6 | layout(points, max_vertices = 1) out; 7 | 8 | uniform int timestamp; 9 | 10 | in SURFEL { 11 | bool valid; 12 | vec2 img_coords; 13 | } gs_in[]; 14 | 15 | out vec4 sfl_position_radius; 16 | out vec4 sfl_normal_confidence; 17 | out int sfl_timestamp; 18 | out vec3 sfl_color_weight_count; 19 | 20 | uniform sampler2DRect vertex_map; 21 | uniform sampler2DRect normal_map; 22 | uniform sampler2DRect radiusConfidence_map; 23 | 24 | uniform mat4 pose; // current pose of laser scanner. 25 | uniform float log_prior; 26 | 27 | 28 | void main() 29 | { 30 | // filter invalid surfels. 31 | if(gs_in[0].valid) 32 | { 33 | vec2 img_coords = gs_in[0].img_coords; 34 | 35 | vec4 v = vec4(texture(vertex_map, img_coords).xyz, 1.0); 36 | vec4 n = vec4(texture(normal_map, img_coords).xyz, 0.0); 37 | vec4 v_global = v; 38 | vec4 n_global = normalize(n); 39 | float radius = texture(radiusConfidence_map, img_coords).x; 40 | 41 | float weight = 1.0f; 42 | 43 | // all surfels are outputed to the transform feedback buffer. 44 | sfl_position_radius = vec4(v_global.xyz, radius); 45 | sfl_normal_confidence = vec4(n_global.xyz, log_prior); 46 | sfl_timestamp = timestamp; 47 | sfl_color_weight_count = vec3(pack(vec3(0,0,1)), weight, timestamp); 48 | 49 | EmitVertex(); 50 | EndPrimitive(); 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /src/shader/gen_surfels.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | #include "shader/color.glsl" 4 | 5 | layout (location = 0) in vec2 img_coords; // image coords = (x, y) in [0, w] x [0, h] 6 | 7 | // centerized vertex map, normal map and precomputed radius/confidence map. 8 | uniform sampler2DRect vertex_map; 9 | uniform sampler2DRect normal_map; 10 | uniform sampler2DRect measurementIntegrated_map; 11 | uniform sampler2DRect radiusConfidence_map; 12 | 13 | uniform int timestamp; 14 | uniform mat4 pose; // current pose of laser scanner. 15 | uniform float log_prior; 16 | 17 | out SURFEL { 18 | bool valid; 19 | vec2 img_coords; 20 | } vs_out; 21 | 22 | void main() 23 | { 24 | vec3 vertex = texture(vertex_map, img_coords).xyz; 25 | vec3 normal = texture(normal_map, img_coords).xyz; 26 | 27 | bool invalid = (texture(vertex_map, img_coords).w < 1.0f) || (texture(normal_map, img_coords).w < 1.0f); 28 | invalid = invalid || (texture(radiusConfidence_map, img_coords).w < 0.5f); 29 | bool integrated = texture(measurementIntegrated_map, img_coords).x > 0.5; 30 | 31 | vec3 view_dir = -vertex / length(vertex); 32 | 33 | // vertex/normal invalid or already integrated or normal pointing in the wrong direction. 34 | vs_out.valid = (!invalid && !integrated && (dot(normal, view_dir) > 0.01)); 35 | vs_out.img_coords = img_coords; 36 | } -------------------------------------------------------------------------------- /src/shader/gen_surfels330.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | #include "shader/color.glsl" 4 | 5 | layout(points) in; 6 | layout(points, max_vertices = 2) out; 7 | 8 | uniform int timestamp; 9 | 10 | in SURFEL { 11 | bool valid; 12 | vec2 img_coords; 13 | } gs_in[]; 14 | 15 | out vec4 sfl_position_radius; 16 | out vec4 sfl_normal_confidence; 17 | out int sfl_timestamp; 18 | out vec3 sfl_color_weight_count; 19 | 20 | uniform sampler2DRect vertex_map; 21 | uniform sampler2DRect normal_map; 22 | uniform sampler2DRect radiusConfidence_map; 23 | 24 | uniform mat4 pose; // current pose of laser scanner. 25 | uniform float log_prior; 26 | uniform float min_radius; 27 | uniform float max_radius; 28 | 29 | // fill-in for OpenGL 3.30: replacing one texture gather with 4 texture accesses. 30 | vec4 textureGather(sampler2DRect sampler, vec2 coords, int comp) 31 | { 32 | vec4 result; 33 | // Note: documentation is somehow incoherent here, but ARB shows this order: 34 | if(comp == 0) 35 | { 36 | result.x = texture(sampler, vec2(coords.x, coords.y)).x; 37 | result.y = texture(sampler, vec2(coords.x + 1, coords.y)).x; 38 | result.z = texture(sampler, vec2(coords.x + 1, coords.y - 1)).x; 39 | result.w = texture(sampler, vec2(coords.x, coords.y - 1)).x; 40 | } 41 | else if(comp == 1) 42 | { 43 | result.x = texture(sampler, vec2(coords.x, coords.y)).y; 44 | result.y = texture(sampler, vec2(coords.x + 1, coords.y)).y; 45 | result.z = texture(sampler, vec2(coords.x + 1, coords.y - 1)).y; 46 | result.w = texture(sampler, vec2(coords.x, coords.y - 1)).y; 47 | } 48 | else if(comp == 2) 49 | { 50 | result.x = texture(sampler, vec2(coords.x, coords.y)).z; 51 | result.y = texture(sampler, vec2(coords.x + 1, coords.y)).z; 52 | result.z = texture(sampler, vec2(coords.x + 1, coords.y - 1)).z; 53 | result.w = texture(sampler, vec2(coords.x, coords.y - 1)).z; 54 | } 55 | else if(comp == 3) 56 | { 57 | result.x = texture(sampler, vec2(coords.x, coords.y)).w; 58 | result.y = texture(sampler, vec2(coords.x + 1, coords.y)).w; 59 | result.z = texture(sampler, vec2(coords.x + 1, coords.y - 1)).w; 60 | result.w = texture(sampler, vec2(coords.x, coords.y - 1)).w; 61 | } 62 | 63 | return result; 64 | } 65 | 66 | void main() 67 | { 68 | // filter invalid surfels. 69 | if(gs_in[0].valid) 70 | { 71 | vec2 img_coords = gs_in[0].img_coords; 72 | 73 | vec4 v = vec4(texture(vertex_map, img_coords).xyz, 1.0); 74 | vec4 n = vec4(texture(normal_map, img_coords).xyz, 0.0); 75 | vec4 v_global = v; 76 | vec4 n_global = normalize(n); 77 | float radius = texture(radiusConfidence_map, img_coords).x; 78 | float weight = 1.0f; 79 | 80 | // all surfels are outputed to the transform feedback buffer. 81 | sfl_position_radius = vec4(v_global.xyz, radius); 82 | sfl_normal_confidence = vec4(n_global.xyz, log_prior); 83 | sfl_timestamp = timestamp; 84 | sfl_color_weight_count = vec3(pack(vec3(0,0,1)), weight, timestamp); 85 | 86 | EmitVertex(); 87 | EndPrimitive(); 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /src/shader/gen_vertexmap.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 vertex_coord; 4 | out vec4 color; 5 | 6 | void main() 7 | { 8 | color = vertex_coord; 9 | } -------------------------------------------------------------------------------- /src/shader/gen_vertexmap.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position; // x, y, z, 1 4 | 5 | out vec4 vertex_coord; // coordinate of the vertex producing this pixel. 6 | 7 | const float pi = 3.14159265358979323846f; 8 | const float inv_pi = 0.31830988618379067154f; 9 | const float pi_2 = 1.57079632679; 10 | 11 | // Velodyne HDL-64E: 26.8 vertical fov (+2 <-> -24.8) 12 | uniform float width; 13 | uniform float height; 14 | uniform float fov_up; 15 | uniform float fov_down; 16 | uniform float min_depth; 17 | uniform float max_depth; 18 | 19 | void main() 20 | { 21 | float fov = abs(fov_up) + abs(fov_down); 22 | float depth = length(position.xyz); 23 | float yaw = atan(position.y, position.x); 24 | float pitch = -asin(position.z / depth); // angle = acos((0,0,1) * p/||p||) - pi/2 = pi/2 - asin(x) + pi/2 25 | 26 | float x = (-yaw * inv_pi); // in [-1, 1] 27 | float y = (1.0 - 2.0 * (degrees(pitch) + fov_up) / fov); // in [-1, 1] 28 | float z = 2.0f * ((depth - min_depth) / (max_depth - min_depth)) - 1.0f; // in [-1, 1] 29 | 30 | // force that each point lies exactly inside the texel enabling reproducible results. 31 | x = 2.0f * ((floor(0.5f * (x + 1.0f) * width ) + 0.5f ) / width) - 1.0; 32 | y = 2.0f * ((floor(0.5f * (y + 1.0f) * height ) + 0.5f ) / height) - 1.0; 33 | 34 | 35 | 36 | gl_Position = vec4(x, y, z, 1.0); 37 | vertex_coord = vec4(position.xyz, 1.0); 38 | } 39 | -------------------------------------------------------------------------------- /src/shader/init_depthimg.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | flat in vec2 depth_index; 4 | flat in vec3 vertex_coord; 5 | 6 | layout (location = 0) out float depth; // depth image. 7 | layout (location = 1) out int index; // index map. 8 | layout (location = 2) out vec3 vertex; // vertex map. 9 | layout (location = 3) out int valid; // validity map. 10 | 11 | void main() 12 | { 13 | depth = -1.0; 14 | index = -1; 15 | vertex = vec3(1./0.); 16 | valid = 0; 17 | } -------------------------------------------------------------------------------- /src/shader/init_radiusConf.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in float valid; 4 | in vec4 centerized_vertex; 5 | in float radius; 6 | in float confidence; 7 | 8 | layout (location = 0) out vec4 centerized_vertex_map; 9 | layout (location = 1) out vec4 radius_confidence_map; 10 | 11 | 12 | void main() 13 | { 14 | centerized_vertex_map = centerized_vertex; 15 | radius_confidence_map = vec4(radius, confidence, 0, valid); 16 | } -------------------------------------------------------------------------------- /src/shader/init_radiusConf.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec2 img_coords; // image coords = (x, y) in [0, w] x [0, h] 4 | 5 | uniform sampler2DRect vertex_map; 6 | uniform sampler2DRect normal_map; 7 | 8 | uniform float fov_up; 9 | uniform float fov_down; 10 | uniform float max_depth; 11 | uniform float min_depth; 12 | uniform float pixel_size; 13 | uniform int confidence_mode; 14 | uniform float min_radius; 15 | uniform float max_radius; 16 | uniform float angle_thresh; 17 | 18 | 19 | const vec2 halfpix = vec2(0.5f, 0.5f); 20 | const float sqrt2 = 1.41421356237f; 21 | const float pi = 3.14159265358979323846f; 22 | const float inv_pi = 0.31830988618379067154f; 23 | const float pi_2 = 1.57079632679; 24 | 25 | out float valid; 26 | out vec4 centerized_vertex; 27 | out float radius; 28 | out float confidence; 29 | 30 | float get_radius(vec4 vertex, vec4 normal, float pixel_size) 31 | { 32 | float d = length(vertex.xyz); 33 | 34 | return 1.41 * d * pixel_size / clamp(dot(normal.xyz, -vertex.xyz / d), 0.5, 1.0); 35 | } 36 | 37 | void main() 38 | { 39 | vec2 tex_dim = textureSize(vertex_map); 40 | 41 | gl_Position = vec4(2.0 * img_coords / tex_dim - vec2(1.0), 0, 1); 42 | valid = 0.0; 43 | radius = 0.0; 44 | confidence = 0.0; 45 | 46 | vec4 vertex = texture(vertex_map, img_coords); 47 | vec4 normal = texture(normal_map, img_coords); 48 | vec3 view_dir = -vertex.xyz / length(vertex.xyz); 49 | 50 | float angle = dot(normal.xyz, view_dir); 51 | 52 | if(vertex.w > 0.5 && normal.w > 0.5 && angle > angle_thresh) 53 | { 54 | valid = 1.0; 55 | 56 | 57 | 58 | float angle = 1.0 - clamp(dot(normal.xyz, view_dir), 0, 1); 59 | 60 | float confidence = 1.0f; 61 | 62 | centerized_vertex = vertex; 63 | radius = get_radius(vertex, normal, pixel_size); 64 | radius = min(max(radius, min_radius), max_radius); 65 | } 66 | } -------------------------------------------------------------------------------- /src/shader/laserscan.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 point_color; 4 | out vec4 color; 5 | 6 | void main() 7 | { 8 | color = point_color; 9 | } 10 | -------------------------------------------------------------------------------- /src/shader/laserscan.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position; 4 | layout (location = 1) in float remission; 5 | 6 | uniform mat4 mvp; 7 | uniform int pointColorMode; // 0 - black, 1 - remission, 2 - depth, 3 - custom color, 4 - remission in w coordinate. 8 | uniform vec4 customColor; 9 | uniform bool removeGround; 10 | 11 | out vec4 point_color; 12 | 13 | #include "shader/color.glsl" 14 | 15 | void main() 16 | { 17 | if(removeGround) 18 | { 19 | if(position.z < -1.0) gl_Position = vec4(-10, -10, -10, 1); 20 | else gl_Position = mvp * vec4(position.xyz, 1.0); 21 | } 22 | else 23 | { 24 | gl_Position = mvp * vec4(position.xyz, 1.0); 25 | } 26 | 27 | if(pointColorMode == 1) 28 | { 29 | point_color = vec4(remission, remission, remission, 1.0f); 30 | } 31 | else if(pointColorMode == 2) 32 | { 33 | float depth = length(vec3(position.x, position.y, position.z)); 34 | vec3 hsv = hsv2rgb(vec3(depth/50.0, 1.0, 1.0)); 35 | point_color = vec4(hsv, 1.0); 36 | } 37 | else if(pointColorMode == 3) 38 | { 39 | //vec3 hsv = rgb2hsv(customColor.xyz); 40 | point_color = customColor;//vec4(hsv2rgb(vec3(hsv.x, 1.0, remission)), 1.0f); 41 | } 42 | else if(pointColorMode == 4) 43 | { 44 | point_color = vec4(position.w, position.w, position.w, 1.0f); 45 | } 46 | else if(pointColorMode == 5) 47 | { 48 | vec3 hsv = rgb2hsv(customColor.xyz); 49 | point_color = vec4(hsv2rgb(vec3(hsv.x, 1.0, max(2*position.w, 0.5))), 1.0f); 50 | } 51 | else if(pointColorMode == 6) 52 | { 53 | vec3 hsv = rgb2hsv(customColor.xyz); 54 | point_color = vec4(hsv2rgb(vec3(hsv.x, 1.0, max(remission, 0.3))), 1.0f); 55 | } 56 | else if(pointColorMode == 7) 57 | { 58 | point_color = vec4(hsv2rgb(vec3(position.z / 10.0, 1.0, max(2*position.w, 0.5))), 1.0f); 59 | } 60 | else 61 | { 62 | point_color = vec4(0.0f, 0.0f, 0.0f, 1.0f); 63 | } 64 | 65 | 66 | } -------------------------------------------------------------------------------- /src/shader/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/shader/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/shader/render_compose.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | 5 | layout (location = 0) out vec4 vertexmap; 6 | layout (location = 1) out vec4 normalmap; 7 | 8 | 9 | 10 | uniform sampler2DRect old_vertexmap; 11 | uniform sampler2DRect old_normalmap; 12 | uniform sampler2DRect new_vertexmap; 13 | uniform sampler2DRect new_normalmap; 14 | 15 | uniform float max_distance; // maximum distance of old vertices to consider. 16 | 17 | 18 | void main() 19 | { 20 | vec2 dim = textureSize(new_vertexmap); 21 | vec2 img_coords = texCoords * dim; 22 | 23 | vertexmap = texture(new_vertexmap, img_coords); 24 | normalmap = texture(new_normalmap, img_coords); 25 | 26 | vec4 old_vertex = texture(old_vertexmap, img_coords); 27 | vec4 old_normal = texture(old_normalmap, img_coords); 28 | 29 | bool valid = (old_vertex.w > 0.5f && old_normal.w > 0.5f); 30 | bool new_valid = (vertexmap.w > 0.5f && normalmap.w > 0.5f); 31 | 32 | if(!new_valid && valid && (vertexmap.w < 0.5f || length(vertexmap.xyz - old_vertex.xyz) < max_distance)) 33 | { 34 | vertexmap = old_vertex; 35 | normalmap = old_normal; 36 | } 37 | } -------------------------------------------------------------------------------- /src/shader/render_surfels.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | in vec4 vertex; 5 | in vec4 normal; 6 | in float confidence; 7 | 8 | layout (location = 0) out vec4 vertexmap; 9 | layout (location = 1) out vec4 normalmap; 10 | 11 | void main() 12 | { 13 | 14 | if(dot(texCoords, texCoords) > 1.0f) 15 | { 16 | vertexmap = vec4(0); 17 | normalmap = vec4(0); 18 | // indexmap = 0; 19 | discard; 20 | } 21 | 22 | vertexmap = vec4(vertex.xyz, 1.0); 23 | normalmap = normal; 24 | // indexmap = index; 25 | 26 | } -------------------------------------------------------------------------------- /src/shader/render_surfels.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(triangle_strip, max_vertices = 6) out; 5 | 6 | in SURFEL 7 | { 8 | vec4 position; 9 | float radius; 10 | vec4 normal; 11 | float confidence; 12 | int index; 13 | int timestamp; 14 | int creation_timestamp; 15 | } gs_in[]; 16 | 17 | out vec2 texCoords; 18 | out vec4 normal; 19 | out vec4 vertex; 20 | out float confidence; 21 | 22 | uniform float fov_up; 23 | uniform float fov_down; 24 | uniform float max_depth; 25 | uniform float min_depth; 26 | 27 | uniform float conf_threshold; 28 | uniform int timestamp_threshold; 29 | uniform bool render_old_surfels; 30 | uniform bool use_stability; 31 | 32 | const float pi = 3.14159265358979323846f; 33 | const float inv_pi = 0.31830988618379067154f; 34 | const float pi_2 = 1.57079632679; 35 | 36 | 37 | vec3 project2model(vec4 position) 38 | { 39 | float fov = abs(fov_up) + abs(fov_down); 40 | float depth = length(position.xyz); 41 | float yaw = atan(position.y, position.x); 42 | float pitch = -asin(position.z / depth); 43 | 44 | float x = 0.5 * ((-yaw * inv_pi) + 1.0); // in [0, 1] 45 | float y = (1.0 - (degrees(pitch) + fov_up) / fov); // in [0, 1] 46 | float z = (depth - min_depth) / (max_depth - min_depth); // in [0, 1] 47 | 48 | return vec3(x, y, z); 49 | } 50 | 51 | vec3 project2model(vec4 position, vec3 center_pixel) 52 | { 53 | float fov = abs(fov_up) + abs(fov_down); 54 | float depth = length(position.xyz); 55 | float yaw = atan(position.y, position.x); 56 | float pitch = -asin(position.z / depth); 57 | 58 | float x = 0.5 * ((-yaw * inv_pi) + 1.0); // in [0, 1] 59 | // FIXME: find better solution for wrap around: 60 | if(center_pixel.x - x > 0.5) x += 1.0; 61 | if(x - center_pixel.x > 0.5) x -= 1.0; 62 | float y = (1.0 - (degrees(pitch) + fov_up) / fov); // in [0, 1] 63 | float z = (depth - min_depth) / (max_depth - min_depth); // in [0, 1] 64 | 65 | return vec3(x, y, z); 66 | } 67 | 68 | void main() 69 | { 70 | vec4 p = gs_in[0].position; 71 | vec4 n = gs_in[0].normal; 72 | vec4 u = normalize(vec4(n.y - n.z, -n.x, n.x, 0.0f)); 73 | vec4 v = vec4(normalize(cross(n.xyz, u.xyz)), 0.0f); 74 | float r = gs_in[0].radius; 75 | 76 | bool visible = (dot(n.xyz, -p.xyz / length(p.xyz)) > 0.01); 77 | 78 | vec3 pp = project2model(p); 79 | if(visible && all(greaterThanEqual(pp, vec3(0))) && all(lessThan(pp, vec3(1))) && (!use_stability || gs_in[0].confidence > conf_threshold)) 80 | { 81 | //p_stable = 1.0 - 1.0 / (1.0 + exp(gs_in[0].confidence)); 82 | bool valid = (render_old_surfels && (gs_in[0].creation_timestamp < timestamp_threshold)); 83 | valid = valid || (!render_old_surfels && (gs_in[0].creation_timestamp >= timestamp_threshold || gs_in[0].timestamp >= timestamp_threshold)); 84 | 85 | if(valid) 86 | { 87 | vertex = vec4(p.xyz, 1.0f); 88 | normal = vec4(n.xyz, 1.0f); 89 | //index = gs_in[0].index + 1; 90 | confidence = gs_in[0].confidence; 91 | 92 | vec4 ru = r * u; 93 | vec4 rv = r * v; 94 | 95 | gl_Position = vec4(2.0 * project2model(p - ru - rv, pp) - 1.0, 1.0f); 96 | texCoords = vec2(-1.0, -1.0); 97 | EmitVertex(); 98 | 99 | gl_Position = vec4(2.0 * project2model(p + ru - rv, pp) - 1.0, 1.0f); 100 | texCoords = vec2(1.0, -1.0); 101 | EmitVertex(); 102 | 103 | gl_Position = vec4(2.0 * project2model(p - ru + rv, pp) - 1.0, 1.0f); 104 | texCoords = vec2(-1.0, 1.0); 105 | EmitVertex(); 106 | 107 | gl_Position = vec4(2.0 * project2model(p + ru + rv, pp) - 1.0, 1.0f); 108 | texCoords = vec2(1.0, 1.0); 109 | 110 | EmitVertex(); 111 | EndPrimitive(); 112 | } 113 | } 114 | } -------------------------------------------------------------------------------- /src/shader/render_surfels.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // \brief essentially the same as drawing, but with projected coordinates. 4 | 5 | // Replicate surfels on borders? 6 | 7 | layout (location = 0) in vec4 position_radius; 8 | layout (location = 1) in vec4 normal_confidence; 9 | layout (location = 2) in int timestamp; 10 | layout (location = 3) in vec3 surfel_color_weight_count; 11 | 12 | uniform mat4 inv_pose; 13 | 14 | uniform samplerBuffer poseBuffer; 15 | 16 | out SURFEL 17 | { 18 | vec4 position; 19 | float radius; 20 | vec4 normal; 21 | float confidence; 22 | int index; 23 | int timestamp; 24 | int creation_timestamp; 25 | } vs_out; 26 | 27 | mat4 get_pose(int t) 28 | { 29 | int offset = 4 * t; 30 | return mat4(texelFetch(poseBuffer, offset), texelFetch(poseBuffer, offset + 1), 31 | texelFetch(poseBuffer, offset + 2), texelFetch(poseBuffer, offset+3)); 32 | } 33 | 34 | void main() 35 | { 36 | mat4 surfelPose = get_pose(int(surfel_color_weight_count.z)); 37 | 38 | vs_out.position = inv_pose * surfelPose * vec4(position_radius.xyz, 1.0f); 39 | vs_out.radius = position_radius.w; 40 | vs_out.normal = inv_pose * surfelPose * vec4(normal_confidence.xyz, 0.0f); 41 | vs_out.confidence = normal_confidence.w; 42 | vs_out.index = gl_VertexID; 43 | vs_out.timestamp = timestamp; 44 | vs_out.creation_timestamp = int(surfel_color_weight_count.z); 45 | } -------------------------------------------------------------------------------- /src/shader/update_surfels.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) out vec4 color; 4 | 5 | void main() 6 | { 7 | color = vec4(1, 0, 0, 0); // integrated measurement. 8 | } -------------------------------------------------------------------------------- /src/shader/update_surfels.geom: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(points) in; 4 | layout(points, max_vertices = 1) out; 5 | 6 | in SURFEL 7 | { 8 | bool valid; 9 | vec4 position_radius; 10 | vec4 normal_confidence; 11 | int timestamp; 12 | vec3 color_weight_count; 13 | } gs_in[]; 14 | 15 | out vec4 sfl_position_radius; 16 | out vec4 sfl_normal_confidence; 17 | out int sfl_timestamp; 18 | out vec3 sfl_color_weight_count; 19 | 20 | void main() 21 | { 22 | if(gs_in[0].valid) 23 | { 24 | gl_Position = gl_in[0].gl_Position; 25 | 26 | sfl_position_radius = gs_in[0].position_radius; 27 | sfl_normal_confidence = gs_in[0].normal_confidence; 28 | sfl_timestamp = gs_in[0].timestamp; 29 | sfl_color_weight_count = gs_in[0].color_weight_count; 30 | 31 | EmitVertex(); 32 | EndPrimitive(); 33 | } 34 | } -------------------------------------------------------------------------------- /src/util/Log.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef LOG_H_ 3 | #define LOG_H_ 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rv { 13 | 14 | /** \brief logfile for storing intermediate results of experiments/evaluations. 15 | * 16 | * The idea is to have an XML representation of some interesting parameters, which 17 | * is accessible globally through a static member function. 18 | * 19 | * If no Log has been created with this specific name, we return a dummy logfile. 20 | * 21 | * \author behley. 22 | * 23 | */ 24 | class Log { 25 | public: 26 | ~Log(); 27 | 28 | /** non-copyable. **/ 29 | Log(const Log& other) = delete; 30 | Log& operator=(const Log& other) = delete; 31 | 32 | /** \brief close the log stream, and remove Log from global map. **/ 33 | void close(); 34 | 35 | /** 36 | * \brief starts an event and adds an Entry with the start timestamp; every new element will be attached to this 37 | * Event, 38 | * until the Event is ended. 39 | */ 40 | void startEvent(const std::string& name); 41 | /** 42 | * \brief ends an event and adds an Entry with the end timestamp 43 | */ 44 | void endEvent(); 45 | 46 | /** \brief return the file stream of the logfile. **/ 47 | std::ostream& stream(); 48 | 49 | /** \brief writing a value to the logfile **/ 50 | void appendParam(const Parameter& param); 51 | template 52 | void appendParam(const std::string& name, const T& value); 53 | 54 | /** \brief generate a new log with given name, which is stored in the specific directory. **/ 55 | static Log& createInstance(const std::string& name, const std::string& filename, bool append = false); 56 | 57 | /** \brief returns an Log instance with given name or a /dev/null-log if there is no such log **/ 58 | static Log& getInstance(const std::string& name); 59 | 60 | protected: 61 | Log(const std::string& name, const std::string& filename, bool append = false); 62 | 63 | std::string name_; 64 | std::ofstream out_; 65 | std::string identation_; 66 | static Log dummy_; 67 | static std::map instances_; 68 | 69 | std::vector event_stack_; 70 | }; 71 | 72 | class LogError : public Exception { 73 | public: 74 | LogError(const std::string& reason) : Exception(reason) {} 75 | }; 76 | } 77 | 78 | #endif /* LOG_H_ */ 79 | -------------------------------------------------------------------------------- /src/util/ScanAccumulator.cpp: -------------------------------------------------------------------------------- 1 | #include "ScanAccumulator.h" 2 | 3 | using namespace rv; 4 | using namespace glow; 5 | 6 | ScanAccumulator::ScanAccumulator(uint32_t history_size, uint32_t max_points) 7 | : vbo_(BufferTarget::ARRAY_BUFFER, BufferUsage::DYNAMIC_DRAW), capacity_(history_size), max_size_(max_points) { 8 | vbo_.resize(history_size * max_points); // allocate memory & set size accordingly... 9 | offsets_.resize(history_size, 0); 10 | poses_.resize(history_size); 11 | sizes_.resize(history_size, 0); 12 | timestamps_.resize(history_size, -1); 13 | std::fill(timestamps_.begin(), timestamps_.end(), -1); 14 | 15 | offsets_[0] = 0; 16 | sizes_[0] = 0; 17 | for (uint32_t i = 1; i < capacity_; ++i) { 18 | offsets_[i] = offsets_[i - 1] + max_points; 19 | sizes_[i] = 0; 20 | } 21 | } 22 | 23 | void ScanAccumulator::clear() { 24 | currentIdx_ = -1; 25 | size_ = 0; 26 | std::fill(timestamps_.begin(), timestamps_.end(), -1); 27 | } 28 | 29 | uint32_t ScanAccumulator::size() const { 30 | return size_; 31 | } 32 | 33 | uint32_t ScanAccumulator::capacity() const { 34 | return capacity_; 35 | } 36 | 37 | uint32_t ScanAccumulator::offset(uint32_t idx) const { 38 | int32_t newIdx = currentIdx_ - idx; 39 | if (newIdx < 0) newIdx = capacity_ + newIdx; 40 | 41 | return offsets_[newIdx]; 42 | } 43 | 44 | const Eigen::Matrix4f& ScanAccumulator::pose(uint32_t idx) const { 45 | int32_t newIdx = currentIdx_ - idx; 46 | if (newIdx < 0) newIdx = capacity_ + newIdx; 47 | 48 | return poses_[newIdx]; 49 | } 50 | 51 | uint32_t ScanAccumulator::timestamp(uint32_t idx) const { 52 | int32_t newIdx = currentIdx_ - idx; 53 | if (newIdx < 0) newIdx = capacity_ + newIdx; 54 | 55 | return timestamps_[newIdx]; 56 | } 57 | 58 | uint32_t ScanAccumulator::size(uint32_t idx) const { 59 | int32_t newIdx = currentIdx_ - idx; 60 | if (newIdx < 0) newIdx = capacity_ + newIdx; 61 | 62 | return sizes_[newIdx]; 63 | } 64 | 65 | glow::GlBuffer& ScanAccumulator::getVBO() { 66 | return vbo_; 67 | } 68 | 69 | void ScanAccumulator::insert(uint32_t timestamp, const Eigen::Matrix4f& pose, const std::vector& points) { 70 | currentIdx_ += 1; 71 | if (uint32_t(currentIdx_) >= capacity_) currentIdx_ = 0; 72 | 73 | timestamps_[currentIdx_] = timestamp; 74 | poses_[currentIdx_] = pose; 75 | sizes_[currentIdx_] = std::min(max_size_, (uint32_t)points.size()); 76 | vbo_.replace(offsets_[currentIdx_], &points[0], sizes_[currentIdx_]); 77 | 78 | if (size_ < capacity_) size_ += 1; 79 | } 80 | 81 | std::vector& ScanAccumulator::getPoses() { 82 | return poses_; 83 | } 84 | 85 | const std::vector& ScanAccumulator::getPoses() const { 86 | return poses_; 87 | } 88 | 89 | std::vector& ScanAccumulator::getTimestamps() { 90 | return timestamps_; 91 | } 92 | const std::vector& ScanAccumulator::getTimestamps() const { 93 | return timestamps_; 94 | } 95 | -------------------------------------------------------------------------------- /src/util/ScanAccumulator.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_CORE_SCANACCUMULATOR_H_ 2 | #define INCLUDE_CORE_SCANACCUMULATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** \brief Accumulate scans with poses like a ring buffer. 10 | * 11 | * Like in a ring buffer scans are added until a maximum capacity is reached. Then 12 | * always the oldest scan is replaced by a new scan. 13 | * 14 | * All scans are stored inside a large GlBuffer. The method getVBO returns the handle to 15 | * the vertex buffer object (VBO). The methods offset, pose and size return the offset, pose 16 | * and size of the scan with given index, where the last inserted scan has index 0, the scan before 17 | * 1, etc. 18 | * 19 | * \author behley 20 | **/ 21 | class ScanAccumulator { 22 | public: 23 | /** \brief Generate scan accumulator for given numer of scans with maximum number of points, 24 | * 25 | * \param history_size capacity of the scan accumulator, i.e., how many scans are stored before an old scan is 26 | *replaced by a newer scan. 27 | * \param max_points maximum number of points inside a scan. 28 | */ 29 | ScanAccumulator(uint32_t history_size, uint32_t max_points); 30 | 31 | /** \brief reset accumulator. **/ 32 | void clear(); 33 | 34 | /** \brief current number of scans inside the buffer. **/ 35 | uint32_t size() const; 36 | 37 | /** \brief capacity of the accumulator. **/ 38 | uint32_t capacity() const; 39 | 40 | /** \brief offset for scan with given index inside the vertex buffer object. **/ 41 | uint32_t offset(uint32_t idx) const; 42 | 43 | /** \brief pose of the scan with given index. **/ 44 | const Eigen::Matrix4f& pose(uint32_t idx) const; 45 | 46 | /** \brief timestamp of the scan with given index. **/ 47 | uint32_t timestamp(uint32_t idx) const; 48 | 49 | /** \brief number of points for scan with index. **/ 50 | uint32_t size(uint32_t idx) const; 51 | 52 | /** \brief get the associated vertex buffer object. **/ 53 | glow::GlBuffer& getVBO(); 54 | 55 | /** \brief inserting given scan and pose. **/ 56 | void insert(uint32_t timestamp, const Eigen::Matrix4f& pose, const std::vector& points); 57 | 58 | /** \brief get poses currently in the ring buffer.**/ 59 | std::vector& getPoses(); 60 | const std::vector& getPoses() const; 61 | 62 | /** \brief get timestamps currently in the ring buffer. **/ 63 | std::vector& getTimestamps(); 64 | const std::vector& getTimestamps() const; 65 | 66 | protected: 67 | glow::GlBuffer vbo_; 68 | 69 | std::vector poses_; 70 | std::vector offsets_; 71 | std::vector sizes_; 72 | 73 | std::vector timestamps_; 74 | 75 | uint32_t capacity_; 76 | uint32_t max_size_; 77 | uint32_t size_{0}; 78 | int32_t currentIdx_{-1}; 79 | }; 80 | 81 | #endif /* INCLUDE_CORE_SCANACCUMULATOR_H_ */ 82 | -------------------------------------------------------------------------------- /src/util/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/visualizer/VisualizerWindow.h: -------------------------------------------------------------------------------- 1 | #ifndef SRC_VISUALIZER_VISUALIZERWINDOW_H_ 2 | #define SRC_VISUALIZER_VISUALIZERWINDOW_H_ 3 | 4 | #include 5 | #include "rv/LaserscanReader.h" 6 | #include "ui_visualizer.h" 7 | 8 | #include "opengl/Model.h" 9 | #include "util/kitti_utils.h" 10 | 11 | #include "io/SimulationReader.h" 12 | #include "util/ScanAccumulator.h" 13 | 14 | class VisualizerWindow : public QMainWindow { 15 | Q_OBJECT 16 | public: 17 | VisualizerWindow(); 18 | ~VisualizerWindow(); 19 | 20 | void initialize(const std::shared_ptr& fusion, const rv::ParameterList& params); 21 | 22 | public slots: 23 | /** \brief open the given file and initialize the laser scan reader. **/ 24 | void openFile(const QString& filename); 25 | void startRecording(); 26 | 27 | protected slots: 28 | /** \brief open a laser scan file and initialize laser scan reader. **/ 29 | void showOpenFileDialog(); 30 | 31 | /** \brief start sequential reading of laser scans and update model. **/ 32 | void play(bool toggle); 33 | 34 | /** \brief read next scan **/ 35 | void nextScan(); 36 | /** \brief read specific scan. **/ 37 | void setScan(int idx); 38 | 39 | /** \brief reset reader's state and model. **/ 40 | void reset(); 41 | 42 | void updateParameters(); 43 | 44 | void updateScanIndex(); 45 | 46 | void renderMaps(); 47 | 48 | void triggerPreprocessing(); 49 | 50 | void optimizeGraph(); 51 | void initializeGraph(); 52 | 53 | void savePoses(); 54 | 55 | protected: 56 | void updatePlaybackControls(); 57 | 58 | void initilizeKITTIrobot(); 59 | void printResultStatistics(const std::vector& gt_poses, 60 | const std::vector& result_poses); 61 | void storeVelodynePoses(); 62 | 63 | Ui::MainWindow ui_; 64 | rv::LaserscanReader* reader_{nullptr}; 65 | 66 | uint32_t currentScanIdx_{0}; 67 | rv::Laserscan currentLaserscan_; 68 | QString lastDirectory_; 69 | QTimer timer_; 70 | 71 | std::shared_ptr fusion_; 72 | 73 | Eigen::Isometry3f pose_{Eigen::Isometry3f::Identity()}; 74 | 75 | std::vector precomputed_; 76 | std::vector oldPoses_; 77 | 78 | std::vector groundtruth_; 79 | KITTICalibration calib_; 80 | 81 | bool recording_{false}; 82 | std::string outputDirectory_; 83 | 84 | std::shared_ptr acc_; 85 | 86 | uint32_t start_scan{0}; 87 | 88 | rv::ParameterList params_; 89 | 90 | Eigen::Matrix4f adjustment_{Eigen::Matrix4f::Identity()}; 91 | bool parameterUpdateNeeded_{false}; 92 | uint32_t history_stride_{1}; 93 | float scanFrequency{10.0f}; 94 | }; 95 | 96 | #endif /* SRC_VISUALIZER_VISUALIZERWINDOW_H_ */ 97 | -------------------------------------------------------------------------------- /src/visualizer/visualizer.cpp: -------------------------------------------------------------------------------- 1 | // qt5 based visualization application using core. 2 | #include 3 | #include 4 | #include "VisualizerWindow.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace rv; 13 | 14 | int main(int argc, char** argv) { 15 | QApplication app(argc, argv); 16 | 17 | setlocale(LC_NUMERIC, "C"); 18 | 19 | VisualizerWindow window; // generates the OpenGL context... 20 | 21 | // initialize Laser Fusion. 22 | rv::ParameterList params; // default parameters. 23 | if (argc <= 1) { 24 | parseXmlFile("../config/default.xml", params); 25 | } else { 26 | parseXmlFile(argv[1], params); 27 | } 28 | 29 | std::shared_ptr fusion = std::shared_ptr(new SurfelMapping(params)); 30 | 31 | window.initialize(fusion, params); 32 | 33 | window.show(); 34 | 35 | // open file: 36 | if (argc > 2) { 37 | std::cout << "Opening " << argv[2] << std::endl; 38 | window.openFile(QString(argv[2])); 39 | } 40 | 41 | int32_t ret = app.exec(); 42 | 43 | std::cout << rv::Stopwatch::active() << " stopwatches active at exit." << std::endl; 44 | 45 | return ret; 46 | } 47 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # GTest Setup ###### 2 | message(STATUS "[SuMa] Fetching GTest.") 3 | 4 | include(FetchContent) 5 | FetchContent_Declare( 6 | googletest 7 | GIT_REPOSITORY https://github.com/google/googletest.git 8 | GIT_TAG e2239ee6043f73722e7aa812a459f54a28552929 # release-1.11.0 9 | ) 10 | 11 | FetchContent_MakeAvailable(googletest) 12 | 13 | set(BUILD_GMOCK OFF CACHE BOOL "" FORCE) 14 | set(BUILD_GTEST ON CACHE BOOL "" FORCE) 15 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 16 | set(gtest_build_tests OFF CACHE BOOL "" FORCE) 17 | 18 | # build the test cases 19 | include_directories(${GTEST_INCLUDE_DIRS} ${robovision_INCLUDE_DIRS} ${BOOST_INCLUDES}) 20 | 21 | file(GLOB SHADER_FILES "../src/shader/*") 22 | 23 | set(TEST_SHADER_SRC ${CMAKE_BINARY_DIR}/test_shaders.cpp) 24 | 25 | COMPILE_SHADERS( 26 | ${TEST_SHADER_SRC} 27 | ${SHADER_FILES} 28 | opengl/pix2ndc.frag 29 | opengl/pix2ndc.vert 30 | opengl/rounding.vert 31 | opengl/rounding.frag 32 | opengl/rounding_access.frag 33 | opengl/rounding_access_rect.frag 34 | opengl/test.frag 35 | opengl/ndc.frag 36 | opengl/ndc.vert) 37 | 38 | add_executable(test_core 39 | ../src/util/kitti_utils.cpp 40 | ../src/core/ImagePyramidGenerator.cpp 41 | ../src/core/lie_algebra.cpp 42 | 43 | core/PyramidTest.cpp 44 | 45 | core/CalibTest.cpp 46 | 47 | core/EvalTest.cpp 48 | core/matrix.cpp 49 | core/lie_test.cpp 50 | ) 51 | 52 | add_executable(test_posegraph 53 | ../src/core/Posegraph.cpp 54 | core/PosegraphTest.cpp 55 | ) 56 | 57 | 58 | add_executable(test_suma_opengl 59 | ../src/io/KITTIReader.cpp 60 | 61 | ../src/core/Preprocessing.cpp 62 | ../src/core/Frame2Model.cpp 63 | 64 | ../src/core/ImagePyramidGenerator.cpp 65 | 66 | ../src/core/lie_algebra.cpp 67 | ../src/core/LieGaussNewton.cpp 68 | 69 | ../src/util/kitti_utils.cpp 70 | 71 | ${TEST_SHADER_SRC} 72 | 73 | opengl/icp-test.cpp 74 | opengl/depthimg-test.cpp 75 | opengl/main-opengl.cpp 76 | opengl/testNDC.cpp 77 | opengl/jacobian-test.cpp 78 | ) 79 | 80 | configure_file(scan0.bin scan0.bin COPYONLY) 81 | configure_file(scan1.bin scan1.bin COPYONLY) 82 | configure_file(calib.txt calib.txt COPYONLY) 83 | 84 | target_link_libraries(test_core PRIVATE gtest_main robovision glow glow_util) 85 | target_link_libraries(test_suma_opengl PRIVATE gtest_main robovision glow glow_util) 86 | target_link_libraries(test_posegraph PRIVATE gtest_main robovision glow glow_util gtsam) 87 | 88 | 89 | # This runs the test cases always... 90 | # add_custom_target(run_tests_core ALL COMMAND core_tests DEPENDS core_tests) 91 | add_test(test_core test_core) 92 | add_test(test_suma_opengl test_suma_opengl) 93 | -------------------------------------------------------------------------------- /test/calib.txt: -------------------------------------------------------------------------------- 1 | P0: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 0.000000000000e+00 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.861448000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 4.538225000000e+01 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 -1.130887000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.779761000000e-03 4 | P3: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.372877000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 2.369057000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 4.915215000000e-03 5 | Tr: 4.276802385584e-04 -9.999672484946e-01 -8.084491683471e-03 -1.198459927713e-02 -7.210626507497e-03 8.081198471645e-03 -9.999413164504e-01 -5.403984729748e-02 9.999738645903e-01 4.859485810390e-04 -7.206933692422e-03 -2.921968648686e-01 6 | -------------------------------------------------------------------------------- /test/core/CalibTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "util/kitti_utils.h" 3 | 4 | namespace { 5 | 6 | TEST(CalibrationTest, testInitialization) { 7 | KITTICalibration calib("calib.txt"); 8 | 9 | ASSERT_TRUE(calib.exists("Tr")); 10 | ASSERT_TRUE(calib.exists("P0")); 11 | ASSERT_TRUE(calib.exists("P1")); 12 | ASSERT_TRUE(calib.exists("P2")); 13 | ASSERT_TRUE(calib.exists("P3")); 14 | ASSERT_FALSE(calib.exists("invalid")); 15 | 16 | Eigen::Matrix4f Tr = calib["Tr"]; 17 | 18 | ASSERT_FLOAT_EQ(0.000427680238558, Tr(0, 0)); 19 | ASSERT_FLOAT_EQ(-0.999967248495, Tr(0, 1)); 20 | ASSERT_FLOAT_EQ(-0.00808449168347, Tr(0, 2)); 21 | ASSERT_FLOAT_EQ(-0.0119845992771, Tr(0, 3)); 22 | 23 | ASSERT_FLOAT_EQ(-0.0072106265075, Tr(1, 0)); 24 | ASSERT_FLOAT_EQ(0.00808119847165, Tr(1, 1)); 25 | ASSERT_FLOAT_EQ(-0.99994131645, Tr(1, 2)); 26 | ASSERT_FLOAT_EQ(-0.0540398472975, Tr(1, 3)); 27 | 28 | ASSERT_FLOAT_EQ(0.99997386459, Tr(2, 0)); 29 | ASSERT_FLOAT_EQ(0.000485948581039, Tr(2, 1)); 30 | ASSERT_FLOAT_EQ(-0.00720693369242, Tr(2, 2)); 31 | ASSERT_FLOAT_EQ(-0.292196864869, Tr(2, 3)); 32 | 33 | ASSERT_FLOAT_EQ(0.0f, Tr(3, 0)); 34 | ASSERT_FLOAT_EQ(0.0f, Tr(3, 1)); 35 | ASSERT_FLOAT_EQ(0.0f, Tr(3, 2)); 36 | ASSERT_FLOAT_EQ(1.0f, Tr(3, 3)); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /test/core/lie_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace { 6 | 7 | TEST(LieTest, testLogExpSE3) { 8 | // simple sanity test, x = exp(log(x)) and x = log(exp(x)) 9 | 10 | Eigen::VectorXd x(6); 11 | x << -0.7, 0.1, 0.1, 0.1, -0.5, 0.33; 12 | 13 | Eigen::VectorXd x_logexp = SE3::log(SE3::exp(x)); 14 | 15 | for (uint32_t i = 0; i < 6; ++i) { 16 | ASSERT_NEAR(x[i], x_logexp[i], 0.0001); 17 | } 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /test/opengl/jacobian-test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include "io/KITTIReader.h" 7 | 8 | #include 9 | 10 | using namespace rv; 11 | using namespace glow; 12 | 13 | void printMat(const Eigen::MatrixXd& mat) { 14 | std::cout << std::fixed; 15 | std::cout << std::setprecision(5); 16 | for (uint32_t i = 0; i < mat.rows(); ++i) { 17 | for (uint32_t j = 0; j < mat.cols(); ++j) { 18 | std::cout << ((i == 0 && j == 0) ? "" : ",") << mat(i, j); 19 | } 20 | } 21 | std::cout << std::endl; 22 | } 23 | 24 | void compareMatrices(const Eigen::MatrixXd& expected, const Eigen::MatrixXd& given) { 25 | ASSERT_EQ(expected.rows(), given.rows()); 26 | ASSERT_EQ(expected.cols(), given.cols()); 27 | for (uint32_t i = 0; i < expected.rows(); ++i) { 28 | for (uint32_t j = 0; j < expected.cols(); ++j) { 29 | ASSERT_NEAR(expected(i, j), given(i, j), 0.001f) 30 | << "Mismatch at (" << i << ", " << j << "): Expected: " << expected(i, j) << ",but got: " << given(i, j); 31 | } 32 | } 33 | } 34 | 35 | TEST(JacobianTest, testjacobian) { 36 | ASSERT_NO_THROW(_CheckGlError(__FILE__, __LINE__)); 37 | 38 | uint32_t width = 720; 39 | uint32_t height = 64; 40 | ParameterList params; 41 | params.insert(IntegerParameter("data_width", width)); 42 | params.insert(IntegerParameter("data_height", height)); 43 | params.insert(FloatParameter("data_fov_up", 2.5)); 44 | params.insert(FloatParameter("data_fov_down", -24.8)); 45 | params.insert(FloatParameter("min_depth", 0.5f)); 46 | params.insert(FloatParameter("max_depth", 100.0f)); 47 | params.insert(FloatParameter("icp-max-angle", 50.0f)); 48 | params.insert(FloatParameter("icp-max-distance", 2.0f)); 49 | params.insert(FloatParameter("cutoff_threshold", 100.0f)); 50 | 51 | KITTIReader reader("./scan0.bin"); 52 | std::vector scans; 53 | Laserscan scan; 54 | while (reader.read(scan)) { 55 | scans.push_back(scan); 56 | } 57 | 58 | ASSERT_EQ(2, scans.size()); 59 | 60 | glow::GlBuffer pts{glow::BufferTarget::ARRAY_BUFFER, glow::BufferUsage::DYNAMIC_READ}; 61 | 62 | Preprocessing preprocessor(params); 63 | 64 | std::shared_ptr frame1 = std::make_shared(width, height); 65 | std::shared_ptr frame2 = std::make_shared(width, height); 66 | 67 | pts.assign(scans[0].points()); 68 | preprocessor.process(pts, *frame1); 69 | 70 | pts.assign(scans[1].points()); 71 | preprocessor.process(pts, *frame2); 72 | 73 | Frame2Model objective(params); 74 | objective.setData(frame1, frame2); 75 | 76 | Eigen::MatrixXd JtJ(6, 6); 77 | Eigen::MatrixXd Jtf(6, 1); 78 | objective.initialize(Eigen::Matrix4d::Identity()); 79 | objective.jacobianProducts(JtJ, Jtf); 80 | 81 | // printMat(JtJ); 82 | // printMat(Jtf); 83 | 84 | Eigen::MatrixXd JtJ_gold(6, 6); 85 | Eigen::MatrixXd Jtf_gold(6, 1); 86 | JtJ_gold << 993.91180, -51.22279, -643.58301, -350.30380, 9306.28320, 7171.18750, -51.22279, 821.79529, -543.81512, 87 | 2145.35474, 3262.42944, -2469.49561, -643.58301, -543.81512, 21746.54688, -51255.28906, -27316.54297, -2912.13965, 88 | -350.30380, 2145.35474, -51255.28906, 2884729.25000, -147322.00000, 71476.58594, 9306.28320, 3262.42944, 89 | -27316.54297, -147322.00000, 2454717.25000, 107829.93750, 7171.18750, -2469.49561, -2912.13965, 71476.58594, 90 | 107829.93750, 560050.56250; 91 | Jtf_gold << 137.19766, -50.23804, -117.34109, -1546.24805, -3293.39111, -2373.92749; 92 | 93 | uint32_t inlier = 23562, outlier = 6443, valid = 30005, invalid = 16075; 94 | 95 | compareMatrices(JtJ_gold, JtJ); 96 | compareMatrices(Jtf_gold, Jtf); 97 | 98 | ASSERT_EQ(inlier, objective.inlier()); 99 | ASSERT_EQ(outlier, objective.outlier()); 100 | ASSERT_EQ(valid, objective.valid()); 101 | ASSERT_EQ(invalid, objective.invalid()); 102 | } 103 | -------------------------------------------------------------------------------- /test/opengl/main-opengl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | int main(int argc, char **argv) { 9 | glow::X11OffscreenContext ctx; 10 | 11 | glow::inititializeGLEW(); 12 | // ASSERT_TRUE(ctx.create()); 13 | if (argc > 1 && std::string(argv[1]) == "info") { 14 | std::cout << "Device's OpenGL Capabilities: \n" << std::endl; 15 | std::cout << glow::GlCapabilities::getInstance() << std::endl; 16 | 17 | std::cout << "Device's current OpenGL state: \n" << std::endl; 18 | std::cout << glow::GlState::queryAll() << std::endl; 19 | } 20 | ::testing::InitGoogleTest(&argc, argv); 21 | 22 | return RUN_ALL_TESTS(); 23 | } 24 | -------------------------------------------------------------------------------- /test/opengl/ndc.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 coords; 4 | out vec4 color; 5 | 6 | void main() 7 | { 8 | color = coords; 9 | } -------------------------------------------------------------------------------- /test/opengl/ndc.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 position; 4 | out vec4 coords; 5 | 6 | void main() 7 | { 8 | gl_Position = vec4(position.x, position.y, 0, 1.0); 9 | coords = position; 10 | 11 | } -------------------------------------------------------------------------------- /test/opengl/pix2ndc.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 data; 4 | out vec4 out_data; 5 | 6 | void main() 7 | { 8 | out_data = data; 9 | } -------------------------------------------------------------------------------- /test/opengl/pix2ndc.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec2 position; 4 | 5 | out vec4 data; 6 | 7 | uniform int width; 8 | uniform int height; 9 | 10 | vec2 pix2ndc(int x, int y, int w, int h) 11 | { 12 | return vec2((2.0 * x + 1.0) / w - 1.0, (2.0 * y + 1.0) / h - 1.0); 13 | } 14 | 15 | void main() 16 | { 17 | vec2 coords = pix2ndc(int(position.x), int(position.y), width, height); 18 | gl_Position = vec4(coords, 0, 1.0); 19 | data = vec4(position.x, position.y, coords.x, coords.y); 20 | } -------------------------------------------------------------------------------- /test/opengl/rounding.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 pix_coords; 4 | out vec4 color; 5 | 6 | void main() 7 | { 8 | color = pix_coords; 9 | } -------------------------------------------------------------------------------- /test/opengl/rounding.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout (location = 0) in vec4 p; 4 | 5 | out vec4 pix_coords; 6 | 7 | void main() 8 | { 9 | gl_Position = vec4(p.x, p.y, 0, 1.0); 10 | pix_coords = vec4(p.x, p.y, p.z, p.w); 11 | } -------------------------------------------------------------------------------- /test/opengl/rounding_access.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | 5 | uniform sampler2D tex_input; // texture coordinates 6 | uniform sampler2D tex_values; // accessed values with texture coordinates. 7 | 8 | out vec4 color; 9 | 10 | 11 | void main() 12 | { 13 | vec2 t = texture(tex_input, texCoords).xy; 14 | color = texture(tex_values, texture(tex_input, texCoords).xy); 15 | color = vec4(floor(t.x*720), floor(t.y*64), color.z, color.w); 16 | } -------------------------------------------------------------------------------- /test/opengl/rounding_access_rect.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec2 texCoords; 4 | 5 | uniform sampler2D tex_input; // texture coordinates 6 | uniform sampler2DRect tex_values; // accessed values with texture coordinates. 7 | 8 | out vec4 color; 9 | 10 | 11 | void main() 12 | { 13 | vec2 t = texture(tex_input, texCoords).xy; 14 | color = texture(tex_values, texture(tex_input, texCoords).xy); 15 | } -------------------------------------------------------------------------------- /test/opengl/test.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec2 texCoords; 4 | 5 | uniform sampler2D tex_input; 6 | uniform float offset; 7 | 8 | out vec3 color; 9 | 10 | void main() 11 | { 12 | vec2 coords = vec2(texCoords.x + offset, texCoords.y); 13 | vec2 tex = texture(tex_input, coords.xy).rg; 14 | color = vec3(tex.xy, 0); 15 | } -------------------------------------------------------------------------------- /test/rv/fileutil-test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | using namespace rv; 8 | 9 | TEST(FileutilTestcase, testExtists) 10 | { 11 | std::ofstream out("test.txt"); 12 | out << "test123"; 13 | out.close(); 14 | 15 | ASSERT_EQ(true, FileUtil::exists("./test.txt")); 16 | ASSERT_EQ(false, FileUtil::exists("/does/not/exists")); 17 | 18 | std::remove("test.txt"); 19 | 20 | ASSERT_EQ(false, FileUtil::exists("./test.txt")); 21 | 22 | } 23 | 24 | TEST(FileutilTestcase, testBaseName) 25 | { 26 | ASSERT_EQ("test.txt", FileUtil::baseName("./test.txt")); 27 | ASSERT_EQ("exists", FileUtil::baseName("/does/not/exists")); 28 | ASSERT_EQ(".", FileUtil::baseName("/does/not/exists/")); 29 | ASSERT_EQ("complicated.example.tar.gz", FileUtil::baseName("/very/very/very/complicated.example.tar.gz")); 30 | } 31 | 32 | TEST(FileutilTestcase, testDirName) 33 | { 34 | ASSERT_EQ(".", FileUtil::dirName("./test.txt")); 35 | ASSERT_EQ("/does/not", FileUtil::dirName("/does/not/exists")); 36 | ASSERT_EQ("/does/not/exists", FileUtil::dirName("/does/not/exists/")); 37 | ASSERT_EQ("/very/very/very", FileUtil::dirName("/very/very/very/complicated.example.tar.gz")); 38 | } 39 | 40 | TEST(FileutilTestcase, testStripExtension) 41 | { 42 | ASSERT_EQ("/test/directory/.", FileUtil::stripExtension("/test/directory/.")); 43 | ASSERT_EQ("/test/directory/..", FileUtil::stripExtension("/test/directory/..")); 44 | 45 | ASSERT_EQ("./test", FileUtil::stripExtension("./test.txt")); 46 | ASSERT_EQ("/does/not/exists", FileUtil::stripExtension("/does/not/exists")); 47 | ASSERT_EQ("/very/very/very/complicated.example.tar", FileUtil::stripExtension("/very/very/very/complicated.example.tar.gz")); 48 | 49 | ASSERT_EQ("./test.txt", FileUtil::stripExtension("./test.txt", 0)); 50 | ASSERT_EQ("/does/not/exists", FileUtil::stripExtension("/does/not/exists", 0)); 51 | ASSERT_EQ("/very/very/very.directory/complicated.example.tar.gz", FileUtil::stripExtension("/very/very/very.directory/complicated.example.tar.gz", 0)); 52 | 53 | 54 | ASSERT_EQ("./test", FileUtil::stripExtension("./test.txt", 2)); 55 | ASSERT_EQ("/does/not/exists", FileUtil::stripExtension("/does/not/exists", 2)); 56 | ASSERT_EQ("/very/very/very.directory/complicated.example", FileUtil::stripExtension("/very/very/very.directory/complicated.example.tar.gz", 2)); 57 | ASSERT_EQ("/very/very/very.directory/complicated", FileUtil::stripExtension("/very/very/very.directory/complicated.example.tar.gz", 3)); 58 | ASSERT_EQ("/very/very/very.directory/complicated", FileUtil::stripExtension("/very/very/very.directory/complicated.example.tar.gz", 4)); 59 | ASSERT_EQ("/very/very/very.directory/complicated", FileUtil::stripExtension("/very/very/very.directory/complicated.example.tar.gz", 120)); 60 | } 61 | 62 | TEST(FileutilTestcase, testExtension) 63 | { 64 | ASSERT_EQ("", FileUtil::extension("/test/directory/.")); 65 | ASSERT_EQ("", FileUtil::extension("/test/directory/..")); 66 | 67 | ASSERT_EQ(".txt", FileUtil::extension("./test.txt")); 68 | ASSERT_EQ("", FileUtil::extension("/does/not/exists")); 69 | ASSERT_EQ(".gz", FileUtil::extension("/very/very/very/complicated.example.tar.gz")); 70 | 71 | ASSERT_EQ("", FileUtil::extension("./test.txt", 0)); 72 | ASSERT_EQ("", FileUtil::extension("/does/not/exists", 0)); 73 | ASSERT_EQ("", FileUtil::extension("/very/very/very/complicated.example.tar.gz", 0)); 74 | 75 | 76 | ASSERT_EQ(".txt", FileUtil::extension("./test.txt", 2)); 77 | ASSERT_EQ("", FileUtil::extension("/does/not/exists", 2)); 78 | ASSERT_EQ(".tar.gz", FileUtil::extension("/very/very/very/complicated.example.tar.gz", 2)); 79 | ASSERT_EQ(".example.tar.gz", FileUtil::extension("/very/very/very/complicated.example.tar.gz", 3)); 80 | ASSERT_EQ(".example.tar.gz", FileUtil::extension("/very/very/very/complicated.example.tar.gz", 4)); 81 | ASSERT_EQ(".example.tar.gz", FileUtil::extension("/very/very/very/complicated.example.tar.gz", 120)); 82 | } 83 | -------------------------------------------------------------------------------- /test/rv/parameterlist-test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "../test_helpers.h" 6 | 7 | using namespace rv; 8 | 9 | namespace 10 | { 11 | 12 | class ParameterListTest: public ::testing::Test 13 | { 14 | protected: 15 | ParameterList params; 16 | }; 17 | 18 | TEST_F(ParameterListTest, testOperations) 19 | { 20 | /*** inserting different parameters ***/ 21 | params.insert(BooleanParameter("bool", true)); 22 | ASSERT_EQ(1, params.size()); 23 | ASSERT_TRUE(params.hasParam("bool")); 24 | params.insert(FloatParameter("float", 3.0)); 25 | ASSERT_EQ(2, params.size()); 26 | ASSERT_TRUE(params.hasParam("float")); 27 | params.insert(StringParameter("string", "test")); 28 | ASSERT_EQ(3, params.size()); 29 | ASSERT_TRUE(params.hasParam("string")); 30 | params.insert(StringParameter("string2", "remove")); 31 | ASSERT_EQ(4, params.size()); 32 | ASSERT_TRUE(params.hasParam("string2")); 33 | ASSERT_FALSE(params.hasParam("bloat")); 34 | ASSERT_FALSE(params.hasParam("STRING")); 35 | 36 | /*** erasing parameters ***/ 37 | params.erase("string2"); 38 | ASSERT_EQ(3, params.size()); 39 | ASSERT_FALSE(params.hasParam("string2")); 40 | 41 | /*** replacing a parameter ***/ 42 | ASSERT_EQ("test", params.getValue("string")); 43 | params.insert(StringParameter("string", "another test")); 44 | ASSERT_EQ("another test", params.getValue("string")); 45 | 46 | /*** testing the exceptions ***/ 47 | // ASSERT_THROW(params.getParameter("string"), legacy_fkie::Exception); 48 | ASSERT_THROW(params.getValue("string"), rv::Exception); 49 | ASSERT_THROW(params.getValue("not existing value"), rv::Exception); 50 | ASSERT_THROW(params["not existing value"], rv::Exception); 51 | 52 | uint32_t count = 0; 53 | std::vector names; 54 | /*** testing the iterator ***/ 55 | for (ParameterList::const_iterator it = params.begin(); it != params.end(); ++it) 56 | { 57 | names.push_back(it->name()); 58 | ++count; 59 | } 60 | 61 | std::vector expected_names; 62 | expected_names.push_back("bool"); 63 | expected_names.push_back("float"); 64 | expected_names.push_back("string"); 65 | 66 | ASSERT_TRUE(similarVectors(expected_names, names)); 67 | ASSERT_EQ(3, count); 68 | 69 | /** test copy **/ 70 | ParameterList other = params; 71 | ASSERT_EQ(params.size(), other.size()); 72 | for (ParameterList::const_iterator it = params.begin(); it != params.end(); ++it) 73 | { 74 | bool found = false; 75 | for (ParameterList::const_iterator it2 = other.begin(); it2 != other.end(); ++it2) 76 | { 77 | if (*it == *it2) 78 | { 79 | found = true; 80 | break; 81 | } 82 | } 83 | 84 | ASSERT_TRUE(found); 85 | } 86 | } 87 | 88 | } 89 | -------------------------------------------------------------------------------- /test/rv/xml-test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | TEST(XmlTest, parse) 5 | { 6 | // parse stream is all we need to test. 7 | std::string xml_doc; 8 | xml_doc += "\n"; 9 | xml_doc += " \n"; 10 | xml_doc += " \n"; 11 | xml_doc += " \n"; 12 | xml_doc += " \n"; 13 | xml_doc += " \n"; 14 | xml_doc += " \n"; 15 | xml_doc += " the content!\n"; 16 | xml_doc += " \n"; 17 | xml_doc += " \n"; 18 | xml_doc += "\n \n\n "; 19 | 20 | //std::cout << xml_doc << "" << std::endl; 21 | 22 | //std::cout << "######################" << std::endl; 23 | 24 | std::stringstream stream(xml_doc); 25 | 26 | rv::XmlDocument doc = rv::XmlDocument::fromStream(stream); 27 | 28 | const rv::XmlNode* node; 29 | 30 | ASSERT_TRUE(doc.root().isValid()); 31 | ASSERT_EQ("root", doc.root().getName()); 32 | ASSERT_EQ(3, doc.root().getChildren().size()); 33 | 34 | node = &doc.root().getChildren()[0]; 35 | ASSERT_EQ("element", node->getName()); 36 | ASSERT_TRUE(node->hasAttribute("name")); 37 | ASSERT_FALSE(node->hasAttribute("Foobar")); 38 | ASSERT_EQ("1337", node->getAttribute("name")); 39 | ASSERT_TRUE(node->hasAttribute("a")); 40 | ASSERT_EQ("1234.5678", node->getAttribute("a")); 41 | ASSERT_EQ(1, node->getChildren().size()); 42 | node = &node->getChildren()[0]; 43 | ASSERT_EQ("inner_element", node->getName()); 44 | ASSERT_TRUE(node->hasAttribute("name")); 45 | ASSERT_EQ("inner node", node->getAttribute("name")); 46 | ASSERT_TRUE(node->hasAttribute("value")); 47 | ASSERT_EQ("1337.1337", node->getAttribute("value")); 48 | 49 | node = &doc.root().getChildren()[1]; 50 | ASSERT_EQ("a", node->getName()); 51 | ASSERT_EQ(2, node->getChildren().size()); 52 | ASSERT_EQ("element", node->getChildren()[1].getName()); 53 | ASSERT_FALSE(node->getChildren()[1].hasAttribute("missing")); 54 | ASSERT_EQ("the content!", node->getChildren()[1].getContent()); 55 | ASSERT_TRUE(doc.root().getChildren()[0].hasAttribute("name")); 56 | 57 | // test accessors: 58 | ASSERT_TRUE(doc.root().getChild("element").isValid()); 59 | ASSERT_EQ("1337", doc.root().getChild("element").getAttribute("name")); 60 | ASSERT_FALSE(doc.root().getChild("missing").isValid()); 61 | 62 | ASSERT_TRUE(doc.root().getChildWith("name", "waldo").isValid()); 63 | ASSERT_FALSE(doc.root().getChildWith("name", "blabla").isValid()); 64 | ASSERT_EQ("element", doc.root().getChildWith("name", "waldo").getName()); 65 | } 66 | 67 | TEST(XmlTest, partial_parse) 68 | { 69 | 70 | std::string xml_doc; 71 | xml_doc += "\n"; 72 | xml_doc += " \n"; 73 | xml_doc += " \n"; 74 | 75 | ASSERT_THROW(rv::XmlDocument::fromData(xml_doc), rv::XmlError); 76 | rv::XmlDocument doc = rv::XmlDocument::fromData(xml_doc, true); 77 | ASSERT_TRUE(doc.root().isValid()); 78 | ASSERT_EQ(1, doc.root().getChildren().size()); 79 | 80 | const rv::XmlNode* node = &doc.root().getChildren()[0]; 81 | ASSERT_EQ("element", node->getName()); 82 | ASSERT_TRUE(node->hasAttribute("name")); 83 | ASSERT_FALSE(node->hasAttribute("Foobar")); 84 | ASSERT_EQ("1337", node->getAttribute("name")); 85 | ASSERT_TRUE(node->hasAttribute("a")); 86 | ASSERT_EQ("1234.5678", node->getAttribute("a")); 87 | 88 | ASSERT_EQ(1, node->getChildren().size()); 89 | node = &node->getChildren()[0]; 90 | ASSERT_EQ("inner_element", node->getName()); 91 | ASSERT_TRUE(node->hasAttribute("name")); 92 | ASSERT_EQ("inner node", node->getAttribute("name")); 93 | ASSERT_TRUE(node->hasAttribute("value")); 94 | ASSERT_EQ("1337.1337", node->getAttribute("value")); 95 | 96 | } 97 | -------------------------------------------------------------------------------- /test/scan0.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jbehley/SuMa/2d191f5a5c83e793ef2768611b2d5cad04943a9e/test/scan0.bin -------------------------------------------------------------------------------- /test/scan1.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jbehley/SuMa/2d191f5a5c83e793ef2768611b2d5cad04943a9e/test/scan1.bin -------------------------------------------------------------------------------- /test/test_helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef TEST_HELPERS_H_ 2 | #define TEST_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | // some utils to ease the generation of tests. 11 | 12 | template 13 | bool similarVectors(std::vector& vec1, std::vector& vec2) 14 | { 15 | if (vec1.size() != vec2.size()) 16 | { 17 | std::cout << "expected size = " << vec1.size() << ", but got size = " << vec2.size() << std::endl; 18 | return false; 19 | } 20 | 21 | for (uint32_t i = 0; i < vec1.size(); ++i) 22 | { 23 | bool found = false; 24 | for (uint32_t j = 0; j < vec2.size(); ++j) 25 | { 26 | if (vec1[i] == vec2[j]) 27 | { 28 | found = true; 29 | break; 30 | } 31 | } 32 | if (!found) 33 | { 34 | std::cout << i << "-th element (" << vec1[i] << ") not found." << std::endl; 35 | return false; 36 | } 37 | } 38 | 39 | return true; 40 | } 41 | 42 | template 43 | bool equalVectors(std::vector& vec1, std::vector& vec2) 44 | { 45 | double eps = 0.00001; 46 | if (vec1.size() != vec2.size()) 47 | { 48 | std::cout << "expected size = " << vec1.size() << ", but got size = " << vec2.size() << std::endl; 49 | return false; 50 | } 51 | 52 | for (uint32_t i = 0; i < vec1.size(); ++i) 53 | { 54 | if (std::abs(vec1[i] - vec2[i]) > eps) 55 | { 56 | std::cout << i << "-th element not equal: " << vec1[i] << " vs. " << vec2[i] << std::endl; 57 | return false; 58 | } 59 | } 60 | 61 | return true; 62 | } 63 | 64 | // check if vec1 is subset of vec2 65 | template 66 | bool subsetOf(std::vector& vec1, std::vector& vec2) 67 | { 68 | for (uint32_t i = 0; i < vec1.size(); ++i) 69 | { 70 | bool found = false; 71 | for (uint32_t j = 0; j < vec2.size(); ++j) 72 | { 73 | if (vec1[i] == vec2[j]) 74 | { 75 | found = true; 76 | break; 77 | } 78 | } 79 | 80 | if (!found) 81 | { 82 | std::cout << i << "-th element (" << vec1[i] << ") is not in superset." << std::endl; 83 | return false; 84 | } 85 | } 86 | 87 | return true; 88 | } 89 | 90 | /** 91 | * \brief generate a uniform randomly samples set of points in [a, b]^3 92 | * @param pts 93 | * @param N 94 | * @param seed 95 | */ 96 | template 97 | void generateRandomPoints(std::vector& pts, uint32_t N, uint32_t seed = 0, float a = -5, float b = 5) 98 | { 99 | boost::mt11213b mtwister(seed); 100 | boost::uniform_01<> gen; 101 | pts.clear(); 102 | pts.reserve(N); 103 | // generate N random points in [a,b] x [a,b] x [a,b]... 104 | float range = b - a; 105 | for (uint32_t i = 0; i < N; ++i) 106 | { 107 | PointT pt; 108 | rv::set<0>(pt, range * gen(mtwister) + a); 109 | rv::set<1>(pt, range * gen(mtwister) + a); 110 | rv::set<2>(pt, range * gen(mtwister) + a); 111 | 112 | pts.push_back(pt); 113 | } 114 | } 115 | 116 | #endif /* TEST_HELPERS_H_ */ 117 | --------------------------------------------------------------------------------