├── .clang-format ├── .github └── workflows │ └── build_test.yml ├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── docs ├── Doxyfile ├── Makefile ├── conf.py ├── index.rst ├── logo.gif ├── pages │ ├── Class-Overview.rst │ ├── Example-Outputs.rst │ ├── Expanding-the-Docs.rst │ ├── How-Does-ESDF-Generation-Work.rst │ ├── Installation.rst │ ├── Modifying-and-Contributing.rst │ ├── Performance.rst │ ├── Running-Voxblox.rst │ ├── The-Voxblox-Node.rst │ ├── Transformations.rst │ └── Using-Voxblox-for-Planning.rst └── requirements.txt ├── rosdoc.yaml ├── voxblox ├── CMakeLists.txt ├── include │ └── voxblox │ │ ├── alignment │ │ └── icp.h │ │ ├── core │ │ ├── block.h │ │ ├── block_hash.h │ │ ├── block_inl.h │ │ ├── color.h │ │ ├── common.h │ │ ├── esdf_map.h │ │ ├── layer.h │ │ ├── layer_inl.h │ │ ├── occupancy_map.h │ │ ├── tsdf_map.h │ │ └── voxel.h │ │ ├── integrator │ │ ├── esdf_integrator.h │ │ ├── esdf_occ_integrator.h │ │ ├── integrator_utils.h │ │ ├── intensity_integrator.h │ │ ├── merge_integration.h │ │ ├── occupancy_integrator.h │ │ └── tsdf_integrator.h │ │ ├── interpolator │ │ ├── interpolator.h │ │ └── interpolator_inl.h │ │ ├── io │ │ ├── layer_io.h │ │ ├── layer_io_inl.h │ │ ├── mesh_ply.h │ │ ├── ply_writer.h │ │ └── sdf_ply.h │ │ ├── mesh │ │ ├── README.md │ │ ├── marching_cubes.h │ │ ├── mesh.h │ │ ├── mesh_integrator.h │ │ ├── mesh_layer.h │ │ └── mesh_utils.h │ │ ├── simulation │ │ ├── objects.h │ │ ├── simulation_world.h │ │ └── simulation_world_inl.h │ │ ├── test │ │ └── layer_test_utils.h │ │ └── utils │ │ ├── approx_hash_array.h │ │ ├── bucket_queue.h │ │ ├── camera_model.h │ │ ├── color_maps.h │ │ ├── distance_utils.h │ │ ├── evaluation_utils.h │ │ ├── layer_utils.h │ │ ├── meshing_utils.h │ │ ├── neighbor_tools.h │ │ ├── planning_utils.h │ │ ├── planning_utils_inl.h │ │ ├── protobuf_utils.h │ │ ├── timing.h │ │ └── voxel_utils.h ├── package.xml ├── proto │ └── voxblox │ │ ├── Block.proto │ │ └── Layer.proto ├── src │ ├── alignment │ │ └── icp.cc │ ├── core │ │ ├── block.cc │ │ ├── esdf_map.cc │ │ └── tsdf_map.cc │ ├── integrator │ │ ├── esdf_integrator.cc │ │ ├── esdf_occ_integrator.cc │ │ ├── integrator_utils.cc │ │ ├── intensity_integrator.cc │ │ └── tsdf_integrator.cc │ ├── io │ │ ├── mesh_ply.cc │ │ └── sdf_ply.cc │ ├── mesh │ │ └── marching_cubes.cc │ ├── simulation │ │ ├── objects.cc │ │ └── simulation_world.cc │ └── utils │ │ ├── camera_model.cc │ │ ├── evaluation_utils.cc │ │ ├── layer_utils.cc │ │ ├── neighbor_tools.cc │ │ ├── protobuf_utils.cc │ │ ├── timing.cc │ │ └── voxel_utils.cc └── test │ ├── test_approx_hash_array.cc │ ├── test_bucket_queue.cc │ ├── test_clear_spheres.cc │ ├── test_layer.cc │ ├── test_layer_utils.cc │ ├── test_load_esdf.cc │ ├── test_merge_integration.cc │ ├── test_protobuf.cc │ ├── test_sdf_integrators.cc │ ├── test_tsdf_interpolator.cc │ ├── test_tsdf_map.cc │ └── tsdf_to_esdf.cc ├── voxblox_https.rosinstall ├── voxblox_msgs ├── CMakeLists.txt ├── msg │ ├── Block.msg │ ├── Layer.msg │ ├── Mesh.msg │ ├── MeshBlock.msg │ └── VoxelEvaluationDetails.msg ├── package.xml └── srv │ └── FilePath.srv ├── voxblox_ros ├── CMakeLists.txt ├── cfg │ ├── calibrations │ │ └── euroc_camchain.yaml │ ├── cow_and_lady.yaml │ ├── cow_dataset.yaml │ ├── euroc_dataset.yaml │ ├── rgbd_dataset.yaml │ └── stereo │ │ ├── kitti_stereo.yaml │ │ ├── kitti_stereo_bm.yaml │ │ └── kitti_stereo_jager.yaml ├── include │ └── voxblox_ros │ │ ├── conversions.h │ │ ├── conversions_inl.h │ │ ├── esdf_server.h │ │ ├── intensity_server.h │ │ ├── intensity_vis.h │ │ ├── interactive_slider.h │ │ ├── mesh_pcl.h │ │ ├── mesh_vis.h │ │ ├── ptcloud_vis.h │ │ ├── ros_params.h │ │ ├── simulation_server.h │ │ ├── transformer.h │ │ └── tsdf_server.h ├── launch │ ├── basement_dataset.launch │ ├── cow_and_lady_dataset.launch │ ├── euroc_dataset.launch │ ├── eval │ │ ├── eval_cow.launch │ │ ├── eval_cow_and_lady.launch │ │ ├── eval_euroc.launch │ │ └── eval_kitti.launch │ ├── kitti_dataset.launch │ ├── rgbd_dataset.launch │ └── sim.launch ├── mesh_results │ └── .keep ├── package.xml └── src │ ├── esdf_server.cc │ ├── esdf_server_node.cc │ ├── intensity_server.cc │ ├── intensity_server_node.cc │ ├── interactive_slider.cc │ ├── simulation_eval.cc │ ├── simulation_server.cc │ ├── transformer.cc │ ├── tsdf_server.cc │ ├── tsdf_server_node.cc │ ├── visualize_tsdf.cc │ └── voxblox_eval.cc ├── voxblox_rviz_plugin ├── CMakeLists.txt ├── icons │ └── classes │ │ └── VoxbloxMesh.png ├── include │ └── voxblox_rviz_plugin │ │ ├── voxblox_mesh_display.h │ │ └── voxblox_mesh_visual.h ├── package.xml ├── plugin_description.xml └── src │ ├── voxblox_mesh_display.cc │ └── voxblox_mesh_visual.cc └── voxblox_ssh.rosinstall /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | ColumnLimit: 80 5 | IncludeBlocks: Preserve 6 | --- 7 | Language: Proto 8 | BasedOnStyle: Google 9 | ... 10 | -------------------------------------------------------------------------------- /.github/workflows/build_test.yml: -------------------------------------------------------------------------------- 1 | 2 | name: Build Test 3 | on: 4 | push: 5 | branches: 6 | - 'master' 7 | pull_request: 8 | branches: 9 | - '*' 10 | 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | config: 18 | - {rosdistro: 'melodic', container: 'ros:melodic-ros-base-bionic'} 19 | - {rosdistro: 'noetic', container: 'ros:noetic-ros-base-focal'} 20 | container: ${{ matrix.config.container }} 21 | steps: 22 | - uses: actions/checkout@v1 23 | with: 24 | token: ${{ secrets.ACCESS_TOKEN }} 25 | github-token: ${{ secrets.GITHUB_TOKEN }} 26 | - name: Install catkin-tools on melodic 27 | if: ${{ matrix.config.container == 'ros:melodic-ros-base-bionic' }} 28 | run: | 29 | apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 30 | apt update && apt install -y python3-wstool python-catkin-tools 31 | - name: Install catkin-tools on Noetic 32 | if: ${{ matrix.config.container == 'ros:noetic-ros-base-focal' }} 33 | run: | 34 | apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 35 | apt update && apt install -y python3-pip 36 | pip3 install osrf-pycommon 37 | apt update && apt install -y python3-wstool python3-catkin-tools 38 | - name: release_build_test 39 | working-directory: 40 | env: 41 | DEBIAN_FRONTEND: noninteractive 42 | run: | 43 | apt update 44 | apt install -y python3-wstool autoconf libtool git 45 | mkdir -p $HOME/catkin_ws/src; 46 | cd $HOME/catkin_ws 47 | catkin init 48 | catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}" 49 | catkin config --merge-devel 50 | cd $HOME/catkin_ws/src 51 | ln -s $GITHUB_WORKSPACE 52 | cd $HOME/catkin_ws 53 | wstool init src src/voxblox/voxblox_https.rosinstall 54 | wstool update -t src -j4 55 | rosdep update 56 | rosdep install --from-paths src --ignore-src -y --rosdistro ${{matrix.config.rosdistro}} 57 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 58 | catkin build -j$(nproc) -l$(nproc) voxblox_ros 59 | shell: bash 60 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # Eclipse 31 | .cproject 32 | .project 33 | /Debug/ 34 | .settings 35 | 36 | # Python (for lint) 37 | *.pyc 38 | 39 | # Temporary files 40 | *~ 41 | 42 | # Mac 43 | .DS_Store 44 | 45 | # Mesh results 46 | mesh_results 47 | 48 | # Tools 49 | autolintc 50 | 51 | # Documentation 52 | docs/_build 53 | docs/doxyoutput 54 | docs/api 55 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/voxblox/c8066b04075d2fee509de295346b1c0b788c4f38/.gitmodules -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, ETHZ ASL 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of voxblox nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Voxblox 2 | 3 | [![Build Test](https://github.com/ethz-asl/voxblox/actions/workflows/build_test.yml/badge.svg)](https://github.com/ethz-asl/voxblox/actions/workflows/build_test.yml) 4 | 5 | ![voxblox_small](https://cloud.githubusercontent.com/assets/5616392/15180357/536a8776-1781-11e6-8c1d-f2dfa34b1408.gif) 6 | 7 | Voxblox is a volumetric mapping library based mainly on Truncated Signed Distance Fields (TSDFs). It varies from other SDF libraries in the following ways: 8 | * CPU-only, can be run single-threaded or multi-threaded for some integrators 9 | * Support for multiple different layer types (containing different types of voxels) 10 | * Serialization using protobufs 11 | * Different ways of handling weighting during merging 12 | * Different ways of inserting pose information about scans 13 | * Tight ROS integration (in voxblox_ros package) 14 | * Easily extensible with whatever integrators you want 15 | * Features an implementation of building Euclidean Signed Distance Fields (ESDFs, EDTs) directly from TSDFs. 16 | 17 | **If you're looking for skeletonization/sparse topology or planning applications, please refer to the [mav_voxblox_planning](https://github.com/ethz-asl/mav_voxblox_planning) repo.** 18 | **If you want to create ground truth maps from meshes or gazebo environments, please check out the [voxblox_ground_truth](https://github.com/ethz-asl/voxblox_ground_truth) pakage!** 19 | 20 | ![example_gif](http://i.imgur.com/2wLztFm.gif) 21 | 22 | # Documentation 23 | * All voxblox documentation can be found on [our readthedocs page](https://voxblox.readthedocs.io/en/latest/index.html) 24 | 25 | ## Table of Contents 26 | * [Paper and Video](#paper-and-video) 27 | * [Credits](#credits) 28 | * [Example Outputs](https://voxblox.readthedocs.io/en/latest/pages/Example-Outputs.html) 29 | * [Performance](https://voxblox.readthedocs.io/en/latest/pages/Performance.html) 30 | * [Installation](https://voxblox.readthedocs.io/en/latest/pages/Installation.html) 31 | * [Running Voxblox](https://voxblox.readthedocs.io/en/latest/pages/Running-Voxblox.html) 32 | * [Using Voxblox for Planning](https://voxblox.readthedocs.io/en/latest/pages/Using-Voxblox-for-Planning.html) 33 | * [Transformations in Voxblox](https://voxblox.readthedocs.io/en/latest/pages/Transformations.html) 34 | * [Contributing to Voxblox](https://voxblox.readthedocs.io/en/latest/pages/Modifying-and-Contributing.html) 35 | * [Library API](https://voxblox.readthedocs.io/en/latest/api/library_root.html) 36 | 37 | # Paper and Video 38 | A video showing sample output from voxblox can be seen [here](https://www.youtube.com/watch?v=PlqT5zNsvwM). 39 | A video of voxblox being used for online planning on-board a multicopter can be seen [here](https://youtu.be/lrGSwAPzMOQ). 40 | 41 | If using voxblox for scientific publications, please cite the following paper, available [here](http://helenol.github.io/publications/iros_2017_voxblox.pdf): 42 | 43 | Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, and Roland Siegwart, “**Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning**”, in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2017. 44 | 45 | ```latex 46 | @inproceedings{oleynikova2017voxblox, 47 | author={Oleynikova, Helen and Taylor, Zachary and Fehr, Marius and Siegwart, Roland and Nieto, Juan}, 48 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 49 | title={Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning}, 50 | year={2017} 51 | } 52 | ``` 53 | 54 | # Credits 55 | This library was written primarily by Helen Oleynikova and Marius Fehr, with significant contributions from Zachary Taylor, Alexander Millane, and others. The marching cubes meshing and ROS mesh generation were taken or heavily derived from [open_chisel](https://github.com/personalrobotics/OpenChisel). We've retained the copyright headers for the relevant files. 56 | 57 | ![offline_manifold](https://i.imgur.com/pvHhVsL.png) 58 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = project 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | name = 'voxblox' 4 | 5 | on_rtd = os.environ.get('READTHEDOCS', None) == 'True' 6 | 7 | if not on_rtd: # only import and set the theme if we're building docs locally 8 | import sphinx_rtd_theme 9 | html_theme = 'sphinx_rtd_theme' 10 | html_theme_path = [sphinx_rtd_theme.get_html_theme_path()] 11 | 12 | html_logo = "logo.gif" 13 | 14 | source_parsers = { 15 | '.md': 'recommonmark.parser.CommonMarkParser', 16 | } 17 | 18 | extensions = [ 19 | 'breathe', 'exhale', 'sphinx.ext.autosectionlabel', 'recommonmark' 20 | ] 21 | 22 | project = name 23 | master_doc = 'index' 24 | 25 | html_theme_options = {'logo_only': True} 26 | 27 | # Setup the breathe extension 28 | breathe_projects = {"project": "./doxyoutput/xml"} 29 | breathe_default_project = "project" 30 | 31 | # Setup the exhale extension 32 | exhale_args = { 33 | "verboseBuild": False, 34 | "containmentFolder": "./api", 35 | "rootFileName": "library_root.rst", 36 | "rootFileTitle": "Library API", 37 | "doxygenStripFromPath": "..", 38 | "createTreeView": True, 39 | "exhaleExecutesDoxygen": True, 40 | "exhaleUseDoxyfile": True, 41 | "pageLevelConfigMeta": ":github_url: https://github.com/ethz-asl/" + name 42 | } 43 | 44 | source_suffix = ['.rst', '.md'] 45 | 46 | # Tell sphinx what the primary language being documented is. 47 | primary_domain = 'cpp' 48 | 49 | # Tell sphinx what the pygments highlight language should be. 50 | highlight_language = 'cpp' 51 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | ======= 2 | Voxblox 3 | ======= 4 | 5 | .. image:: https://cloud.githubusercontent.com/assets/5616392/15180357/536a8776-1781-11e6-8c1d-f2dfa34b1408.gif 6 | :align: center 7 | 8 | Voxblox is a volumetric mapping library based mainly on Truncated Signed Distance Fields (TSDFs). It varies from other SDF libraries in the following ways: 9 | 10 | * CPU-only, can be run single-threaded or multi-threaded for some integrators 11 | * Support for multiple different layer types (containing different types of voxels) 12 | * Serialization using protobufs 13 | * Different ways of handling weighting during merging 14 | * Different ways of inserting pose information about scans 15 | * Tight ROS integration (in voxblox_ros package) 16 | * Easily extensible with whatever integrators you want 17 | * Features an implementation of building Euclidean Signed Distance Fields (ESDFs, EDTs) directly from TSDFs. 18 | 19 | .. image:: http://i.imgur.com/2wLztFm.gif 20 | :align: center 21 | 22 | .. toctree:: 23 | :maxdepth: 3 24 | :caption: Table of Contents 25 | :glob: 26 | 27 | pages/* 28 | 29 | api/library_root 30 | 31 | Paper and Video 32 | =============== 33 | A video showing sample output from voxblox can be seen `here `_. 34 | A video of voxblox being used for online planning on-board a multicopter can be seen `here `_. 35 | 36 | If using voxblox for scientific publications, please cite the following paper, available `here `_: 37 | 38 | Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, and Roland Siegwart, “**Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning**”, in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2017.:: 39 | 40 | @inproceedings{oleynikova2017voxblox, 41 | author={Oleynikova, Helen and Taylor, Zachary and Fehr, Marius and Siegwart, Roland and Nieto, Juan}, 42 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 43 | title={Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning}, 44 | year={2017} 45 | } 46 | 47 | Credits 48 | ======= 49 | This library was written primarily by Helen Oleynikova and Marius Fehr, with significant contributions from Zachary Taylor, Alexander Millane, and others. The marching cubes meshing and ROS mesh generation were taken or heavily derived from `open_chisel `_. We've retained the copyright headers for the relevant files. 50 | 51 | .. image:: https://i.imgur.com/pvHhVsL.png 52 | :align: center 53 | -------------------------------------------------------------------------------- /docs/logo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/voxblox/c8066b04075d2fee509de295346b1c0b788c4f38/docs/logo.gif -------------------------------------------------------------------------------- /docs/pages/Class-Overview.rst: -------------------------------------------------------------------------------- 1 | ================== 2 | Map Class Overview 3 | ================== 4 | 5 | Voxblox Maps 6 | ============ 7 | 8 | Maps in Voxblox hold and maintain access to the individual voxels that contain the information about the volume. The organization of the voxels within the map is shown in the image below: 9 | 10 | .. image:: https://user-images.githubusercontent.com/730680/48905123-f638f880-ee60-11e8-9ca1-62e411320636.png 11 | :align: center 12 | 13 | Other Class Types 14 | ================= 15 | 16 | A large number of other classes exist in Voxblox that interact with the maps, see the library API for the complete list. Some of the more common class types are shown below: 17 | 18 | .. image:: https://user-images.githubusercontent.com/730680/48905124-f638f880-ee60-11e8-95d4-ceca86f61335.png 19 | :align: center 20 | -------------------------------------------------------------------------------- /docs/pages/Example-Outputs.rst: -------------------------------------------------------------------------------- 1 | =============== 2 | Example Outputs 3 | =============== 4 | 5 | Machine Hall 6 | ============ 7 | A mesh produced by Voxblox running inside a manifold mapper that fuses a SLAM systems poses with the output of a realsense D415 depthcamera. The map was generated while all systems were running fully onboard the pictured micro aerial vehicle. 8 | 9 | .. image:: https://i.imgur.com/t5DHpJh.png/ 10 | :align: center 11 | 12 | A higher resolution mesh of the same area that was processed by voxblox offline is shown below. 13 | 14 | .. image:: https://i.imgur.com/pvHhVsL.png/ 15 | :align: center 16 | 17 | Cow and Lady Dataset 18 | ==================== 19 | Voxblox running on the cow and lady dataset on a laptop equiped with an i7-4810MQ 2.80GHz CPU. In this example the system is integrating a TSDF, generating a mesh and publishing the result to RViz in real time. 20 | 21 | .. image:: http://i.imgur.com/2wLztFm.gif/ 22 | :align: center 23 | 24 | Constrained Hardware 25 | ==================== 26 | Voxblox running fully onboard the Atom processor of an Intel-Euclid. Again, the system is integrating, meshing and publishing in realtime. In this example the system was also sharing the CPU with the localization system (ROVIO) and the sensor drivers. This left around one CPU core for Voxblox to use. 27 | 28 | .. image:: http://i.imgur.com/98nAed3.gif/ 29 | :align: center 30 | 31 | KITTI Dataset 32 | ============= 33 | A mesh produced from Voxblox when run on the KITTI dataset on a Desktop PC. The given localization solution and the pointcloud produced by the Velodyne were used. 34 | 35 | .. image:: http://i.imgur.com/jAgLrZk.jpg/ 36 | :align: center 37 | 38 | EuRoC Dataset 39 | ============= 40 | A voxblox mesh produced by the Maplab library running on the Stereo data provided by the EuRoC dataset. 41 | 42 | .. image:: https://raw.githubusercontent.com/wiki/ethz-asl/maplab/readme_images/stereo.png 43 | :align: center 44 | 45 | Beach Mapping 46 | ============= 47 | A map of a beach produced by a platform with two sets of stereo cameras flying an automated coverage path. 48 | 49 | .. image:: http://i.imgur.com/uiE7WAx.gif/ 50 | :align: center 51 | -------------------------------------------------------------------------------- /docs/pages/Expanding-the-Docs.rst: -------------------------------------------------------------------------------- 1 | ================== 2 | Expanding the Docs 3 | ================== 4 | 5 | Building Locally 6 | ================ 7 | 8 | If you wish to compile the online docs locally you will need the following dependencies:: 9 | 10 | sudo apt-get install python-pip doxygen python-pyaudio 11 | pip install sphinx exhale breath sphinx_rtd_theme recommonmark --user 12 | 13 | 14 | The html documentation can then be compiled by:: 15 | 16 | cd ~/catkin_ws/src/voxblox/voxblox/docs 17 | make html 18 | 19 | this will generate a folder ``docs/_build`` with the homepage of the website located at ``docs/_build/html/index.html``. Note that while sphinx will fire off literally hundreds of warnings they can usually be ignored. 20 | 21 | Adding a Page 22 | ============= 23 | 24 | Simply add either a ``.rst`` or ``.md`` file to the ``docs/pages`` folder, it will automatically be found and included in the docs. Note that the markdown parser used does not support tables. 25 | 26 | The Docs Generation Structure 27 | ============================= 28 | 29 | The docs are generated by `Exhale `_ which automates the combined use of `Doxygen `_, `Sphnix `_ and `Breathe `_. The docs use the readthedocs theme and are hosted on `readthedocs `_ The following files are used in the docs generation: 30 | 31 | conf.py 32 | A python script that controls how Sphinx runs, responsible for loading the Breathe and Exhale extensions and setting up the project. 33 | Doxyfile 34 | A file containing all the options used by doxygen. 35 | index.rst 36 | The homepage of the generated website in ``.rst`` format. It also contains the specifications for the Sphinx TOC Tree which is displayed on the websites sidebar. 37 | logo.gif 38 | The image shown as the logo of the project. 39 | Makefile 40 | Builds the website using Sphinx. 41 | requirements.txt 42 | Used by readthedocs to install the Exhale extension. 43 | pages 44 | Folder containing the additional wiki pages in either ``.rst`` or ``.md`` format. 45 | 46 | Using the Same Documentation on Another Project 47 | =============================================== 48 | 49 | To use the same approach on another project perform the following: 50 | 51 | 1. Copy the Docs folder to the root directory of the project. 52 | 2. Change the value of the ``name`` variable in ``conf.py``. 53 | 3. Modify the index.rst to fit your new project. 54 | 4. Change the contents of the pages folder to your projects ``.md`` and ``.rst`` files. 55 | 5. Add ``docs/_build``, ``docs/doxyoutput`` and ``docs/api`` to your ``.gitignore`` file (for building locally). 56 | 6. Push the changes to github. 57 | 7. Create a new project on readthedocs.io and in advanced settings set the ``Requirements file`` path to ``docs/requirements.txt``. 58 | -------------------------------------------------------------------------------- /docs/pages/How-Does-ESDF-Generation-Work.rst: -------------------------------------------------------------------------------- 1 | ============================== 2 | How Does ESDF Generation Work? 3 | ============================== 4 | 5 | Description of the algorithm 6 | ============================ 7 | 8 | The algorithm id described in `this paper `_: 9 | 10 | Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, and Roland Siegwart, “**Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning**”, in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2017. 11 | 12 | .. code-block:: latex 13 | 14 | @inproceedings{oleynikova2017voxblox, 15 | author={Oleynikova, Helen and Taylor, Zachary and Fehr, Marius and Siegwart, Roland and Nieto, Juan}, 16 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 17 | title={Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning}, 18 | year={2017} 19 | } 20 | 21 | 22 | We have some system flowcharts below to make it easier to understand the general flow of data. 23 | 24 | How TSDF values are propagated to ESDF 25 | -------------------------------------- 26 | 27 | .. image:: https://user-images.githubusercontent.com/5616392/45752912-4dc7a780-bc17-11e8-80fe-9b5a43b373f5.png 28 | :align: center 29 | 30 | How the Raise Wavefront works 31 | ----------------------------- 32 | 33 | .. image:: https://user-images.githubusercontent.com/5616392/45752919-50c29800-bc17-11e8-9737-69929f252d85.png 34 | :align: center 35 | 36 | How the Lower Wavefront works 37 | ----------------------------- 38 | 39 | .. image:: https://user-images.githubusercontent.com/5616392/45752914-4ef8d480-bc17-11e8-93cb-4230b57eb186.png 40 | :align: center 41 | -------------------------------------------------------------------------------- /docs/pages/Installation.rst: -------------------------------------------------------------------------------- 1 | ============ 2 | Installation 3 | ============ 4 | 5 | To install voxblox, please install `ROS Indigo `_, `ROS Kinetic `_ or `ROS Melodic `_. 6 | These instructions are for Ubuntu, Voxblox will also run on OS X, but you're more or less on your own there. 7 | 8 | First install additional system dependencies (swap kinetic for indigo or melodic as necessary):: 9 | 10 | sudo apt-get install python-wstool python-catkin-tools ros-kinetic-cmake-modules protobuf-compiler autoconf libprotobuf-dev protobuf-c-compiler 11 | 12 | Next, add a few other dependencies. 13 | If you don't have a catkin workspace yet, set it up as follows:: 14 | 15 | mkdir -p ~/catkin_ws/src 16 | cd ~/catkin_ws 17 | catkin init 18 | catkin config --extend /opt/ros/kinetic 19 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 20 | catkin config --merge-devel 21 | 22 | If using `SSH keys for github `_ (recommended):: 23 | 24 | cd ~/catkin_ws/src/ 25 | git clone git@github.com:ethz-asl/voxblox.git 26 | wstool init . ./voxblox/voxblox_ssh.rosinstall 27 | wstool update 28 | 29 | 30 | If **not using SSH** keys but using https instead:: 31 | 32 | cd ~/catkin_ws/src/ 33 | git clone https://github.com/ethz-asl/voxblox.git 34 | wstool init . ./voxblox/voxblox_https.rosinstall 35 | wstool update 36 | 37 | If you have already initalized wstool replace the above ``wstool init`` with ``wstool merge -t`` 38 | 39 | Compile:: 40 | 41 | cd ~/catkin_ws/src/ 42 | catkin build voxblox_ros 43 | -------------------------------------------------------------------------------- /docs/pages/Modifying-and-Contributing.rst: -------------------------------------------------------------------------------- 1 | ======================= 2 | Contributing to Voxblox 3 | ======================= 4 | 5 | These steps are only necessary if you plan on contributing to voxblox. 6 | 7 | Code style 8 | ========== 9 | 10 | We follow the style and best practices listed in the `Google C++ Style Guide `_. 11 | 12 | Setting up the linter 13 | --------------------- 14 | 15 | This sets up a linter which checks if the code conforms to our style guide during commits. 16 | 17 | First, install the dependencies listed `here `_. 18 | 19 | .. code-block:: bash 20 | 21 | cd ~/catkin_ws/src/ 22 | git clone git@github.com:ethz-asl/linter.git 23 | cd linter 24 | echo ". $(realpath setup_linter.sh)" >> ~/.bashrc # Or the matching file for 25 | # your shell. 26 | bash 27 | 28 | # Initialize linter in voxblox repo 29 | cd ~/catkin_ws/src/voxblox 30 | init_linter_git_hooks 31 | 32 | For more information about the linter visit `ethz/linter `_ 33 | 34 | Modifying Voxblox 35 | ================= 36 | Here's some hints on how to extend voxblox to fit your needs... 37 | 38 | Serialization 39 | ------------- 40 | 41 | Serialization is currently implemented for: 42 | 43 | * TSDF layers 44 | * ESDF layers 45 | * Occupancy layers 46 | 47 | The following serialization tools are implemented: 48 | 49 | * Store a layer to file 50 | * Load layer from file 51 | * Store a subset of the blocks of a layer to file 52 | * Load blocks from file and add to a layer 53 | 54 | How to add your own voxel/layer type 55 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 56 | 57 | - Add your own voxel type and implement the ``getVoxelType()``, e.g. ``fancy_voxel.h`` : 58 | 59 | .. code-block:: bash 60 | 61 | namespace voxblox { 62 | 63 | // Used for serialization only. 64 | namespace voxel_types { 65 | const std::string kYOUR_FANCY_VOXEL = "fancy_voxel" 66 | } // namespace voxel_types 67 | 68 | template <> 69 | inline std::string getVoxelType() { 70 | return voxel_types::kYOUR_FANCY_VOXEL; 71 | } 72 | 73 | } // namespace voxblox 74 | 75 | - Implement the block (de)serialization functions for your voxel type, e.g. ``fancy_block_serialization.cc`` 76 | 77 | .. code-block:: bash 78 | 79 | namespace voxblox { 80 | 81 | template <> 82 | void Block::DeserializeVoxelData(const BlockProto& proto, 83 | YOUR_FANCY_VOXEL* voxels) { 84 | // Your serialization code. 85 | } 86 | 87 | template <> 88 | void Block::SerializeVoxelData(const YOUR_FANCY_VOXEL* voxels, 89 | BlockProto* proto) const { 90 | // Your serialization code. 91 | } 92 | 93 | } // namespace voxblox 94 | 95 | - Create your own fancy_integrator.h, fancy_mesh_integrator.h, ... 96 | 97 | **Have a look at the example package:** 98 | 99 | TODO(mfehr, helenol): add example package with a new voxel type 100 | -------------------------------------------------------------------------------- /docs/pages/Running-Voxblox.rst: -------------------------------------------------------------------------------- 1 | =============== 2 | Running Voxblox 3 | =============== 4 | 5 | The easiest way to test out voxblox is to try it out on a dataset. 6 | We have launch files for our `own dataset `_, the `Euroc Vicon Room datasets `_, and the `KITTI raw datasets `_ processed through `kitti_to_rosbag `_. 7 | 8 | For each of these datasets, there's a launch file associated under `voxblox_ros/launch`. 9 | 10 | The easiest way to start is to download the `cow and lady dataset `_, edit the path to the bagfile in ``cow_and_lady_dataset.launch``, and then simply:: 11 | 12 | roslaunch voxblox_ros cow_and_lady_dataset.launch 13 | 14 | An alternative dataset the `basement dataset `_ is also available. While this dataset lacks ground truth it demonstrates the capabilities of Voxblox running on Velodyne lidar data and uses ICP corrections to compensate for a drifting pose estimate. To run the dataset edit the path to the bagfile in ``basement_dataset.launch``, and then simply:: 15 | 16 | roslaunch voxblox_ros basement_dataset.launch 17 | 18 | 19 | If you open rviz, you should be able to see the the mesh visualized on the ``/voxblox_node/mesh`` topic of type voxblox_msgs/Mesh, in the ``world`` static frame, as shown below. One should source ``catkin_ws/devel/setup.bash`` before starting rviz, to make it recognize this topic type. 20 | 21 | The mesh only updates once per second (this is a setting in the launch file). 22 | 23 | .. image:: http://i.imgur.com/nSX5Qsh.jpg 24 | :align: center 25 | 26 | The rest of the commonly-used settings are parameters in the launch file. The the voxblox node page has the full list of settings. 27 | -------------------------------------------------------------------------------- /docs/pages/Transformations.rst: -------------------------------------------------------------------------------- 1 | ========================== 2 | Transformations in Voxblox 3 | ========================== 4 | 5 | Voxblox uses active transforms and Hamilton quaternions. For further details on the notation used throughout the code see `the minkindr wiki `_ 6 | -------------------------------------------------------------------------------- /docs/pages/Using-Voxblox-for-Planning.rst: -------------------------------------------------------------------------------- 1 | ========================== 2 | Using Voxblox for Planning 3 | ========================== 4 | 5 | The planners described in `Continuous-Time Trajectory Optimization for Online UAV Replanning `_, `Safe Local Exploration for Replanning in Cluttered Unknown Environments for Micro-Aerial Vehicles `_, and `Sparse 3D Topological Graphs for Micro-Aerial Vehicle Planning `_ will be open-sourced shortly. 6 | 7 | In the mean-time, the general idea behind using voxblox for planning is to have two nodes running: one for the mapping, which ingests pointcloud data and produces both a TSDF and an ESDF, and one for planning, which subscribes to the latest ESDF layer over ROS. 8 | 9 | The planner should have a ``voxblox::EsdfServer`` as a member, and simply remap the ``esdf_map_out`` and ``esdf_map_in`` topics to match. 10 | 11 | A sample launch file is shown below: 12 | 13 | .. code-block:: xml 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | And some scaffolding for writing your own planner using ESDF collision checking: 48 | 49 | .. code-block:: c++ 50 | 51 | class YourPlannerVoxblox { 52 | public: 53 | YourPlannerVoxblox(const ros::NodeHandle& nh, 54 | const ros::NodeHandle& nh_private); 55 | virtual ~YourPlannerVoxblox() {} 56 | double getMapDistance(const Eigen::Vector3d& position) const; 57 | private: 58 | ros::NodeHandle nh_; 59 | ros::NodeHandle nh_private_; 60 | 61 | // Map! 62 | voxblox::EsdfServer voxblox_server_; 63 | }; 64 | 65 | There's also a traversability pointcloud you can enable/disable, that if you set the radius to your robot's collision checking radius, can show you parts of the map the planner thinks are traversable in a pointcloud: 66 | 67 | .. code-block:: c++ 68 | 69 | YourPlannerVoxblox::YourPlannerVoxblox(const ros::NodeHandle& nh, 70 | const ros::NodeHandle& nh_private) 71 | : nh_(nh), 72 | nh_private_(nh_private), 73 | voxblox_server_(nh_, nh_private_) { 74 | // Optionally load a map saved with the save_map service call in voxblox. 75 | std::string input_filepath; 76 | nh_private_.param("voxblox_path", input_filepath, input_filepath); 77 | if (!input_filepath.empty()) { 78 | if (!voxblox_server_.loadMap(input_filepath)) { 79 | ROS_ERROR("Couldn't load ESDF map!"); 80 | } 81 | } 82 | double robot_radius = 1.0; 83 | voxblox_server_.setTraversabilityRadius(robot_radius); 84 | voxblox_server_.publishTraversable(); 85 | } 86 | 87 | 88 | Then to check for collisions you can just compare map distance to your robot radius: 89 | 90 | .. code-block:: c++ 91 | 92 | double YourPlannerVoxblox::getMapDistance( 93 | const Eigen::Vector3d& position) const { 94 | if (!voxblox_server_.getEsdfMapPtr()) { 95 | return 0.0; 96 | } 97 | double distance = 0.0; 98 | if (!voxblox_server_.getEsdfMapPtr()->getDistanceAtPosition(position, 99 | &distance)) { 100 | return 0.0; 101 | } 102 | return distance; 103 | } 104 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | exhale 2 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx # specify document generator. e.g) doxygen, epidoc, sphinx 2 | sphinx_root_dir: a # document directory -------------------------------------------------------------------------------- /voxblox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(voxblox) 3 | 4 | find_package(catkin_simple REQUIRED ) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | set(CMAKE_MACOSX_RPATH 0) 8 | add_definitions(-std=c++11 -Wall -Wextra) 9 | 10 | ############ 11 | # PROTOBUF # 12 | ############ 13 | # General idea: first check if we have protobuf catkin, then use that. 14 | # Otherwise use system protobuf. 15 | set(PROTO_DEFNS proto/voxblox/Block.proto 16 | proto/voxblox/Layer.proto) 17 | set(BASE_PATH "proto") 18 | set(PROTOBUF_COMPILE_PYTHON true) 19 | 20 | PROTOBUF_CATKIN_GENERATE_CPP2(${BASE_PATH} PROTO_SRCS PROTO_HDRS ${PROTO_DEFNS}) 21 | 22 | #################### 23 | # SET SOURCE FILES # 24 | #################### 25 | 26 | set("${PROJECT_NAME}_SRCS" 27 | src/alignment/icp.cc 28 | src/core/block.cc 29 | src/core/esdf_map.cc 30 | src/core/tsdf_map.cc 31 | src/integrator/esdf_integrator.cc 32 | src/integrator/esdf_occ_integrator.cc 33 | src/integrator/integrator_utils.cc 34 | src/integrator/intensity_integrator.cc 35 | src/integrator/tsdf_integrator.cc 36 | src/io/mesh_ply.cc 37 | src/io/sdf_ply.cc 38 | src/mesh/marching_cubes.cc 39 | src/simulation/objects.cc 40 | src/simulation/simulation_world.cc 41 | src/utils/camera_model.cc 42 | src/utils/evaluation_utils.cc 43 | src/utils/layer_utils.cc 44 | src/utils/neighbor_tools.cc 45 | src/utils/protobuf_utils.cc 46 | src/utils/timing.cc 47 | src/utils/voxel_utils.cc 48 | ) 49 | 50 | ############# 51 | # LIBRARIES # 52 | ############# 53 | cs_add_library(${PROJECT_NAME}_proto 54 | ${PROTO_SRCS} 55 | ) 56 | 57 | cs_add_library(${PROJECT_NAME} 58 | ${${PROJECT_NAME}_SRCS} 59 | ) 60 | target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_proto) 61 | 62 | ############ 63 | # BINARIES # 64 | ############ 65 | 66 | add_executable(tsdf_to_esdf 67 | test/tsdf_to_esdf.cc 68 | ) 69 | target_link_libraries(tsdf_to_esdf ${PROJECT_NAME}) 70 | 71 | add_executable(test_load_esdf 72 | test/test_load_esdf.cc 73 | ) 74 | target_link_libraries(test_load_esdf ${PROJECT_NAME}) 75 | 76 | ######### 77 | # TESTS # 78 | ######### 79 | add_custom_target(test_data) 80 | add_custom_command(TARGET test_data 81 | COMMAND rm -rf test_data 82 | COMMAND mkdir -p test_data 83 | COMMAND cp -r ${CMAKE_SOURCE_DIR}/test/test_data/* 84 | test_data/ || :) 85 | 86 | #add_definitions(-DVISUALIZE_UNIT_TEST_RESULTS) 87 | 88 | catkin_add_gtest(test_approx_hash_array 89 | test/test_approx_hash_array.cc 90 | ) 91 | target_link_libraries(test_approx_hash_array ${PROJECT_NAME}) 92 | 93 | catkin_add_gtest(test_tsdf_map 94 | test/test_tsdf_map.cc 95 | ) 96 | target_link_libraries(test_tsdf_map ${PROJECT_NAME}) 97 | 98 | catkin_add_gtest(test_protobuf 99 | test/test_protobuf.cc 100 | ) 101 | target_link_libraries(test_protobuf ${PROJECT_NAME}) 102 | 103 | catkin_add_gtest(test_tsdf_interpolator 104 | test/test_tsdf_interpolator.cc 105 | ) 106 | target_link_libraries(test_tsdf_interpolator ${PROJECT_NAME}) 107 | 108 | catkin_add_gtest(test_layer 109 | test/test_layer.cc 110 | ) 111 | target_link_libraries(test_layer ${PROJECT_NAME}) 112 | 113 | catkin_add_gtest(test_merge_integration 114 | test/test_merge_integration.cc 115 | ) 116 | target_link_libraries(test_merge_integration ${PROJECT_NAME}) 117 | 118 | catkin_add_gtest(test_layer_utils 119 | test/test_layer_utils.cc 120 | ) 121 | target_link_libraries(test_layer_utils ${PROJECT_NAME}) 122 | 123 | catkin_add_gtest(test_sdf_integrators 124 | test/test_sdf_integrators.cc 125 | ) 126 | target_link_libraries(test_sdf_integrators ${PROJECT_NAME}) 127 | 128 | catkin_add_gtest(test_bucket_queue 129 | test/test_bucket_queue.cc 130 | ) 131 | target_link_libraries(test_bucket_queue ${PROJECT_NAME}) 132 | 133 | catkin_add_gtest(test_clear_spheres 134 | test/test_clear_spheres.cc 135 | ) 136 | target_link_libraries(test_clear_spheres ${PROJECT_NAME}) 137 | 138 | ########## 139 | # EXPORT # 140 | ########## 141 | cs_install() 142 | cs_export(INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include) 143 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/core/block_hash.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_BLOCK_HASH_H_ 2 | #define VOXBLOX_CORE_BLOCK_HASH_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "voxblox/core/common.h" 12 | 13 | namespace voxblox { 14 | 15 | /** 16 | * Performs deco hashing on block indexes. Based on recommendations of 17 | * "Investigating the impact of Suboptimal Hashing Functions" by L. Buckley et 18 | * al. 19 | */ 20 | struct AnyIndexHash { 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | /// number was arbitrarily chosen with no good justification 24 | static constexpr size_t sl = 17191; 25 | static constexpr size_t sl2 = sl * sl; 26 | 27 | std::size_t operator()(const AnyIndex& index) const { 28 | return static_cast(index.x() + index.y() * sl + 29 | index.z() * sl2); 30 | } 31 | }; 32 | 33 | template 34 | struct AnyIndexHashMapType { 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | 37 | typedef std::unordered_map< 38 | AnyIndex, ValueType, AnyIndexHash, std::equal_to, 39 | Eigen::aligned_allocator > > 40 | type; 41 | }; 42 | 43 | typedef std::unordered_set, 44 | Eigen::aligned_allocator > 45 | IndexSet; 46 | 47 | typedef typename AnyIndexHashMapType::type HierarchicalIndexMap; 48 | 49 | typedef typename AnyIndexHashMapType::type HierarchicalIndexSet; 50 | 51 | typedef typename HierarchicalIndexMap::value_type HierarchicalIndex; 52 | 53 | /// Hash for large index values, see AnyIndexHash. 54 | struct LongIndexHash { 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | static constexpr size_t sl = 17191; 58 | static constexpr size_t sl2 = sl * sl; 59 | 60 | std::size_t operator()(const LongIndex& index) const { 61 | return static_cast(index.x() + index.y() * sl + 62 | index.z() * sl2); 63 | } 64 | }; 65 | 66 | template 67 | struct LongIndexHashMapType { 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 69 | 70 | typedef std::unordered_map< 71 | LongIndex, ValueType, LongIndexHash, std::equal_to, 72 | Eigen::aligned_allocator > > 73 | type; 74 | }; 75 | 76 | typedef std::unordered_set, 77 | Eigen::aligned_allocator > 78 | LongIndexSet; 79 | 80 | } // namespace voxblox 81 | 82 | #endif // VOXBLOX_CORE_BLOCK_HASH_H_ 83 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/core/block_inl.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_BLOCK_INL_H_ 2 | #define VOXBLOX_CORE_BLOCK_INL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxblox/Block.pb.h" 8 | #include "voxblox/utils/voxel_utils.h" 9 | 10 | namespace voxblox { 11 | 12 | template 13 | size_t Block::computeLinearIndexFromVoxelIndex( 14 | const VoxelIndex& index) const { 15 | size_t linear_index = static_cast( 16 | index.x() + 17 | voxels_per_side_ * (index.y() + index.z() * voxels_per_side_)); 18 | 19 | DCHECK(index.x() >= 0 && index.x() < static_cast(voxels_per_side_)); 20 | DCHECK(index.y() >= 0 && index.y() < static_cast(voxels_per_side_)); 21 | DCHECK(index.z() >= 0 && index.z() < static_cast(voxels_per_side_)); 22 | 23 | DCHECK_LT(linear_index, 24 | voxels_per_side_ * voxels_per_side_ * voxels_per_side_); 25 | DCHECK_GE(linear_index, 0u); 26 | return linear_index; 27 | } 28 | 29 | template 30 | VoxelIndex Block::computeTruncatedVoxelIndexFromCoordinates( 31 | const Point& coords) const { 32 | const IndexElement max_value = voxels_per_side_ - 1; 33 | VoxelIndex voxel_index = 34 | getGridIndexFromPoint(coords - origin_, voxel_size_inv_); 35 | // check is needed as getGridIndexFromPoint gives results that have a tiny 36 | // chance of being outside the valid voxel range. 37 | return VoxelIndex(std::max(std::min(voxel_index.x(), max_value), 0), 38 | std::max(std::min(voxel_index.y(), max_value), 0), 39 | std::max(std::min(voxel_index.z(), max_value), 0)); 40 | } 41 | 42 | template 43 | VoxelIndex Block::computeVoxelIndexFromLinearIndex( 44 | size_t linear_index) const { 45 | int rem = linear_index; 46 | VoxelIndex result; 47 | std::div_t div_temp = std::div(rem, voxels_per_side_ * voxels_per_side_); 48 | rem = div_temp.rem; 49 | result.z() = div_temp.quot; 50 | div_temp = std::div(rem, voxels_per_side_); 51 | result.y() = div_temp.quot; 52 | result.x() = div_temp.rem; 53 | return result; 54 | } 55 | 56 | template 57 | bool Block::isValidVoxelIndex(const VoxelIndex& index) const { 58 | if (index.x() < 0 || 59 | index.x() >= static_cast(voxels_per_side_)) { 60 | return false; 61 | } 62 | if (index.y() < 0 || 63 | index.y() >= static_cast(voxels_per_side_)) { 64 | return false; 65 | } 66 | if (index.z() < 0 || 67 | index.z() >= static_cast(voxels_per_side_)) { 68 | return false; 69 | } 70 | return true; 71 | } 72 | 73 | template 74 | Block::Block(const BlockProto& proto) 75 | : Block(proto.voxels_per_side(), proto.voxel_size(), 76 | Point(proto.origin_x(), proto.origin_y(), proto.origin_z())) { 77 | has_data_ = proto.has_data(); 78 | 79 | // Convert the data into a vector of integers. 80 | std::vector data; 81 | data.reserve(proto.voxel_data_size()); 82 | 83 | for (uint32_t word : proto.voxel_data()) { 84 | data.push_back(word); 85 | } 86 | 87 | deserializeFromIntegers(data); 88 | } 89 | 90 | template 91 | void Block::getProto(BlockProto* proto) const { 92 | CHECK_NOTNULL(proto); 93 | 94 | proto->set_voxels_per_side(voxels_per_side_); 95 | proto->set_voxel_size(voxel_size_); 96 | 97 | proto->set_origin_x(origin_.x()); 98 | proto->set_origin_y(origin_.y()); 99 | proto->set_origin_z(origin_.z()); 100 | 101 | proto->set_has_data(has_data_); 102 | 103 | std::vector data; 104 | serializeToIntegers(&data); 105 | // Not quite actually a word since we're in a 64-bit age now, but whatever. 106 | for (uint32_t word : data) { 107 | proto->add_voxel_data(word); 108 | } 109 | } 110 | 111 | template 112 | void Block::mergeBlock(const Block& other_block) { 113 | CHECK_EQ(other_block.voxel_size(), voxel_size()); 114 | CHECK_EQ(other_block.voxels_per_side(), voxels_per_side()); 115 | 116 | if (!other_block.has_data()) { 117 | return; 118 | } else { 119 | has_data() = true; 120 | updated().set(); 121 | 122 | for (IndexElement voxel_idx = 0; 123 | voxel_idx < static_cast(num_voxels()); ++voxel_idx) { 124 | mergeVoxelAIntoVoxelB( 125 | other_block.getVoxelByLinearIndex(voxel_idx), 126 | &(getVoxelByLinearIndex(voxel_idx))); 127 | } 128 | } 129 | } 130 | 131 | template 132 | size_t Block::getMemorySize() const { 133 | size_t size = 0u; 134 | 135 | // Calculate size of members 136 | size += sizeof(voxels_per_side_); 137 | size += sizeof(voxel_size_); 138 | size += sizeof(origin_); 139 | size += sizeof(num_voxels_); 140 | size += sizeof(voxel_size_inv_); 141 | size += sizeof(block_size_); 142 | 143 | size += sizeof(has_data_); 144 | size += sizeof(updated_); 145 | 146 | if (num_voxels_ > 0u) { 147 | size += (num_voxels_ * sizeof(voxels_[0])); 148 | } 149 | return size; 150 | } 151 | 152 | } // namespace voxblox 153 | 154 | #endif // VOXBLOX_CORE_BLOCK_INL_H_ 155 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/core/color.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_COLOR_H_ 2 | #define VOXBLOX_CORE_COLOR_H_ 3 | 4 | #include "voxblox/core/common.h" 5 | 6 | namespace voxblox { 7 | 8 | // Color maps. 9 | 10 | /** 11 | * Maps an input h from a value between 0.0 and 1.0 into a rainbow. Copied from 12 | * OctomapProvider in octomap. 13 | */ 14 | inline Color rainbowColorMap(double h) { 15 | Color color; 16 | color.a = 255; 17 | // blend over HSV-values (more colors) 18 | 19 | double s = 1.0; 20 | double v = 1.0; 21 | 22 | h -= floor(h); 23 | h *= 6; 24 | int i; 25 | double m, n, f; 26 | 27 | i = floor(h); 28 | f = h - i; 29 | if (!(i & 1)) f = 1 - f; // if i is even 30 | m = v * (1 - s); 31 | n = v * (1 - s * f); 32 | 33 | switch (i) { 34 | case 6: 35 | case 0: 36 | color.r = 255 * v; 37 | color.g = 255 * n; 38 | color.b = 255 * m; 39 | break; 40 | case 1: 41 | color.r = 255 * n; 42 | color.g = 255 * v; 43 | color.b = 255 * m; 44 | break; 45 | case 2: 46 | color.r = 255 * m; 47 | color.g = 255 * v; 48 | color.b = 255 * n; 49 | break; 50 | case 3: 51 | color.r = 255 * m; 52 | color.g = 255 * n; 53 | color.b = 255 * v; 54 | break; 55 | case 4: 56 | color.r = 255 * n; 57 | color.g = 255 * m; 58 | color.b = 255 * v; 59 | break; 60 | case 5: 61 | color.r = 255 * v; 62 | color.g = 255 * m; 63 | color.b = 255 * n; 64 | break; 65 | default: 66 | color.r = 255; 67 | color.g = 127; 68 | color.b = 127; 69 | break; 70 | } 71 | 72 | return color; 73 | } 74 | 75 | /// Maps an input h from a value between 0.0 and 1.0 into a grayscale color. 76 | inline Color grayColorMap(double h) { 77 | Color color; 78 | color.a = 255; 79 | 80 | color.r = round(h * 255); 81 | color.b = color.r; 82 | color.g = color.r; 83 | 84 | return color; 85 | } 86 | 87 | inline Color randomColor() { 88 | Color color; 89 | 90 | color.a = 255; 91 | 92 | color.r = rand() % 256; 93 | color.b = rand() % 256; 94 | color.g = rand() % 256; 95 | 96 | return color; 97 | } 98 | 99 | } // namespace voxblox 100 | 101 | #endif // VOXBLOX_CORE_COLOR_H_ 102 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/core/occupancy_map.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_OCCUPANCY_MAP_H_ 2 | #define VOXBLOX_CORE_OCCUPANCY_MAP_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "voxblox/core/common.h" 10 | #include "voxblox/core/layer.h" 11 | #include "voxblox/core/voxel.h" 12 | 13 | namespace voxblox { 14 | /// Map holding an Occupancy Layer, inspired by Octomap. 15 | class OccupancyMap { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | typedef std::shared_ptr Ptr; 20 | 21 | struct Config { 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | FloatingPoint occupancy_voxel_size = 0.2; 25 | size_t occupancy_voxels_per_side = 16u; 26 | }; 27 | 28 | explicit OccupancyMap(const Config& config) 29 | : occupancy_layer_(new Layer( 30 | config.occupancy_voxel_size, config.occupancy_voxels_per_side)) { 31 | block_size_ = 32 | config.occupancy_voxel_size * config.occupancy_voxels_per_side; 33 | } 34 | 35 | // Creates a new OccupancyMap based on a COPY of this layer. 36 | explicit OccupancyMap(const Layer& layer) 37 | : OccupancyMap(aligned_shared>(layer)) {} 38 | 39 | // Creates a new OccupancyMap that contains this layer. 40 | explicit OccupancyMap(Layer::Ptr layer) 41 | : occupancy_layer_(layer) { 42 | CHECK(layer); 43 | block_size_ = layer->block_size(); 44 | } 45 | 46 | virtual ~OccupancyMap() {} 47 | 48 | Layer* getOccupancyLayerPtr() { 49 | return occupancy_layer_.get(); 50 | } 51 | const Layer& getOccupancyLayer() const { 52 | return *occupancy_layer_; 53 | } 54 | 55 | FloatingPoint block_size() const { return block_size_; } 56 | 57 | protected: 58 | FloatingPoint block_size_; 59 | 60 | // The layers. 61 | Layer::Ptr occupancy_layer_; 62 | }; 63 | 64 | } // namespace voxblox 65 | 66 | #endif // VOXBLOX_CORE_OCCUPANCY_MAP_H_ 67 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/core/tsdf_map.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_TSDF_MAP_H_ 2 | #define VOXBLOX_CORE_TSDF_MAP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "voxblox/core/common.h" 11 | #include "voxblox/core/layer.h" 12 | #include "voxblox/core/voxel.h" 13 | #include "voxblox/interpolator/interpolator.h" 14 | 15 | namespace voxblox { 16 | /** 17 | * Map holding a Truncated Signed Distance Field Layer. Contains functions for 18 | * interacting with the layer and getting gradient and distance information. 19 | */ 20 | class TsdfMap { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | typedef std::shared_ptr Ptr; 25 | 26 | struct Config { 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | FloatingPoint tsdf_voxel_size = 0.2; 30 | size_t tsdf_voxels_per_side = 16u; 31 | 32 | std::string print() const; 33 | }; 34 | 35 | explicit TsdfMap(const Config& config) 36 | : tsdf_layer_(new Layer(config.tsdf_voxel_size, 37 | config.tsdf_voxels_per_side)), 38 | interpolator_(tsdf_layer_.get()) { 39 | block_size_ = config.tsdf_voxel_size * config.tsdf_voxels_per_side; 40 | } 41 | 42 | /// Creates a new TsdfMap based on a COPY of this layer. 43 | explicit TsdfMap(const Layer& layer) 44 | : TsdfMap(aligned_shared>(layer)) {} 45 | 46 | /// Creates a new TsdfMap that contains this layer. 47 | explicit TsdfMap(Layer::Ptr layer) 48 | : tsdf_layer_(layer), interpolator_(tsdf_layer_.get()) { 49 | if (!layer) { 50 | /* NOTE(mereweth@jpl.nasa.gov) - throw std exception for Python to catch 51 | * This is idiomatic when wrapping C++ code for Python, especially with 52 | * pybind11 53 | */ 54 | throw std::runtime_error(std::string("Null Layer::Ptr") + 55 | " in TsdfMap constructor"); 56 | } 57 | 58 | CHECK(layer); 59 | block_size_ = layer->block_size(); 60 | } 61 | 62 | virtual ~TsdfMap() {} 63 | 64 | Layer* getTsdfLayerPtr() { return tsdf_layer_.get(); } 65 | const Layer* getTsdfLayerConstPtr() const { 66 | return tsdf_layer_.get(); 67 | } 68 | const Layer& getTsdfLayer() const { return *tsdf_layer_; } 69 | 70 | FloatingPoint block_size() const { return block_size_; } 71 | FloatingPoint voxel_size() const { return tsdf_layer_->voxel_size(); } 72 | 73 | /* NOTE(mereweth@jpl.nasa.gov) 74 | * EigenDRef is fully dynamic stride type alias for Numpy array slices 75 | * Use column-major matrices; column-by-column traversal is faster 76 | * Convenience alias borrowed from pybind11 77 | */ 78 | using EigenDStride = Eigen::Stride; 79 | template 80 | using EigenDRef = Eigen::Ref; 81 | 82 | /** 83 | * Extract all voxels on a slice plane that is parallel to one of the 84 | * axis-aligned planes. free_plane_index specifies the free coordinate 85 | * (zero-based; x, y, z order) free_plane_val specifies the plane intercept 86 | * coordinate along that axis 87 | */ 88 | unsigned int coordPlaneSliceGetDistanceWeight( 89 | unsigned int free_plane_index, double free_plane_val, 90 | EigenDRef>& positions, 91 | Eigen::Ref distances, 92 | Eigen::Ref weights, unsigned int max_points) const; 93 | 94 | bool getWeightAtPosition(const Eigen::Vector3d& position, 95 | double* weight) const; 96 | bool getWeightAtPosition(const Eigen::Vector3d& position, 97 | const bool interpolate, double* weight) const; 98 | 99 | protected: 100 | FloatingPoint block_size_; 101 | 102 | // The layers. 103 | Layer::Ptr tsdf_layer_; 104 | 105 | // Interpolator for the layer. 106 | Interpolator interpolator_; 107 | }; 108 | 109 | } // namespace voxblox 110 | 111 | #endif // VOXBLOX_CORE_TSDF_MAP_H_ 112 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/core/voxel.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_VOXEL_H_ 2 | #define VOXBLOX_CORE_VOXEL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxblox/core/color.h" 8 | #include "voxblox/core/common.h" 9 | 10 | namespace voxblox { 11 | 12 | struct TsdfVoxel { 13 | float distance = 0.0f; 14 | float weight = 0.0f; 15 | Color color; 16 | }; 17 | 18 | struct EsdfVoxel { 19 | float distance = 0.0f; 20 | 21 | bool observed = false; 22 | /** 23 | * Whether the voxel was copied from the TSDF (false) or created from a pose 24 | * or some other source (true). This member is not serialized!!! 25 | */ 26 | bool hallucinated = false; 27 | bool in_queue = false; 28 | bool fixed = false; 29 | 30 | /** 31 | * Relative direction toward parent. If itself, then either uninitialized 32 | * or in the fixed frontier. 33 | */ 34 | Eigen::Vector3i parent = Eigen::Vector3i::Zero(); 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | }; 38 | 39 | struct OccupancyVoxel { 40 | float probability_log = 0.0f; 41 | bool observed = false; 42 | }; 43 | 44 | struct IntensityVoxel { 45 | float intensity = 0.0f; 46 | float weight = 0.0f; 47 | }; 48 | 49 | /// Used for serialization only. 50 | namespace voxel_types { 51 | const std::string kNotSerializable = "not_serializable"; 52 | const std::string kTsdf = "tsdf"; 53 | const std::string kEsdf = "esdf"; 54 | const std::string kOccupancy = "occupancy"; 55 | const std::string kIntensity = "intensity"; 56 | } // namespace voxel_types 57 | 58 | template 59 | std::string getVoxelType() { 60 | return voxel_types::kNotSerializable; 61 | } 62 | 63 | template <> 64 | inline std::string getVoxelType() { 65 | return voxel_types::kTsdf; 66 | } 67 | 68 | template <> 69 | inline std::string getVoxelType() { 70 | return voxel_types::kEsdf; 71 | } 72 | 73 | template <> 74 | inline std::string getVoxelType() { 75 | return voxel_types::kOccupancy; 76 | } 77 | 78 | template <> 79 | inline std::string getVoxelType() { 80 | return voxel_types::kIntensity; 81 | } 82 | 83 | } // namespace voxblox 84 | 85 | #endif // VOXBLOX_CORE_VOXEL_H_ 86 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/integrator/esdf_occ_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_INTEGRATOR_ESDF_OCC_INTEGRATOR_H_ 2 | #define VOXBLOX_INTEGRATOR_ESDF_OCC_INTEGRATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "voxblox/core/layer.h" 13 | #include "voxblox/core/voxel.h" 14 | #include "voxblox/integrator/integrator_utils.h" 15 | #include "voxblox/utils/bucket_queue.h" 16 | #include "voxblox/utils/timing.h" 17 | 18 | namespace voxblox { 19 | 20 | /** 21 | * Builds an ESDF layer out of a given occupancy layer. 22 | */ 23 | class EsdfOccIntegrator { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | struct Config { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | /** 31 | * Maximum distance to calculate the actual distance to. 32 | * Any values above this will be set to default_distance_m. 33 | */ 34 | FloatingPoint max_distance_m = 2.0; 35 | /// Default distance set for unknown values and values > max_distance_m. 36 | FloatingPoint default_distance_m = 2.0; 37 | /// Number of buckets for the bucketed priority queue. 38 | int num_buckets = 20; 39 | }; 40 | 41 | EsdfOccIntegrator(const Config& config, Layer* occ_layer, 42 | Layer* esdf_layer); 43 | 44 | /** 45 | * Fixed is overloaded as occupied in this case. 46 | * Only batch operations are currently supported for the occupancy map. 47 | */ 48 | void updateFromOccLayerBatch(); 49 | void updateFromOccBlocks(const BlockIndexList& occ_blocks); 50 | 51 | void processOpenSet(); 52 | 53 | /** 54 | * Uses 26-connectivity and quasi-Euclidean distances. 55 | * Directions is the direction that the neighbor voxel lives in. If you 56 | * need the direction FROM the neighbor voxel TO the current voxel, take 57 | * negative of the given direction. 58 | */ 59 | void getNeighborsAndDistances( 60 | const BlockIndex& block_index, const VoxelIndex& voxel_index, 61 | AlignedVector* neighbors, AlignedVector* distances, 62 | AlignedVector* directions) const; 63 | void getNeighbor(const BlockIndex& block_index, const VoxelIndex& voxel_index, 64 | const Eigen::Vector3i& direction, 65 | BlockIndex* neighbor_block_index, 66 | VoxelIndex* neighbor_voxel_index) const; 67 | 68 | protected: 69 | Config config_; 70 | 71 | Layer* occ_layer_; 72 | Layer* esdf_layer_; 73 | 74 | /** 75 | * Open Queue for incremental updates. Contains global voxel indices 76 | * for the ESDF layer. 77 | */ 78 | BucketQueue open_; 79 | 80 | /** Raise set for updates; these are values that used to be in the fixed 81 | * frontier and now have a higher value, or their children which need to have 82 | * their values invalidated. 83 | */ 84 | AlignedQueue raise_; 85 | 86 | size_t esdf_voxels_per_side_; 87 | FloatingPoint esdf_voxel_size_; 88 | }; 89 | 90 | } // namespace voxblox 91 | 92 | #endif // VOXBLOX_INTEGRATOR_ESDF_OCC_INTEGRATOR_H_ 93 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/integrator/intensity_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_INTEGRATOR_INTENSITY_INTEGRATOR_H_ 2 | #define VOXBLOX_INTEGRATOR_INTENSITY_INTEGRATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "voxblox/core/layer.h" 13 | #include "voxblox/core/voxel.h" 14 | #include "voxblox/integrator/integrator_utils.h" 15 | #include "voxblox/utils/timing.h" 16 | 17 | namespace voxblox { 18 | 19 | /** 20 | * Integrates intensities from a set of bearing vectors (i.e., an intensity 21 | * image, such as a thermal image) by projecting them onto the TSDF surface 22 | * and coloring near the surface crossing. 23 | */ 24 | class IntensityIntegrator { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | IntensityIntegrator(const Layer& tsdf_layer, 29 | Layer* intensity_layer); 30 | 31 | /// Set the max distance for projecting into the TSDF layer. 32 | void setMaxDistance(const FloatingPoint max_distance) { 33 | max_distance_ = max_distance; 34 | } 35 | FloatingPoint getMaxDistance() const { return max_distance_; } 36 | 37 | /** 38 | * Integrates intensities into the intensity layer by projecting normalized 39 | * bearing vectors (in the WORLD coordinate frame) from the origin (also in 40 | * the world coordinate frame) into the TSDF layer, and then setting the 41 | * intensities near the surface boundaries. 42 | */ 43 | void addIntensityBearingVectors(const Point& origin, 44 | const Pointcloud& bearing_vectors, 45 | const std::vector& intensities); 46 | 47 | private: 48 | FloatingPoint max_distance_; 49 | float max_weight_; 50 | /// Number of voxels to propagate from the surface along the bearing vector. 51 | int intensity_prop_voxel_radius_; 52 | 53 | const Layer& tsdf_layer_; 54 | Layer* intensity_layer_; 55 | }; 56 | 57 | } // namespace voxblox 58 | 59 | #endif // VOXBLOX_INTEGRATOR_INTENSITY_INTEGRATOR_H_ 60 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/interpolator/interpolator.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_INTERPOLATOR_INTERPOLATOR_H_ 2 | #define VOXBLOX_INTERPOLATOR_INTERPOLATOR_H_ 3 | 4 | #include 5 | 6 | #include "voxblox/core/common.h" 7 | #include "voxblox/core/layer.h" 8 | #include "voxblox/core/voxel.h" 9 | 10 | namespace voxblox { 11 | 12 | /** 13 | * Interpolates voxels to give distances and gradients 14 | */ 15 | template 16 | class Interpolator { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | typedef std::shared_ptr Ptr; 21 | 22 | explicit Interpolator(const Layer* layer); 23 | 24 | bool getGradient(const Point& pos, Point* grad, 25 | const bool interpolate = false) const; 26 | 27 | bool getDistance(const Point& pos, FloatingPoint* distance, 28 | bool interpolate = false) const; 29 | 30 | bool getWeight(const Point& pos, FloatingPoint* weight, 31 | bool interpolate = false) const; 32 | 33 | bool getVoxel(const Point& pos, VoxelType* voxel, 34 | bool interpolate = false) const; 35 | 36 | /** 37 | * This tries to use whatever information is available to interpolate the 38 | * distance and gradient -- if only one side is available, for instance, 39 | * this will still estimate a 1-sided gradient. Should give the same results 40 | * as getGradient() and getDistance() if all neighbors are filled. 41 | */ 42 | bool getAdaptiveDistanceAndGradient(const Point& pos, FloatingPoint* distance, 43 | Point* grad) const; 44 | 45 | /// Without interpolation. 46 | bool getNearestDistanceAndWeight(const Point& pos, FloatingPoint* distance, 47 | float* weight) const; 48 | 49 | bool setIndexes(const Point& pos, BlockIndex* block_index, 50 | InterpIndexes* voxel_indexes) const; 51 | 52 | bool getVoxelsAndQVector(const Point& pos, const VoxelType** voxels, 53 | InterpVector* q_vector) const; 54 | 55 | private: 56 | /** 57 | * Q vector from http://spie.org/samples/PM159.pdf 58 | * Relates the interpolation distance of any arbitrary point inside a voxel 59 | * to the values of the voxel corners. 60 | */ 61 | void getQVector(const Point& voxel_pos, const Point& pos, 62 | const FloatingPoint voxel_size_inv, 63 | InterpVector* q_vector) const; 64 | 65 | bool getVoxelsAndQVector(const BlockIndex& block_index, 66 | const InterpIndexes& voxel_indexes, const Point& pos, 67 | const VoxelType** voxels, 68 | InterpVector* q_vector) const; 69 | 70 | bool getInterpDistance(const Point& pos, FloatingPoint* distance) const; 71 | 72 | bool getNearestDistance(const Point& pos, FloatingPoint* distance) const; 73 | 74 | bool getInterpWeight(const Point& pos, FloatingPoint* weight) const; 75 | 76 | bool getNearestWeight(const Point& pos, FloatingPoint* weight) const; 77 | 78 | bool getInterpVoxel(const Point& pos, VoxelType* voxel) const; 79 | 80 | bool getNearestVoxel(const Point& pos, VoxelType* voxel) const; 81 | 82 | static float getVoxelSdf(const VoxelType& voxel); 83 | static float getVoxelWeight(const VoxelType& voxel); 84 | 85 | static uint8_t getRed(const VoxelType& voxel); 86 | static uint8_t getBlue(const VoxelType& voxel); 87 | static uint8_t getGreen(const VoxelType& voxel); 88 | static uint8_t getAlpha(const VoxelType& voxel); 89 | 90 | template 91 | static FloatingPoint interpMember(const InterpVector& q_vector, 92 | const VoxelType** voxels, 93 | TGetter (*getter)(const VoxelType&)); 94 | 95 | static VoxelType interpVoxel(const InterpVector& q_vector, 96 | const VoxelType** voxels); 97 | 98 | const Layer* layer_; 99 | }; 100 | 101 | } // namespace voxblox 102 | 103 | #endif // VOXBLOX_INTERPOLATOR_INTERPOLATOR_H_ 104 | 105 | #include "voxblox/interpolator/interpolator_inl.h" 106 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/io/layer_io.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_IO_LAYER_IO_H_ 2 | #define VOXBLOX_IO_LAYER_IO_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "voxblox/core/common.h" 9 | #include "voxblox/core/layer.h" 10 | 11 | namespace voxblox { 12 | namespace io { 13 | 14 | /** 15 | * By default, loads blocks without multiple layer support. 16 | * Loading blocks assumes that the layer is already setup and allocated. 17 | */ 18 | template 19 | bool LoadBlocksFromFile( 20 | const std::string& file_path, 21 | typename Layer::BlockMergingStrategy strategy, 22 | Layer* layer_ptr); 23 | 24 | /** 25 | * By default, loads blocks without multiple layer support. 26 | * Loading the full layer allocates a layer of the correct size. 27 | */ 28 | template 29 | bool LoadBlocksFromFile( 30 | const std::string& file_path, 31 | typename Layer::BlockMergingStrategy strategy, 32 | bool multiple_layer_support, Layer* layer_ptr); 33 | 34 | template 35 | bool LoadBlocksFromStream( 36 | const size_t num_blocks, 37 | typename Layer::BlockMergingStrategy strategy, 38 | std::istream* proto_file_ptr, Layer* layer_ptr, 39 | uint64_t* tmp_byte_offset_ptr); 40 | 41 | /** 42 | * Unlike LoadBlocks above, this actually allocates the layer as well. 43 | * By default loads without multiple layer support (i.e., only checks the first 44 | * layer in the file). 45 | */ 46 | template 47 | bool LoadLayer(const std::string& file_path, 48 | typename Layer::Ptr* layer_ptr); 49 | 50 | /** 51 | * Unlike LoadBlocks, this actually allocates the layer as well. 52 | * By default loads without multiple layer support (i.e., only checks the first 53 | * layer in the file). 54 | */ 55 | template 56 | bool LoadLayer(const std::string& file_path, const bool multiple_layer_support, 57 | typename Layer::Ptr* layer_ptr); 58 | 59 | /** 60 | * By default, clears (truncates) the output file. Set clear_file to false in 61 | * case writing the second (or subsequent) layer into the same file. 62 | */ 63 | template 64 | bool SaveLayer(const Layer& layer, const std::string& file_path, 65 | bool clear_file = true); 66 | 67 | /// Saves only some parts of the layer to the file. Clears the file by default. 68 | template 69 | bool SaveLayerSubset(const Layer& layer, 70 | const std::string& file_path, 71 | const BlockIndexList& blocks_to_include, 72 | bool include_all_blocks); 73 | 74 | } // namespace io 75 | } // namespace voxblox 76 | 77 | #include "voxblox/io/layer_io_inl.h" 78 | 79 | #endif // VOXBLOX_IO_LAYER_IO_H_ 80 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/io/mesh_ply.h: -------------------------------------------------------------------------------- 1 | // NOTE: From open_chisel: github.com/personalrobotics/OpenChisel/ 2 | // The MIT License (MIT) 3 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef VOXBLOX_IO_MESH_PLY_H_ 24 | #define VOXBLOX_IO_MESH_PLY_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "voxblox/mesh/mesh_layer.h" 31 | 32 | namespace voxblox { 33 | 34 | /** 35 | * Generates a mesh from the mesh layer. 36 | * @param connected_mesh if true veracities will be shared between triangles 37 | * @param vertex_proximity_threshold verticies that are within the specified 38 | * thershold distance will be merged together, simplifying the mesh. 39 | */ 40 | bool convertMeshLayerToMesh( 41 | const MeshLayer& mesh_layer, Mesh* mesh, const bool connected_mesh = true, 42 | const FloatingPoint vertex_proximity_threshold = 1e-10); 43 | 44 | /// Default behaviour is to simplify the mesh. 45 | bool outputMeshLayerAsPly(const std::string& filename, 46 | const MeshLayer& mesh_layer); 47 | 48 | /** 49 | * @param connected_mesh if true vertices will be shared between triangles 50 | */ 51 | bool outputMeshLayerAsPly(const std::string& filename, 52 | const bool connected_mesh, 53 | const MeshLayer& mesh_layer); 54 | 55 | bool outputMeshAsPly(const std::string& filename, const Mesh& mesh); 56 | 57 | } // namespace voxblox 58 | 59 | #endif // VOXBLOX_IO_MESH_PLY_H_ 60 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/io/ply_writer.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_IO_PLY_WRITER_H_ 2 | #define VOXBLOX_IO_PLY_WRITER_H_ 3 | 4 | #include // NOLINT 5 | #include 6 | 7 | #include 8 | 9 | #include "voxblox/core/common.h" 10 | 11 | namespace voxblox { 12 | 13 | namespace io { 14 | /** 15 | * Writes a mesh to a .ply file. For reference on the format, see: 16 | * http://paulbourke.net/dataformats/ply/ 17 | */ 18 | class PlyWriter { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | explicit PlyWriter(const std::string& filename) 23 | : header_written_(false), 24 | parameters_set_(false), 25 | vertices_total_(0), 26 | vertices_written_(0), 27 | has_color_(false), 28 | file_(filename) {} 29 | 30 | virtual ~PlyWriter() {} 31 | 32 | void addVerticesWithProperties(size_t num_vertices, bool has_color) { 33 | vertices_total_ = num_vertices; 34 | has_color_ = has_color; 35 | parameters_set_ = true; 36 | } 37 | 38 | bool writeHeader() { 39 | if (!file_) { 40 | // Output a warning -- couldn't open file? 41 | LOG(WARNING) << "Could not open file for PLY output."; 42 | return false; 43 | } 44 | if (!parameters_set_) { 45 | LOG(WARNING) << "Could not write header out -- parameters not set."; 46 | return false; 47 | } 48 | if (header_written_) { 49 | LOG(WARNING) << "Header already written."; 50 | return false; 51 | } 52 | 53 | file_ << "ply" << std::endl; 54 | file_ << "format ascii 1.0" << std::endl; 55 | file_ << "element vertex " << vertices_total_ << std::endl; 56 | file_ << "property float x" << std::endl; 57 | file_ << "property float y" << std::endl; 58 | file_ << "property float z" << std::endl; 59 | 60 | if (has_color_) { 61 | file_ << "property uchar red" << std::endl; 62 | file_ << "property uchar green" << std::endl; 63 | file_ << "property uchar blue" << std::endl; 64 | } 65 | 66 | file_ << "end_header" << std::endl; 67 | 68 | header_written_ = true; 69 | return true; 70 | } 71 | 72 | bool writeVertex(const Point& coord) { 73 | if (!header_written_) { 74 | if (!writeHeader()) { 75 | return false; 76 | } 77 | } 78 | if (vertices_written_ >= vertices_total_ || has_color_) { 79 | return false; 80 | } 81 | file_ << coord.x() << " " << coord.y() << " " << coord.z() << std::endl; 82 | return true; 83 | } 84 | 85 | bool writeVertex(const Point& coord, const Color& rgb) { 86 | if (!header_written_) { 87 | if (!writeHeader()) { 88 | return false; 89 | } 90 | } 91 | if (vertices_written_ >= vertices_total_ || !has_color_) { 92 | return false; 93 | } 94 | file_ << coord.x() << " " << coord.y() << " " << coord.z() << " "; 95 | file_ << static_cast(rgb.r) << " " << static_cast(rgb.g) << " " 96 | << static_cast(rgb.b) << std::endl; 97 | return true; 98 | } 99 | 100 | void closeFile() { file_.close(); } 101 | 102 | private: 103 | bool header_written_; 104 | bool parameters_set_; 105 | 106 | size_t vertices_total_; 107 | size_t vertices_written_; 108 | bool has_color_; 109 | 110 | std::ofstream file_; 111 | }; 112 | 113 | } // namespace io 114 | 115 | } // namespace voxblox 116 | 117 | #endif // VOXBLOX_IO_PLY_WRITER_H_ 118 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/mesh/README.md: -------------------------------------------------------------------------------- 1 | # NOTICE 2 | 3 | This code is mostly adapted from OpenChisel: 4 | https://github.com/personalrobotics/OpenChisel 5 | We've retained the license headers whenever relevant. 6 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/simulation/simulation_world.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_SIMULATION_SIMULATION_WORLD_H_ 2 | #define VOXBLOX_SIMULATION_SIMULATION_WORLD_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "voxblox/core/common.h" 10 | #include "voxblox/core/layer.h" 11 | #include "voxblox/core/voxel.h" 12 | #include "voxblox/simulation/objects.h" 13 | 14 | namespace voxblox { 15 | 16 | class SimulationWorld { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | SimulationWorld(); 21 | virtual ~SimulationWorld() {} 22 | 23 | /// === Creating an environment === 24 | void addObject(std::unique_ptr object); 25 | 26 | /** 27 | * Convenience functions for setting up bounded areas. 28 | * Ground level can also be used for ceiling. ;) 29 | */ 30 | void addGroundLevel(FloatingPoint height); 31 | 32 | /** 33 | * Add 4 walls (infinite planes) bounding the space. In case this is not the 34 | * desired behavior, can use addObject to add walls manually one by one. 35 | * If infinite walls are undesirable, then use cubes. 36 | */ 37 | void addPlaneBoundaries(FloatingPoint x_min, FloatingPoint x_max, 38 | FloatingPoint y_min, FloatingPoint y_max); 39 | 40 | /// Deletes all objects! 41 | void clear(); 42 | 43 | /** === Generating synthetic data from environment === 44 | * Generates a synthetic view 45 | * Assumes square pixels for ease... Takes in FoV in radians. 46 | */ 47 | void getPointcloudFromViewpoint(const Point& view_origin, 48 | const Point& view_direction, 49 | const Eigen::Vector2i& camera_res, 50 | FloatingPoint fov_h_rad, 51 | FloatingPoint max_dist, Pointcloud* ptcloud, 52 | Colors* colors) const; 53 | void getPointcloudFromTransform(const Transformation& pose, 54 | const Eigen::Vector2i& camera_res, 55 | FloatingPoint fov_h_rad, 56 | FloatingPoint max_dist, Pointcloud* ptcloud, 57 | Colors* colors) const; 58 | 59 | /** 60 | * Same thing as getPointcloudFromViewpoint, but also adds a noise in the 61 | * *distance* of the measurement, given by noise_sigma (Gaussian noise). No 62 | * noise in the bearing. 63 | */ 64 | void getNoisyPointcloudFromViewpoint(const Point& view_origin, 65 | const Point& view_direction, 66 | const Eigen::Vector2i& camera_res, 67 | FloatingPoint fov_h_rad, 68 | FloatingPoint max_dist, 69 | FloatingPoint noise_sigma, 70 | Pointcloud* ptcloud, Colors* colors); 71 | void getNoisyPointcloudFromTransform(const Transformation& pose, 72 | const Eigen::Vector2i& camera_res, 73 | FloatingPoint fov_h_rad, 74 | FloatingPoint max_dist, 75 | FloatingPoint noise_sigma, 76 | Pointcloud* ptcloud, Colors* colors); 77 | 78 | // === Computing ground truth SDFs === 79 | template 80 | void generateSdfFromWorld(FloatingPoint max_dist, 81 | Layer* layer) const; 82 | 83 | FloatingPoint getDistanceToPoint(const Point& coords, 84 | FloatingPoint max_dist) const; 85 | 86 | /// Set and get the map generation and display bounds. 87 | void setBounds(const Point& min_bound, const Point& max_bound) { 88 | min_bound_ = min_bound; 89 | max_bound_ = max_bound; 90 | } 91 | 92 | Point getMinBound() const { return min_bound_; } 93 | Point getMaxBound() const { return max_bound_; } 94 | 95 | protected: 96 | template 97 | void setVoxel(FloatingPoint dist, const Color& color, VoxelType* voxel) const; 98 | 99 | FloatingPoint getNoise(FloatingPoint noise_sigma); 100 | 101 | /// List storing pointers to all the objects in this world. 102 | std::list > objects_; 103 | 104 | // World boundaries... Can be changed arbitrarily, just sets ground truth 105 | // generation and visualization bounds, accurate only up to block size. 106 | Point min_bound_; 107 | Point max_bound_; 108 | 109 | /// For producing noise. Sets a fixed seed (0). 110 | std::default_random_engine generator_; 111 | }; 112 | 113 | } // namespace voxblox 114 | 115 | #endif // VOXBLOX_SIMULATION_SIMULATION_WORLD_H_ 116 | 117 | #include "voxblox/simulation/simulation_world_inl.h" 118 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/simulation/simulation_world_inl.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_SIMULATION_SIMULATION_WORLD_INL_H_ 2 | #define VOXBLOX_SIMULATION_SIMULATION_WORLD_INL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "voxblox/core/block.h" 9 | #include "voxblox/utils/timing.h" 10 | 11 | namespace voxblox { 12 | 13 | template 14 | void SimulationWorld::generateSdfFromWorld(FloatingPoint max_dist, 15 | Layer* layer) const { 16 | timing::Timer sim_timer("sim/generate_sdf"); 17 | 18 | CHECK_NOTNULL(layer); 19 | // Iterate over every voxel in the layer and compute its distance to all 20 | // objects. 21 | 22 | // Get all blocks within bounds. For now, only respect bounds approximately: 23 | // that is, up to block boundaries. 24 | FloatingPoint block_size = layer->block_size(); 25 | FloatingPoint half_block_size = block_size / 2.0; 26 | 27 | BlockIndexList blocks; 28 | for (FloatingPoint x = min_bound_.x() - half_block_size; 29 | x <= max_bound_.x() + half_block_size; x += block_size) { 30 | for (FloatingPoint y = min_bound_.y() - half_block_size; 31 | y <= max_bound_.y() + half_block_size; y += block_size) { 32 | for (FloatingPoint z = min_bound_.z() - half_block_size; 33 | z <= max_bound_.z() + half_block_size; z += block_size) { 34 | blocks.push_back( 35 | layer->computeBlockIndexFromCoordinates(Point(x, y, z))); 36 | } 37 | } 38 | } 39 | 40 | // Iterate over all blocks filling this stuff in. 41 | for (const BlockIndex& block_index : blocks) { 42 | typename Block::Ptr block = 43 | layer->allocateBlockPtrByIndex(block_index); 44 | for (size_t i = 0; i < block->num_voxels(); ++i) { 45 | VoxelType& voxel = block->getVoxelByLinearIndex(i); 46 | Point coords = block->computeCoordinatesFromLinearIndex(i); 47 | // Check that it's in bounds, otherwise skip it. 48 | if (!(coords.x() >= min_bound_.x() && coords.x() <= max_bound_.x() && 49 | coords.y() >= min_bound_.y() && coords.y() <= max_bound_.y() && 50 | coords.z() >= min_bound_.z() && coords.z() <= max_bound_.z())) { 51 | continue; 52 | } 53 | 54 | // Iterate over all objects and get distances to this thing. 55 | FloatingPoint voxel_dist = max_dist; 56 | Color color; 57 | for (const std::unique_ptr& object : objects_) { 58 | FloatingPoint object_dist = object->getDistanceToPoint(coords); 59 | if (object_dist < voxel_dist) { 60 | voxel_dist = object_dist; 61 | color = object->getColor(); 62 | } 63 | } 64 | 65 | // Then update the thing. 66 | voxel_dist = std::max(voxel_dist, -max_dist); 67 | setVoxel(voxel_dist, color, &voxel); 68 | } 69 | } 70 | } 71 | 72 | template <> 73 | void SimulationWorld::setVoxel(FloatingPoint dist, const Color& color, 74 | TsdfVoxel* voxel) const { 75 | voxel->distance = static_cast(dist); 76 | voxel->color = color; 77 | voxel->weight = 1.0f; // Just to make sure it gets visualized/meshed/etc. 78 | } 79 | 80 | // Color ignored. 81 | template <> 82 | void SimulationWorld::setVoxel(FloatingPoint dist, const Color& /*color*/, 83 | EsdfVoxel* voxel) const { 84 | voxel->distance = static_cast(dist); 85 | voxel->observed = true; 86 | } 87 | 88 | } // namespace voxblox 89 | 90 | #endif // VOXBLOX_SIMULATION_SIMULATION_WORLD_INL_H_ 91 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/bucket_queue.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_BUCKET_QUEUE_H_ 2 | #define VOXBLOX_UTILS_BUCKET_QUEUE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "voxblox/core/common.h" 11 | 12 | namespace voxblox { 13 | /** 14 | * Bucketed priority queue, mostly following L. Yatziv et al in 15 | * O(N) Implementation of the Fast Marching Algorithm, though skipping the 16 | * circular aspect (don't care about a bit more memory used for this). 17 | */ 18 | template 19 | class BucketQueue { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | BucketQueue() : last_bucket_index_(0) {} 24 | explicit BucketQueue(int num_buckets, double max_val) 25 | : num_buckets_(num_buckets), 26 | max_val_(max_val), 27 | last_bucket_index_(0), 28 | num_elements_(0) { 29 | buckets_.resize(num_buckets_); 30 | } 31 | 32 | /// WARNING: will CLEAR THE QUEUE! 33 | void setNumBuckets(int num_buckets, double max_val) { 34 | max_val_ = max_val; 35 | num_buckets_ = num_buckets; 36 | buckets_.clear(); 37 | buckets_.resize(num_buckets_); 38 | num_elements_ = 0; 39 | } 40 | 41 | void push(const T& key, double value) { 42 | CHECK_NE(num_buckets_, 0); 43 | if (value > max_val_) { 44 | value = max_val_; 45 | } 46 | int bucket_index = 47 | std::floor(std::abs(value) / max_val_ * (num_buckets_ - 1)); 48 | if (bucket_index >= num_buckets_) { 49 | bucket_index = num_buckets_ - 1; 50 | } 51 | if (bucket_index < last_bucket_index_) { 52 | last_bucket_index_ = bucket_index; 53 | } 54 | buckets_[bucket_index].push(key); 55 | num_elements_++; 56 | } 57 | 58 | void pop() { 59 | if (empty()) { 60 | return; 61 | } 62 | while (buckets_[last_bucket_index_].empty() && 63 | last_bucket_index_ < num_buckets_) { 64 | last_bucket_index_++; 65 | } 66 | if (last_bucket_index_ < num_buckets_) { 67 | buckets_[last_bucket_index_].pop(); 68 | num_elements_--; 69 | } 70 | } 71 | 72 | T front() { 73 | CHECK_NE(num_buckets_, 0); 74 | CHECK(!empty()); 75 | while (buckets_[last_bucket_index_].empty() && 76 | last_bucket_index_ < num_buckets_) { 77 | last_bucket_index_++; 78 | } 79 | return buckets_[last_bucket_index_].front(); 80 | } 81 | 82 | bool empty() { return num_elements_ == 0; } 83 | 84 | void clear() { 85 | buckets_.clear(); 86 | buckets_.resize(num_buckets_); 87 | last_bucket_index_ = 0; 88 | num_elements_ = 0; 89 | } 90 | 91 | private: 92 | int num_buckets_; 93 | double max_val_; 94 | voxblox::AlignedVector> buckets_; 95 | 96 | /// Speeds up retrivals. 97 | int last_bucket_index_; 98 | /// This is also to speed up empty checks. 99 | size_t num_elements_; 100 | }; 101 | } // namespace voxblox 102 | #endif // VOXBLOX_UTILS_BUCKET_QUEUE_H_ 103 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/camera_model.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_CAMERA_MODEL_H_ 2 | #define VOXBLOX_UTILS_CAMERA_MODEL_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "voxblox/core/common.h" 11 | 12 | namespace voxblox { 13 | 14 | /** 15 | * Represents a plane in Hesse normal form (normal + distance) for faster 16 | * distance calculations to points. 17 | */ 18 | class Plane { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | Plane() : normal_(Point::Identity()), distance_(0) {} 23 | virtual ~Plane() {} 24 | 25 | void setFromPoints(const Point& p1, const Point& p2, const Point& p3); 26 | void setFromDistanceNormal(const Point& normal, double distance); 27 | 28 | /// I guess more correctly, is the point on the correct side of the plane. 29 | bool isPointInside(const Point& point) const; 30 | 31 | Point normal() const { return normal_; } 32 | double distance() const { return distance_; } 33 | 34 | private: 35 | Point normal_; 36 | double distance_; 37 | }; 38 | 39 | /** 40 | * Virtual camera model for use in simulating a systems view 41 | */ 42 | class CameraModel { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | 46 | CameraModel() : initialized_(false) {} 47 | virtual ~CameraModel() {} 48 | 49 | /// Set up the camera model, intrinsics and extrinsics. 50 | void setIntrinsicsFromFocalLength( 51 | const Eigen::Matrix& resolution, double focal_length, 52 | double min_distance, double max_distance); 53 | void setIntrinsicsFromFoV(double horizontal_fov, double vertical_fov, 54 | double min_distance, double max_distance); 55 | void setExtrinsics(const Transformation& T_C_B); 56 | 57 | /** 58 | * Get and set the current poses for the camera (should be called after 59 | * the camera is properly set up). 60 | */ 61 | Transformation getCameraPose() const; 62 | Transformation getBodyPose() const; 63 | /// Set camera pose actually computes the new bounding plane positions. 64 | void setCameraPose(const Transformation& cam_pose); 65 | void setBodyPose(const Transformation& body_pose); 66 | 67 | /// Check whether a point belongs in the current view. 68 | void getAabb(Point* aabb_min, Point* aabb_max) const; 69 | bool isPointInView(const Point& point) const; 70 | 71 | /** 72 | * Accessor functions for visualization (or other reasons). 73 | * Bounding planes are returned in the global coordinate frame. 74 | */ 75 | const AlignedVector& getBoundingPlanes() const { 76 | return bounding_planes_; 77 | } 78 | /** 79 | * Gives all the bounding lines of the planes in connecting order, expressed 80 | * in the global coordinate frame. 81 | */ 82 | void getBoundingLines(AlignedVector* lines) const; 83 | 84 | /** 85 | * Get the 3 points definining the plane at the back (far end) of the camera 86 | * frustum. Expressed in global coordinates. 87 | */ 88 | void getFarPlanePoints(AlignedVector* points) const; 89 | 90 | private: 91 | void calculateBoundingPlanes(); 92 | 93 | bool initialized_; 94 | 95 | /// Extrinsic calibration to body. 96 | Transformation T_C_B_; 97 | /// Current pose of the camera. 98 | Transformation T_G_C_; 99 | 100 | /** 101 | * The original vertices of the frustum, in the axis-aligned coordinate frame 102 | * (before rotation). 103 | */ 104 | AlignedVector corners_C_; 105 | 106 | /** 107 | * The 6 bounding planes for the current camera pose, and their corresponding 108 | * AABB (Axis Aligned Bounding Box). Expressed in global coordinate frame. 109 | */ 110 | AlignedVector bounding_planes_; 111 | Point aabb_min_; 112 | Point aabb_max_; 113 | }; 114 | } // namespace voxblox 115 | 116 | #endif // VOXBLOX_UTILS_CAMERA_MODEL_H_ 117 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/distance_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_DISTANCE_UTILS_H_ 2 | #define VOXBLOX_UTILS_DISTANCE_UTILS_H_ 3 | 4 | #include "voxblox/core/block.h" 5 | #include "voxblox/core/common.h" 6 | #include "voxblox/core/layer.h" 7 | #include "voxblox/core/voxel.h" 8 | 9 | namespace voxblox { 10 | 11 | /** 12 | * Returns true if there is a valid distance (intersection with the surface), 13 | * false otherwise (no known space, no surface boundary, etc.). 14 | * VoxelType must have a distance defined. 15 | */ 16 | template 17 | bool getSurfaceDistanceAlongRay(const Layer& layer, 18 | const Point& ray_origin, 19 | const Point& bearing_vector, 20 | FloatingPoint max_distance, 21 | Point* triangulated_pose) { 22 | CHECK_NOTNULL(triangulated_pose); 23 | // Make sure bearing vector is normalized. 24 | const Point ray_direction = bearing_vector.normalized(); 25 | 26 | // Keep track of current distance along the ray. 27 | FloatingPoint t = 0.0; 28 | // General ray equations: p = o + d * t 29 | 30 | // Cache voxel sizes for faster moving. 31 | const FloatingPoint voxel_size = layer.voxel_size(); 32 | 33 | bool surface_found = false; 34 | 35 | while (t < max_distance) { 36 | const Point current_pos = ray_origin + t * ray_direction; 37 | typename Block::ConstPtr block_ptr = 38 | layer.getBlockPtrByCoordinates(current_pos); 39 | if (!block_ptr) { 40 | // How much should we move up by? 1 voxel? 1 block? Could be close to the 41 | // block boundary though.... 42 | // Let's start with the naive choice: 1 voxel. 43 | t += voxel_size; 44 | continue; 45 | } 46 | const VoxelType& voxel = block_ptr->getVoxelByCoordinates(current_pos); 47 | if (voxel.weight < 1e-6) { 48 | t += voxel_size; 49 | continue; 50 | } 51 | if (voxel.distance > voxel_size) { 52 | // Move forward as much as we can. 53 | t += voxel.distance; 54 | continue; 55 | } 56 | // The other cases are when we are actually at or behind the surface. 57 | // TODO(helenol): these are gross generalizations; use actual interpolation 58 | // to get the surface boundary. 59 | if (voxel.distance < 0.0) { 60 | surface_found = true; 61 | break; 62 | } 63 | if (voxel.distance < voxel_size) { 64 | // Also assume this is finding the surface. 65 | surface_found = true; 66 | t += voxel.distance; 67 | break; 68 | } 69 | // Default case... 70 | t += voxel_size; 71 | } 72 | 73 | if (surface_found) { 74 | *triangulated_pose = ray_origin + t * ray_direction; 75 | } 76 | 77 | return surface_found; 78 | } 79 | 80 | } // namespace voxblox 81 | 82 | #endif // VOXBLOX_UTILS_DISTANCE_UTILS_H_ 83 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/meshing_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_MESHING_UTILS_H_ 2 | #define VOXBLOX_UTILS_MESHING_UTILS_H_ 3 | 4 | #include "voxblox/core/common.h" 5 | #include "voxblox/core/voxel.h" 6 | 7 | namespace voxblox { 8 | 9 | namespace utils { 10 | 11 | template 12 | bool getSdfIfValid(const VoxelType& voxel, const FloatingPoint min_weight, 13 | FloatingPoint* sdf); 14 | 15 | template <> 16 | inline bool getSdfIfValid(const TsdfVoxel& voxel, 17 | const FloatingPoint min_weight, FloatingPoint* sdf) { 18 | DCHECK(sdf != nullptr); 19 | if (voxel.weight <= min_weight) { 20 | return false; 21 | } 22 | *sdf = voxel.distance; 23 | return true; 24 | } 25 | 26 | template <> 27 | inline bool getSdfIfValid(const EsdfVoxel& voxel, 28 | const FloatingPoint /*min_weight*/, 29 | FloatingPoint* sdf) { 30 | DCHECK(sdf != nullptr); 31 | if (!voxel.observed) { 32 | return false; 33 | } 34 | *sdf = voxel.distance; 35 | return true; 36 | } 37 | 38 | template 39 | bool getColorIfValid(const VoxelType& voxel, const FloatingPoint min_weight, 40 | Color* color); 41 | 42 | template <> 43 | inline bool getColorIfValid(const TsdfVoxel& voxel, 44 | const FloatingPoint min_weight, Color* color) { 45 | DCHECK(color != nullptr); 46 | if (voxel.weight <= min_weight) { 47 | return false; 48 | } 49 | *color = voxel.color; 50 | return true; 51 | } 52 | 53 | template <> 54 | inline bool getColorIfValid(const EsdfVoxel& voxel, 55 | const FloatingPoint /*min_weight*/, Color* color) { 56 | DCHECK(color != nullptr); 57 | if (!voxel.observed) { 58 | return false; 59 | } 60 | *color = Color(255u, 255u, 255u); 61 | return true; 62 | } 63 | 64 | } // namespace utils 65 | } // namespace voxblox 66 | 67 | #endif // VOXBLOX_UTILS_MESHING_UTILS_H_ 68 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/neighbor_tools.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_NEIGHBOR_TOOLS_H_ 2 | #define VOXBLOX_UTILS_NEIGHBOR_TOOLS_H_ 3 | 4 | #include "voxblox/core/common.h" 5 | #include "voxblox/core/layer.h" 6 | 7 | namespace voxblox { 8 | 9 | /// Define what connectivity you want to have in the voxels. 10 | enum Connectivity : unsigned int { 11 | kSix = 6u, 12 | kEighteen = 18u, 13 | kTwentySix = 26u 14 | }; 15 | 16 | class NeighborhoodLookupTables { 17 | public: 18 | typedef Eigen::Matrix 19 | LongIndexOffsets; 20 | typedef Eigen::Matrix IndexOffsets; 21 | typedef Eigen::Matrix Distances; 22 | 23 | /* 24 | * Stores the distances to the 6, 18, and 26 neighborhood, in that order. 25 | * These distances need to be scaled by the voxel distance to get metric 26 | * distances. 27 | */ 28 | static const Distances kDistances; 29 | 30 | /* 31 | * Lookup table for the offsets between a index and its 6, 18, and 26 32 | * neighborhood, in that order. These two offset tables are the same except 33 | * for the type, this saves casting the offset when used with either global 34 | * index (long) or local index (int) in the neighborhood lookup. 35 | */ 36 | static const IndexOffsets kOffsets; 37 | static const LongIndexOffsets kLongOffsets; 38 | }; 39 | 40 | template 41 | class Neighborhood : public NeighborhoodLookupTables { 42 | public: 43 | typedef Eigen::Matrix IndexMatrix; 44 | 45 | /// Get the global index of all (6, 18, or 26) neighbors of the input index. 46 | static void getFromGlobalIndex(const GlobalIndex& global_index, 47 | IndexMatrix* neighbors) { 48 | CHECK_NOTNULL(neighbors); 49 | for (unsigned int i = 0u; i < kConnectivity; ++i) { 50 | neighbors->col(i) = global_index + kLongOffsets.col(i); 51 | } 52 | } 53 | 54 | /** 55 | * Get the block idx and local voxel index of a neighbor voxel. The neighbor 56 | * voxel is defined by providing a direction. The main purpose of this 57 | * function is to solve the cross-block indexing that happens when looking up 58 | * neighbors at the block boundaries. 59 | */ 60 | static void getFromBlockAndVoxelIndexAndDirection( 61 | const BlockIndex& block_index, const VoxelIndex& voxel_index, 62 | const SignedIndex& direction, const size_t voxels_per_side, 63 | BlockIndex* neighbor_block_index, VoxelIndex* neighbor_voxel_index) { 64 | CHECK_GT(voxels_per_side, 0u); 65 | CHECK_NOTNULL(neighbor_block_index); 66 | CHECK_NOTNULL(neighbor_voxel_index); 67 | 68 | *neighbor_block_index = block_index; 69 | *neighbor_voxel_index = voxel_index + direction; 70 | 71 | for (unsigned int i = 0u; i < 3u; ++i) { 72 | while ((*neighbor_voxel_index)(i) < 0) { 73 | (*neighbor_block_index)(i)--; 74 | (*neighbor_voxel_index)(i) += voxels_per_side; 75 | } 76 | while ((*neighbor_voxel_index)(i) >= 77 | static_cast(voxels_per_side)) { 78 | (*neighbor_block_index)(i)++; 79 | (*neighbor_voxel_index)(i) -= voxels_per_side; 80 | } 81 | } 82 | } 83 | 84 | /** 85 | * Get the hierarchical indices (block idx, local voxel index) for all 86 | * neighbors (6, 18, or 26 neighborhood) of a hierarcical index. This function 87 | * solves the cross-block indexing that happens when looking up neighbors at 88 | * the block boundary. 89 | */ 90 | static void getFromBlockAndVoxelIndex( 91 | const BlockIndex& block_index, const VoxelIndex& voxel_index, 92 | const size_t voxels_per_side, AlignedVector* neighbors_ptr) { 93 | CHECK_NOTNULL(neighbors_ptr)->resize(kConnectivity); 94 | 95 | AlignedVector& neighbors = *neighbors_ptr; 96 | for (unsigned int i = 0u; i < kConnectivity; ++i) { 97 | VoxelKey& neighbor = neighbors[i]; 98 | getFromBlockAndVoxelIndexAndDirection(block_index, voxel_index, 99 | kOffsets.col(i), voxels_per_side, 100 | &neighbor.first, &neighbor.second); 101 | } 102 | } 103 | 104 | /// Get the signed offset between the global indices of two voxels. 105 | static SignedIndex getOffsetBetweenVoxels(const BlockIndex& start_block_index, 106 | const VoxelIndex& start_voxel_index, 107 | const BlockIndex& end_block_index, 108 | const VoxelIndex& end_voxel_index, 109 | const size_t voxels_per_side) { 110 | CHECK_NE(voxels_per_side, 0u); 111 | return (end_voxel_index - start_voxel_index) + 112 | (end_block_index - start_block_index) * voxels_per_side; 113 | } 114 | }; 115 | } // namespace voxblox 116 | 117 | #endif // VOXBLOX_UTILS_NEIGHBOR_TOOLS_H_ 118 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/planning_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_PLANNING_UTILS_H_ 2 | #define VOXBLOX_UTILS_PLANNING_UTILS_H_ 3 | 4 | #include "voxblox/core/layer.h" 5 | #include "voxblox/core/voxel.h" 6 | 7 | namespace voxblox { 8 | 9 | namespace utils { 10 | 11 | /// Gets the indices of all points within the sphere. 12 | template 13 | void getSphereAroundPoint(const Layer& layer, const Point& center, 14 | FloatingPoint radius, 15 | HierarchicalIndexMap* block_voxel_list); 16 | 17 | /** 18 | * Gets the indices of all points around a sphere, and also allocates any 19 | * blocks that don't already exist. 20 | */ 21 | template 22 | void getAndAllocateSphereAroundPoint(const Point& center, FloatingPoint radius, 23 | Layer* layer, 24 | HierarchicalIndexMap* block_voxel_list); 25 | 26 | /** 27 | * Tools for manually editing a set of voxels. Sets the values around a sphere 28 | * to be artifically free or occupied, and marks them as hallucinated. 29 | */ 30 | template 31 | void fillSphereAroundPoint(const Point& center, const FloatingPoint radius, 32 | const FloatingPoint max_distance_m, 33 | Layer* layer); 34 | template 35 | void clearSphereAroundPoint(const Point& center, const FloatingPoint radius, 36 | const FloatingPoint max_distance_m, 37 | Layer* layer); 38 | 39 | /** 40 | * Utility function to get map bounds from an arbitrary layer. 41 | * Only accurate to block level (i.e., outer bounds of allocated blocks). 42 | */ 43 | template 44 | void computeMapBoundsFromLayer(const voxblox::Layer& layer, 45 | Eigen::Vector3d* lower_bound, 46 | Eigen::Vector3d* upper_bound); 47 | 48 | } // namespace utils 49 | } // namespace voxblox 50 | 51 | #include "voxblox/utils/planning_utils_inl.h" 52 | 53 | #endif // VOXBLOX_UTILS_PLANNING_UTILS_H_ 54 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/protobuf_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_PROTOBUF_UTILS_H_ 2 | #define VOXBLOX_UTILS_PROTOBUF_UTILS_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace voxblox { 12 | 13 | namespace utils { 14 | bool readProtoMsgCountFromStream(std::istream* stream_in, 15 | uint32_t* message_count, 16 | uint64_t* byte_offset); 17 | 18 | bool writeProtoMsgCountToStream(uint32_t message_count, 19 | std::fstream* stream_out); 20 | 21 | bool readProtoMsgFromStream(std::istream* stream_in, 22 | google::protobuf::Message* message, 23 | uint64_t* byte_offset); 24 | 25 | bool writeProtoMsgToStream(const google::protobuf::Message& message, 26 | std::fstream* stream_out); 27 | 28 | } // namespace utils 29 | } // namespace voxblox 30 | 31 | #endif // VOXBLOX_UTILS_PROTOBUF_UTILS_H_ 32 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/utils/voxel_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_VOXEL_UTILS_H_ 2 | #define VOXBLOX_UTILS_VOXEL_UTILS_H_ 3 | 4 | #include "voxblox/core/color.h" 5 | #include "voxblox/core/common.h" 6 | #include "voxblox/core/voxel.h" 7 | 8 | namespace voxblox { 9 | template 10 | void mergeVoxelAIntoVoxelB(const VoxelType& voxel_A, VoxelType* voxel_B); 11 | 12 | template <> 13 | void mergeVoxelAIntoVoxelB(const TsdfVoxel& voxel_A, TsdfVoxel* voxel_B); 14 | 15 | template <> 16 | void mergeVoxelAIntoVoxelB(const EsdfVoxel& voxel_A, EsdfVoxel* voxel_B); 17 | 18 | template <> 19 | void mergeVoxelAIntoVoxelB(const OccupancyVoxel& voxel_A, 20 | OccupancyVoxel* voxel_B); 21 | 22 | } // namespace voxblox 23 | 24 | #endif // VOXBLOX_UTILS_VOXEL_UTILS_H_ 25 | -------------------------------------------------------------------------------- /voxblox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxblox 4 | 0.0.0 5 | Voxblox 6 | 7 | Helen Oleynikova 8 | Marius Fehr 9 | 10 | Helen Oleynikova 11 | Marius Fehr 12 | 13 | BSD 14 | 15 | catkin 16 | catkin_simple 17 | 18 | eigen_catkin 19 | eigen_checks 20 | gflags_catkin 21 | glog_catkin 22 | minkindr 23 | protobuf_catkin 24 | 25 | 26 | -------------------------------------------------------------------------------- /voxblox/proto/voxblox/Block.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package voxblox; 3 | 4 | message BlockProto { 5 | optional int32 voxels_per_side = 1; 6 | 7 | optional double voxel_size = 2; 8 | 9 | optional double origin_x = 3; 10 | optional double origin_y = 4; 11 | optional double origin_z = 5; 12 | 13 | optional bool has_data = 6; 14 | 15 | repeated uint32 voxel_data = 7; 16 | } 17 | -------------------------------------------------------------------------------- /voxblox/proto/voxblox/Layer.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package voxblox; 3 | 4 | message LayerProto { 5 | optional double voxel_size = 1; 6 | 7 | optional uint32 voxels_per_side = 2; 8 | 9 | // See voxblox::voxel_type 10 | optional string type = 3; 11 | } 12 | -------------------------------------------------------------------------------- /voxblox/src/core/tsdf_map.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/core/tsdf_map.h" 2 | 3 | namespace voxblox { 4 | 5 | unsigned int TsdfMap::coordPlaneSliceGetDistanceWeight( 6 | unsigned int free_plane_index, double free_plane_val, 7 | EigenDRef>& positions, 8 | Eigen::Ref distances, Eigen::Ref weights, 9 | unsigned int max_points) const { 10 | BlockIndexList blocks; 11 | tsdf_layer_->getAllAllocatedBlocks(&blocks); 12 | 13 | // Cache layer settings. 14 | const size_t vps = tsdf_layer_->voxels_per_side(); 15 | const size_t num_voxels_per_block = vps * vps * vps; 16 | 17 | // Temp variables. 18 | bool did_all_fit = true; 19 | unsigned int count = 0; 20 | 21 | /* TODO(mereweth@jpl.nasa.gov) - store min/max index (per axis) allocated in 22 | * Layer This extra bookeeping will make this much faster 23 | * // Iterate over all blocks corresponding to slice plane 24 | * Point on_slice_plane(0, 0, 0); 25 | * on_slice_plane(free_plane_index) = free_plane_val; 26 | * BlockIndex block_index = 27 | * tsdf_layer_->computeBlockIndexFromCoordinates(on_slice_plane); 28 | */ 29 | 30 | for (const BlockIndex& index : blocks) { 31 | // Iterate over all voxels in said blocks. 32 | const Block& block = tsdf_layer_->getBlockByIndex(index); 33 | 34 | if (!block.has_data()) { 35 | continue; 36 | } 37 | 38 | const Point origin = block.origin(); 39 | if (std::abs(origin(free_plane_index) - free_plane_val) > 40 | block.block_size()) { 41 | continue; 42 | } 43 | 44 | for (size_t linear_index = 0; linear_index < num_voxels_per_block; 45 | ++linear_index) { 46 | const Point coord = block.computeCoordinatesFromLinearIndex(linear_index); 47 | const TsdfVoxel& voxel = block.getVoxelByLinearIndex(linear_index); 48 | if (std::abs(coord(free_plane_index) - free_plane_val) <= 49 | block.voxel_size()) { 50 | const double distance = voxel.distance; 51 | const double weight = voxel.weight; 52 | 53 | if (count < positions.cols()) { 54 | positions.col(count) = 55 | Eigen::Vector3d(coord.x(), coord.y(), coord.z()); 56 | } else { 57 | did_all_fit = false; 58 | } 59 | if (count < distances.size()) { 60 | distances(count) = distance; 61 | weights(count) = weight; 62 | } else { 63 | did_all_fit = false; 64 | } 65 | count++; 66 | if (count >= max_points) { 67 | return count; 68 | } 69 | } 70 | } 71 | } 72 | 73 | if (!did_all_fit) { 74 | throw std::runtime_error(std::string("Unable to store ") + 75 | std::to_string(count) + " values."); 76 | } 77 | 78 | return count; 79 | } 80 | 81 | bool TsdfMap::getWeightAtPosition(const Eigen::Vector3d& position, 82 | double* weight) const { 83 | constexpr bool interpolate = true; 84 | return getWeightAtPosition(position, interpolate, weight); 85 | } 86 | 87 | bool TsdfMap::getWeightAtPosition(const Eigen::Vector3d& position, 88 | const bool interpolate, 89 | double* weight) const { 90 | FloatingPoint weight_fp; 91 | bool success = interpolator_.getWeight(position.cast(), 92 | &weight_fp, interpolate); 93 | if (success) { 94 | *weight = static_cast(weight_fp); 95 | } 96 | return success; 97 | } 98 | 99 | std::string TsdfMap::Config::print() const { 100 | std::stringstream ss; 101 | // clang-format off 102 | ss << "====================== TSDF Map Config ========================\n"; 103 | ss << " - tsdf_voxel_size: " << tsdf_voxel_size << "\n"; 104 | ss << " - tsdf_voxels_per_side: " << tsdf_voxels_per_side << "\n"; 105 | ss << "==============================================================\n"; 106 | // clang-format on 107 | return ss.str(); 108 | } 109 | 110 | } // namespace voxblox 111 | -------------------------------------------------------------------------------- /voxblox/src/integrator/intensity_integrator.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/integrator/intensity_integrator.h" 2 | 3 | #include "voxblox/utils/distance_utils.h" 4 | 5 | namespace voxblox { 6 | 7 | IntensityIntegrator::IntensityIntegrator(const Layer& tsdf_layer, 8 | Layer* intensity_layer) 9 | : max_distance_(5.0), 10 | max_weight_(100.0), 11 | intensity_prop_voxel_radius_(2), 12 | tsdf_layer_(tsdf_layer), 13 | intensity_layer_(intensity_layer) {} 14 | 15 | void IntensityIntegrator::addIntensityBearingVectors( 16 | const Point& origin, const Pointcloud& bearing_vectors, 17 | const std::vector& intensities) { 18 | timing::Timer intensity_timer("intensity/integrate"); 19 | 20 | CHECK_EQ(bearing_vectors.size(), intensities.size()) 21 | << "Intensity and bearing vector size does not match!"; 22 | const FloatingPoint voxel_size = tsdf_layer_.voxel_size(); 23 | 24 | for (size_t i = 0; i < bearing_vectors.size(); ++i) { 25 | Point surface_intersection = Point::Zero(); 26 | // Cast ray from the origin in the direction of the bearing vector until 27 | // finding an intersection with a surface. 28 | bool success = getSurfaceDistanceAlongRay( 29 | tsdf_layer_, origin, bearing_vectors[i], max_distance_, 30 | &surface_intersection); 31 | 32 | if (!success) { 33 | continue; 34 | } 35 | 36 | // Now look up the matching voxels in the intensity layer and mark them. 37 | // Let's just start with 1. 38 | Block::Ptr block_ptr = 39 | intensity_layer_->allocateBlockPtrByCoordinates(surface_intersection); 40 | IntensityVoxel& voxel = 41 | block_ptr->getVoxelByCoordinates(surface_intersection); 42 | voxel.intensity = 43 | (voxel.weight * voxel.intensity + intensities[i]) / (voxel.weight + 1); 44 | voxel.weight += 1.0; 45 | if (voxel.weight > max_weight_) { 46 | voxel.weight = max_weight_; 47 | } 48 | 49 | // Now check the surrounding voxels along the bearing vector. If they have 50 | // never been observed, then fill in their value. Otherwise don't. 51 | Point close_voxel = surface_intersection; 52 | for (int voxel_offset = -intensity_prop_voxel_radius_; 53 | voxel_offset <= intensity_prop_voxel_radius_; voxel_offset++) { 54 | close_voxel = 55 | surface_intersection + bearing_vectors[i] * voxel_offset * voxel_size; 56 | Block::Ptr new_block_ptr = 57 | intensity_layer_->allocateBlockPtrByCoordinates(close_voxel); 58 | IntensityVoxel& new_voxel = block_ptr->getVoxelByCoordinates(close_voxel); 59 | if (new_voxel.weight < 1e-6) { 60 | new_voxel.intensity = intensities[i]; 61 | new_voxel.weight += 1.0; 62 | } 63 | } 64 | } 65 | } 66 | 67 | } // namespace voxblox 68 | -------------------------------------------------------------------------------- /voxblox/src/io/mesh_ply.cc: -------------------------------------------------------------------------------- 1 | // NOTE: From open_chisel: github.com/personalrobotics/OpenChisel/ 2 | // The MIT License (MIT) 3 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "voxblox/io/mesh_ply.h" 24 | 25 | namespace voxblox { 26 | 27 | bool convertMeshLayerToMesh(const MeshLayer& mesh_layer, Mesh* mesh, 28 | const bool connected_mesh, 29 | const FloatingPoint vertex_proximity_threshold) { 30 | CHECK_NOTNULL(mesh); 31 | 32 | if (connected_mesh) { 33 | mesh_layer.getConnectedMesh(mesh, vertex_proximity_threshold); 34 | } else { 35 | mesh_layer.getMesh(mesh); 36 | } 37 | return mesh->size() > 0u; 38 | } 39 | 40 | bool outputMeshLayerAsPly(const std::string& filename, 41 | const MeshLayer& mesh_layer) { 42 | constexpr bool kConnectedMesh = true; 43 | return outputMeshLayerAsPly(filename, kConnectedMesh, mesh_layer); 44 | } 45 | 46 | bool outputMeshLayerAsPly(const std::string& filename, 47 | const bool connected_mesh, 48 | const MeshLayer& mesh_layer) { 49 | Mesh combined_mesh(mesh_layer.block_size(), Point::Zero()); 50 | 51 | if (!convertMeshLayerToMesh(mesh_layer, &combined_mesh, connected_mesh)) { 52 | return false; 53 | } 54 | 55 | bool success = outputMeshAsPly(filename, combined_mesh); 56 | if (!success) { 57 | LOG(WARNING) << "Saving to PLY failed!"; 58 | } 59 | return success; 60 | } 61 | 62 | bool outputMeshAsPly(const std::string& filename, const Mesh& mesh) { 63 | std::ofstream stream(filename.c_str()); 64 | 65 | if (!stream) { 66 | return false; 67 | } 68 | 69 | size_t num_points = mesh.vertices.size(); 70 | stream << "ply" << std::endl; 71 | stream << "format ascii 1.0" << std::endl; 72 | stream << "element vertex " << num_points << std::endl; 73 | stream << "property float x" << std::endl; 74 | stream << "property float y" << std::endl; 75 | stream << "property float z" << std::endl; 76 | if (mesh.hasNormals()) { 77 | stream << "property float normal_x" << std::endl; 78 | stream << "property float normal_y" << std::endl; 79 | stream << "property float normal_z" << std::endl; 80 | } 81 | if (mesh.hasColors()) { 82 | stream << "property uchar red" << std::endl; 83 | stream << "property uchar green" << std::endl; 84 | stream << "property uchar blue" << std::endl; 85 | stream << "property uchar alpha" << std::endl; 86 | } 87 | if (mesh.hasTriangles()) { 88 | stream << "element face " << mesh.indices.size() / 3 << std::endl; 89 | stream << "property list uchar int vertex_indices" 90 | << std::endl; // pcl-1.7(ros::kinetic) breaks ply convention by not 91 | // using "vertex_index" 92 | } 93 | stream << "end_header" << std::endl; 94 | size_t vert_idx = 0; 95 | for (const Point& vert : mesh.vertices) { 96 | stream << vert(0) << " " << vert(1) << " " << vert(2); 97 | 98 | if (mesh.hasNormals()) { 99 | const Point& normal = mesh.normals[vert_idx]; 100 | stream << " " << normal.x() << " " << normal.y() << " " << normal.z(); 101 | } 102 | if (mesh.hasColors()) { 103 | const Color& color = mesh.colors[vert_idx]; 104 | int r = static_cast(color.r); 105 | int g = static_cast(color.g); 106 | int b = static_cast(color.b); 107 | int a = static_cast(color.a); 108 | // Uint8 prints as character otherwise. :( 109 | stream << " " << r << " " << g << " " << b << " " << a; 110 | } 111 | 112 | stream << std::endl; 113 | vert_idx++; 114 | } 115 | if (mesh.hasTriangles()) { 116 | for (size_t i = 0; i < mesh.indices.size(); i += 3) { 117 | stream << "3 "; 118 | 119 | for (int j = 0; j < 3; j++) { 120 | stream << mesh.indices.at(i + j) << " "; 121 | } 122 | 123 | stream << std::endl; 124 | } 125 | } 126 | return true; 127 | } 128 | 129 | } // namespace voxblox 130 | -------------------------------------------------------------------------------- /voxblox/src/io/sdf_ply.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/io/sdf_ply.h" 2 | 3 | #include 4 | 5 | namespace voxblox { 6 | 7 | namespace io { 8 | 9 | template <> 10 | bool getColorFromVoxel(const TsdfVoxel& voxel, const float sdf_color_range, 11 | const float sdf_max_value, Color* color) { 12 | CHECK_NOTNULL(color); 13 | 14 | static constexpr float kTolerance = 1e-6; 15 | if (voxel.weight <= kTolerance) { 16 | return false; 17 | } 18 | 19 | if (std::abs(voxel.distance) > sdf_max_value && sdf_max_value > 0.0f) { 20 | return false; 21 | } 22 | 23 | const float truncated_voxel_distance = 24 | std::min(std::max(voxel.distance, -sdf_color_range), sdf_color_range); 25 | 26 | const float color_factor = 27 | 0.5f * (1.0f + (truncated_voxel_distance / sdf_color_range)); 28 | 29 | CHECK_LE(color_factor, 1.0f); 30 | CHECK_GE(color_factor, 0.0f); 31 | 32 | *color = rainbowColorMap(0.66f - 0.66f * color_factor); 33 | 34 | return true; 35 | } 36 | 37 | template <> 38 | bool getColorFromVoxel(const EsdfVoxel& voxel, const float sdf_color_range, 39 | const float sdf_max_value, Color* color) { 40 | CHECK_NOTNULL(color); 41 | if (!voxel.observed) { 42 | return false; 43 | } 44 | 45 | if (std::abs(voxel.distance) > sdf_max_value && sdf_max_value > 0.0f) { 46 | return false; 47 | } 48 | 49 | const float truncated_voxel_distance = 50 | std::min(std::max(voxel.distance, -sdf_color_range), sdf_color_range); 51 | 52 | const float color_factor = 53 | 0.5f * (1.0f + (truncated_voxel_distance / sdf_color_range)); 54 | 55 | CHECK_LE(color_factor, 1.0f); 56 | CHECK_GE(color_factor, 0.0f); 57 | 58 | *color = rainbowColorMap(0.66f - 0.66f * color_factor); 59 | 60 | return true; 61 | } 62 | 63 | } // namespace io 64 | } // namespace voxblox 65 | -------------------------------------------------------------------------------- /voxblox/src/simulation/objects.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/simulation/objects.h" 2 | 3 | namespace voxblox {} // namespace voxblox 4 | -------------------------------------------------------------------------------- /voxblox/src/utils/evaluation_utils.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/utils/evaluation_utils.h" 2 | 3 | #include "voxblox/core/layer.h" 4 | #include "voxblox/core/voxel.h" 5 | 6 | namespace voxblox { 7 | 8 | namespace utils { 9 | 10 | template <> 11 | VoxelEvaluationResult computeVoxelError( 12 | const TsdfVoxel& voxel_gt, const TsdfVoxel& voxel_test, 13 | const VoxelEvaluationMode evaluation_mode, FloatingPoint* error) { 14 | CHECK_NOTNULL(error); 15 | *error = 0.0; 16 | 17 | // Ignore voxels that are not observed in both layers. 18 | if (!isObservedVoxel(voxel_gt) || !isObservedVoxel(voxel_test)) { 19 | return VoxelEvaluationResult::kNoOverlap; 20 | } 21 | 22 | const bool ignore_behind_test_surface = 23 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindTestSurface) || 24 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindAllSurfaces); 25 | 26 | const bool ignore_behind_gt_surface = 27 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindGtSurface) || 28 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindAllSurfaces); 29 | 30 | if ((ignore_behind_test_surface && voxel_test.distance < 0.0) || 31 | (ignore_behind_gt_surface && voxel_gt.distance < 0.0)) { 32 | return VoxelEvaluationResult::kIgnored; 33 | } 34 | 35 | *error = voxel_test.distance - voxel_gt.distance; 36 | 37 | return VoxelEvaluationResult::kEvaluated; 38 | } 39 | 40 | template <> 41 | VoxelEvaluationResult computeVoxelError( 42 | const EsdfVoxel& voxel_gt, const EsdfVoxel& voxel_test, 43 | const VoxelEvaluationMode evaluation_mode, FloatingPoint* error) { 44 | CHECK_NOTNULL(error); 45 | *error = 0.0; 46 | 47 | // Ignore voxels that are not observed in both layers. 48 | if (!isObservedVoxel(voxel_gt) || !isObservedVoxel(voxel_test)) { 49 | return VoxelEvaluationResult::kNoOverlap; 50 | } 51 | 52 | const bool ignore_behind_test_surface = 53 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindTestSurface) || 54 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindAllSurfaces); 55 | 56 | const bool ignore_behind_gt_surface = 57 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindGtSurface) || 58 | (evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindAllSurfaces); 59 | 60 | if ((ignore_behind_test_surface && voxel_test.distance < 0.0) || 61 | (ignore_behind_gt_surface && voxel_gt.distance < 0.0)) { 62 | return VoxelEvaluationResult::kIgnored; 63 | } 64 | 65 | *error = voxel_test.distance - voxel_gt.distance; 66 | 67 | return VoxelEvaluationResult::kEvaluated; 68 | } 69 | 70 | template 71 | bool isObservedVoxel(const VoxelType& /*voxel*/) { 72 | return false; 73 | } 74 | 75 | template <> 76 | bool isObservedVoxel(const TsdfVoxel& voxel) { 77 | return voxel.weight > 1e-6; 78 | } 79 | 80 | template <> 81 | bool isObservedVoxel(const EsdfVoxel& voxel) { 82 | return voxel.observed; 83 | } 84 | 85 | template <> 86 | FloatingPoint getVoxelSdf(const TsdfVoxel& voxel) { 87 | return voxel.distance; 88 | } 89 | 90 | template <> 91 | FloatingPoint getVoxelSdf(const EsdfVoxel& voxel) { 92 | return voxel.distance; 93 | } 94 | 95 | template <> 96 | void setVoxelSdf(const FloatingPoint sdf, TsdfVoxel* voxel) { 97 | CHECK_NOTNULL(voxel); 98 | voxel->distance = sdf; 99 | } 100 | 101 | template <> 102 | void setVoxelSdf(const FloatingPoint sdf, EsdfVoxel* voxel) { 103 | CHECK_NOTNULL(voxel); 104 | voxel->distance = sdf; 105 | } 106 | 107 | template <> 108 | void setVoxelWeight(const FloatingPoint weight, TsdfVoxel* voxel) { 109 | CHECK_NOTNULL(voxel); 110 | voxel->weight = weight; 111 | } 112 | 113 | template <> 114 | void setVoxelWeight(const FloatingPoint weight, EsdfVoxel* voxel) { 115 | CHECK_NOTNULL(voxel); 116 | voxel->observed = weight > 0.; 117 | } 118 | 119 | } // namespace utils 120 | } // namespace voxblox 121 | -------------------------------------------------------------------------------- /voxblox/src/utils/layer_utils.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/utils/layer_utils.h" 2 | 3 | namespace voxblox { 4 | 5 | namespace utils { 6 | 7 | template <> 8 | bool isSameVoxel(const TsdfVoxel& voxel_A, const TsdfVoxel& voxel_B) { 9 | bool is_the_same = true; 10 | 11 | constexpr double kTolerance = 1e-10; 12 | 13 | is_the_same &= std::abs(voxel_A.distance - voxel_B.distance) < kTolerance; 14 | is_the_same &= std::abs(voxel_A.weight - voxel_B.weight) < kTolerance; 15 | is_the_same &= voxel_A.color.r == voxel_B.color.r; 16 | is_the_same &= voxel_A.color.g == voxel_B.color.g; 17 | is_the_same &= voxel_A.color.b == voxel_B.color.b; 18 | is_the_same &= voxel_A.color.a == voxel_B.color.a; 19 | 20 | return is_the_same; 21 | } 22 | 23 | template <> 24 | bool isSameVoxel(const EsdfVoxel& voxel_A, const EsdfVoxel& voxel_B) { 25 | bool is_the_same = true; 26 | 27 | constexpr double kTolerance = 1e-10; 28 | 29 | is_the_same &= std::abs(voxel_A.distance - voxel_B.distance) < kTolerance; 30 | is_the_same &= voxel_A.observed == voxel_B.observed; 31 | is_the_same &= voxel_A.in_queue == voxel_B.in_queue; 32 | is_the_same &= voxel_A.fixed == voxel_B.fixed; 33 | 34 | is_the_same &= voxel_A.parent.x() == voxel_B.parent.x(); 35 | is_the_same &= voxel_A.parent.y() == voxel_B.parent.y(); 36 | is_the_same &= voxel_A.parent.z() == voxel_B.parent.z(); 37 | 38 | return is_the_same; 39 | } 40 | 41 | template <> 42 | bool isSameVoxel(const OccupancyVoxel& voxel_A, const OccupancyVoxel& voxel_B) { 43 | bool is_the_same = true; 44 | 45 | constexpr double kTolerance = 1e-10; 46 | 47 | is_the_same &= 48 | std::abs(voxel_A.probability_log - voxel_B.probability_log) < kTolerance; 49 | is_the_same &= voxel_A.observed == voxel_B.observed; 50 | 51 | return is_the_same; 52 | } 53 | 54 | } // namespace utils 55 | } // namespace voxblox 56 | -------------------------------------------------------------------------------- /voxblox/src/utils/neighbor_tools.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/utils/neighbor_tools.h" 2 | 3 | #include "voxblox/core/common.h" 4 | 5 | namespace voxblox { 6 | 7 | // clang-format off 8 | const NeighborhoodLookupTables::Distances NeighborhoodLookupTables::kDistances = [] { 9 | const float sqrt_2 = std::sqrt(2); 10 | const float sqrt_3 = std::sqrt(3); 11 | Distances distance_matrix; 12 | distance_matrix << 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 13 | 14 | sqrt_2, sqrt_2, sqrt_2, sqrt_2, 15 | sqrt_2, sqrt_2, sqrt_2, sqrt_2, 16 | sqrt_2, sqrt_2, sqrt_2, sqrt_2, 17 | 18 | sqrt_3, sqrt_3, sqrt_3, sqrt_3, 19 | sqrt_3, sqrt_3, sqrt_3, sqrt_3; 20 | return distance_matrix; 21 | }(); 22 | 23 | 24 | const NeighborhoodLookupTables::IndexOffsets NeighborhoodLookupTables::kOffsets = [] { 25 | IndexOffsets directions_matrix; 26 | directions_matrix << -1, 1, 0, 0, 0, 0, -1, -1, 1, 1, 0, 0, 0, 0, -1, 1, -1, 1, -1, -1, -1, -1, 1, 1, 1, 1, 27 | 0, 0, -1, 1, 0, 0, -1, 1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0, -1, -1, 1, 1, -1, -1, 1, 1, 28 | 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 1, -1, 1, -1, -1, 1, 1, -1, 1, -1, 1, -1, 1, -1, 1; 29 | return directions_matrix; 30 | }(); 31 | 32 | const NeighborhoodLookupTables::LongIndexOffsets NeighborhoodLookupTables::kLongOffsets = [] { 33 | return NeighborhoodLookupTables::kOffsets.cast(); 34 | }(); 35 | // clang-format on 36 | 37 | } // namespace voxblox 38 | -------------------------------------------------------------------------------- /voxblox/src/utils/protobuf_utils.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/utils/protobuf_utils.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace voxblox { 8 | 9 | namespace utils { 10 | bool readProtoMsgCountFromStream(std::istream* stream_in, 11 | uint32_t* message_count, 12 | uint64_t* byte_offset) { 13 | CHECK_NOTNULL(stream_in); 14 | CHECK_NOTNULL(message_count); 15 | CHECK_NOTNULL(byte_offset); 16 | stream_in->clear(); 17 | stream_in->seekg(*byte_offset, std::ios::beg); 18 | google::protobuf::io::IstreamInputStream raw_in(stream_in); 19 | google::protobuf::io::CodedInputStream coded_in(&raw_in); 20 | const int prev_position = coded_in.CurrentPosition(); 21 | if (!coded_in.ReadVarint32(message_count)) { 22 | LOG(ERROR) << "Could not read message size."; 23 | return false; 24 | } 25 | *byte_offset += coded_in.CurrentPosition() - prev_position; 26 | return true; 27 | } 28 | 29 | bool writeProtoMsgCountToStream(uint32_t message_count, 30 | std::fstream* stream_out) { 31 | CHECK_NOTNULL(stream_out); 32 | CHECK(stream_out->is_open()); 33 | stream_out->clear(); 34 | google::protobuf::io::OstreamOutputStream raw_out(stream_out); 35 | google::protobuf::io::CodedOutputStream coded_out(&raw_out); 36 | coded_out.WriteVarint32(message_count); 37 | return true; 38 | } 39 | 40 | bool readProtoMsgFromStream(std::istream* stream_in, 41 | google::protobuf::Message* message, 42 | uint64_t* byte_offset) { 43 | CHECK_NOTNULL(stream_in); 44 | CHECK_NOTNULL(message); 45 | CHECK_NOTNULL(byte_offset); 46 | 47 | stream_in->clear(); 48 | stream_in->seekg(*byte_offset, std::ios::beg); 49 | google::protobuf::io::IstreamInputStream raw_in(stream_in); 50 | google::protobuf::io::CodedInputStream coded_in(&raw_in); 51 | 52 | const int prev_position = coded_in.CurrentPosition(); 53 | 54 | uint32_t message_size; 55 | if (!coded_in.ReadVarint32(&message_size)) { 56 | LOG(ERROR) << "Could not read protobuf message size."; 57 | return false; 58 | } 59 | 60 | if (message_size == 0u) { 61 | LOG(ERROR) << "Empty protobuf message!"; 62 | return false; 63 | } 64 | google::protobuf::io::CodedInputStream::Limit limit = 65 | coded_in.PushLimit(message_size); 66 | if (!message->ParseFromCodedStream(&coded_in)) { 67 | LOG(ERROR) << "Could not parse stream."; 68 | return false; 69 | } 70 | if (!coded_in.ConsumedEntireMessage()) { 71 | LOG(ERROR) << "Could not consume protobuf message."; 72 | return false; 73 | } 74 | coded_in.PopLimit(limit); 75 | *byte_offset += coded_in.CurrentPosition() - prev_position; 76 | return true; 77 | } 78 | 79 | bool writeProtoMsgToStream(const google::protobuf::Message& message, 80 | std::fstream* stream_out) { 81 | CHECK_NOTNULL(stream_out); 82 | CHECK(stream_out->is_open()); 83 | google::protobuf::io::OstreamOutputStream raw_out(stream_out); 84 | google::protobuf::io::CodedOutputStream coded_out(&raw_out); 85 | const uint32_t size_bytes = message.ByteSize(); 86 | coded_out.WriteVarint32(size_bytes); 87 | uint8_t* buffer = coded_out.GetDirectBufferForNBytesAndAdvance(size_bytes); 88 | if (buffer != nullptr) { 89 | message.SerializeWithCachedSizesToArray(buffer); 90 | } else { 91 | message.SerializeWithCachedSizes(&coded_out); 92 | if (coded_out.HadError()) { 93 | return false; 94 | } 95 | } 96 | 97 | return true; 98 | } 99 | 100 | } // namespace utils 101 | } // namespace voxblox 102 | -------------------------------------------------------------------------------- /voxblox/src/utils/voxel_utils.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/utils/voxel_utils.h" 2 | 3 | #include "voxblox/core/color.h" 4 | #include "voxblox/core/common.h" 5 | #include "voxblox/core/voxel.h" 6 | 7 | namespace voxblox { 8 | 9 | template <> 10 | void mergeVoxelAIntoVoxelB(const TsdfVoxel& voxel_A, TsdfVoxel* voxel_B) { 11 | float combined_weight = voxel_A.weight + voxel_B->weight; 12 | if (combined_weight > 0) { 13 | voxel_B->distance = (voxel_A.distance * voxel_A.weight + 14 | voxel_B->distance * voxel_B->weight) / 15 | combined_weight; 16 | 17 | voxel_B->color = Color::blendTwoColors(voxel_A.color, voxel_A.weight, 18 | voxel_B->color, voxel_B->weight); 19 | 20 | voxel_B->weight = combined_weight; 21 | } 22 | } 23 | 24 | template <> 25 | void mergeVoxelAIntoVoxelB(const EsdfVoxel& voxel_A, EsdfVoxel* voxel_B) { 26 | if (voxel_A.observed && voxel_B->observed) { 27 | voxel_B->distance = (voxel_A.distance + voxel_B->distance) / 2.f; 28 | } else if (voxel_A.observed && !voxel_B->observed) { 29 | voxel_B->distance = voxel_A.distance; 30 | } 31 | voxel_B->observed = voxel_B->observed || voxel_A.observed; 32 | } 33 | 34 | template <> 35 | void mergeVoxelAIntoVoxelB(const OccupancyVoxel& voxel_A, 36 | OccupancyVoxel* voxel_B) { 37 | voxel_B->probability_log += voxel_A.probability_log; 38 | voxel_B->observed = voxel_B->observed || voxel_A.observed; 39 | } 40 | 41 | } // namespace voxblox 42 | -------------------------------------------------------------------------------- /voxblox/test/test_approx_hash_array.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "voxblox/core/block_hash.h" 7 | #include "voxblox/core/common.h" 8 | #include "voxblox/utils/approx_hash_array.h" 9 | 10 | using namespace voxblox; // NOLINT 11 | 12 | class ApproxHashArrayTest : public ::testing::Test { 13 | protected: 14 | virtual void SetUp() {} 15 | }; 16 | 17 | TEST_F(ApproxHashArrayTest, InsertValues) { 18 | // storing 256 ints 19 | ApproxHashArray<8, int, AnyIndex, AnyIndexHash> approx_hash_array; 20 | 21 | // write 512 values, first 256 should be overwritten 22 | for (int i = 0; i < 512; ++i) { 23 | approx_hash_array.get(i) = i; 24 | } 25 | 26 | for (int i = 0; i < 256; ++i) { 27 | EXPECT_EQ(approx_hash_array.get(i), i + 256); 28 | } 29 | } 30 | 31 | TEST_F(ApproxHashArrayTest, ArrayRandomWriteRead) { 32 | // storing 65536 size_ts 33 | ApproxHashArray<16, size_t, AnyIndex, AnyIndexHash> approx_hash_array; 34 | 35 | // generate 1000 random elements 36 | AlignedVector rand_indexes; 37 | std::mt19937 gen(1); 38 | std::uniform_int_distribution<> dis(1, std::numeric_limits::max()); 39 | for (int i = 0; i < 1000; ++i) { 40 | rand_indexes.push_back(AnyIndex(dis(gen), dis(gen), dis(gen))); 41 | } 42 | 43 | // insert their hash 44 | AnyIndexHash hasher; 45 | for (const AnyIndex& rand_index : rand_indexes) { 46 | size_t hash = hasher(rand_index); 47 | approx_hash_array.get(rand_index) = hash; 48 | } 49 | 50 | // find out how many we can recover 51 | int recovered = 0; 52 | for (const AnyIndex& rand_index : rand_indexes) { 53 | size_t hash = hasher(rand_index); 54 | if (approx_hash_array.get(rand_index) == hash) { 55 | ++recovered; 56 | } 57 | } 58 | 59 | // require at least a 95% success rate 60 | EXPECT_GT(recovered, 950); 61 | } 62 | 63 | TEST_F(ApproxHashArrayTest, SetRandomWriteRead) { 64 | // storing 65536 size_ts 65 | ApproxHashSet<16, 10, AnyIndex, AnyIndexHash> approx_hash_set; 66 | 67 | // generate 1000 random elements 68 | AlignedVector rand_indexes; 69 | std::mt19937 gen(1); 70 | std::uniform_int_distribution<> dis(1, std::numeric_limits::max()); 71 | for (int i = 0; i < 1000; ++i) { 72 | rand_indexes.push_back(AnyIndex(dis(gen), dis(gen), dis(gen))); 73 | } 74 | 75 | // test insert and clearing 76 | for (size_t i = 0; i < 50u; ++i) { 77 | approx_hash_set.resetApproxSet(); 78 | 79 | // insert their values 80 | int inserted = 0; 81 | for (const AnyIndex& rand_index : rand_indexes) { 82 | if (approx_hash_set.replaceHash(rand_index)) { 83 | ++inserted; 84 | } 85 | } 86 | // require at least a 95% success rate 87 | EXPECT_GT(inserted, 950); 88 | 89 | // insert a second time 90 | inserted = 0; 91 | for (const AnyIndex& rand_index : rand_indexes) { 92 | if (approx_hash_set.replaceHash(rand_index)) { 93 | ++inserted; 94 | } 95 | } 96 | // require at least a 95% failure rate 97 | EXPECT_LT(inserted, 50); 98 | } 99 | 100 | // find out how many we can recover 101 | int recovered = 0; 102 | for (const AnyIndex& rand_index : rand_indexes) { 103 | AnyIndexHash hasher; 104 | size_t hash = hasher(rand_index); 105 | if (approx_hash_set.isHashCurrentlyPresent(hash)) { 106 | ++recovered; 107 | } 108 | } 109 | 110 | // require at least a 95% success rate 111 | EXPECT_GT(recovered, 950); 112 | } 113 | 114 | TEST_F(ApproxHashArrayTest, SetRandomWriteReadLongIndex) { 115 | // storing 65536 size_ts 116 | ApproxHashSet<16, 10, LongIndex, LongIndexHash> approx_hash_set; 117 | 118 | // generate 1000 random elements 119 | AlignedVector rand_indexes; 120 | std::mt19937 gen(1); 121 | std::uniform_int_distribution dis( 122 | 1, std::numeric_limits::max()); 123 | for (int i = 0; i < 1000; ++i) { 124 | rand_indexes.push_back(LongIndex(dis(gen), dis(gen), dis(gen))); 125 | } 126 | 127 | // test insert and clearing 128 | for (size_t i = 0; i < 50u; ++i) { 129 | approx_hash_set.resetApproxSet(); 130 | 131 | // insert their values 132 | unsigned int inserted = 0; 133 | for (const LongIndex& rand_index : rand_indexes) { 134 | if (approx_hash_set.replaceHash(rand_index)) { 135 | ++inserted; 136 | } 137 | } 138 | // require at least a 95% success rate 139 | EXPECT_GT(inserted, 950u); 140 | 141 | // insert a second time 142 | inserted = 0; 143 | for (const LongIndex& rand_index : rand_indexes) { 144 | if (approx_hash_set.replaceHash(rand_index)) { 145 | ++inserted; 146 | } 147 | } 148 | // require at least a 95% failure rate 149 | EXPECT_LT(inserted, 50u); 150 | } 151 | 152 | // find out how many we can recover 153 | unsigned int recovered = 0; 154 | for (const LongIndex& rand_index : rand_indexes) { 155 | LongIndexHash hasher; 156 | size_t hash = hasher(rand_index); 157 | if (approx_hash_set.isHashCurrentlyPresent(hash)) { 158 | ++recovered; 159 | } 160 | } 161 | 162 | // require at least a 95% success rate 163 | EXPECT_GT(recovered, 950u); 164 | } 165 | 166 | int main(int argc, char** argv) { 167 | ::testing::InitGoogleTest(&argc, argv); 168 | google::InitGoogleLogging(argv[0]); 169 | 170 | int result = RUN_ALL_TESTS(); 171 | 172 | return result; 173 | } 174 | -------------------------------------------------------------------------------- /voxblox/test/test_bucket_queue.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "voxblox/utils/bucket_queue.h" 4 | 5 | namespace voxblox { 6 | 7 | double randomDoubleInRange(double f_min, double f_max) { 8 | double f = static_cast(rand()) / RAND_MAX; // NOLINT 9 | return f_min + f * (f_max - f_min); 10 | } 11 | 12 | TEST(BucketQueueTest, GetSmallestValue) { 13 | // Make sure this is deterministic. 14 | std::srand(0); 15 | 16 | // We're going to have a vector of random distances from 0. 17 | constexpr size_t kNumDistances = 100; 18 | constexpr double kMaxDistance = 20.0; 19 | constexpr int kNumBuckets = 20; 20 | 21 | // Create a bucket queue. It maps vector index to distance. 22 | BucketQueue bucket_queue(kNumBuckets, kMaxDistance); 23 | 24 | std::vector distances; 25 | distances.resize(kNumDistances); 26 | for (size_t i = 0; i < kNumDistances; i++) { 27 | distances[i] = randomDoubleInRange(-kMaxDistance, kMaxDistance); 28 | bucket_queue.push(i, distances[i]); 29 | } 30 | 31 | double last_val = 0.0; 32 | // Twice because we could switch buckets between. 33 | const double max_diff = 2 * kMaxDistance / (kNumBuckets - 1); 34 | while (!bucket_queue.empty()) { 35 | size_t ind = bucket_queue.front(); 36 | double val = distances[ind]; 37 | bucket_queue.pop(); 38 | EXPECT_LT(std::abs(std::abs(val) - std::abs(last_val)), max_diff); 39 | last_val = val; 40 | } 41 | 42 | for (size_t i = 0; i < kNumDistances; i++) { 43 | bucket_queue.push(i, distances[i]); 44 | } 45 | last_val = 0.0; 46 | for (size_t i = 0; i < kNumDistances / 2.0; i++) { 47 | size_t ind = bucket_queue.front(); 48 | double val = distances[ind]; 49 | bucket_queue.pop(); 50 | EXPECT_LT(std::abs(std::abs(val) - std::abs(last_val)), max_diff); 51 | last_val = val; 52 | } 53 | 54 | for (size_t i = 0; i < kNumDistances / 2.0; i++) { 55 | bucket_queue.push(i, distances[i]); 56 | } 57 | last_val = 0.0; 58 | while (!bucket_queue.empty()) { 59 | size_t ind = bucket_queue.front(); 60 | double val = distances[ind]; 61 | bucket_queue.pop(); 62 | EXPECT_LT(std::abs(std::abs(val) - std::abs(last_val)), max_diff); 63 | last_val = val; 64 | } 65 | } 66 | 67 | } // namespace voxblox 68 | 69 | int main(int argc, char** argv) { 70 | ::testing::InitGoogleTest(&argc, argv); 71 | google::InitGoogleLogging(argv[0]); 72 | 73 | int result = RUN_ALL_TESTS(); 74 | 75 | return result; 76 | } 77 | -------------------------------------------------------------------------------- /voxblox/test/test_layer.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "voxblox/Block.pb.h" 4 | #include "voxblox/Layer.pb.h" 5 | #include "voxblox/core/block.h" 6 | #include "voxblox/core/layer.h" 7 | #include "voxblox/core/voxel.h" 8 | #include "voxblox/test/layer_test_utils.h" 9 | #include "voxblox/utils/layer_utils.h" 10 | 11 | using namespace voxblox; // NOLINT 12 | 13 | template 14 | class LayerTest : public ::testing::Test, 15 | public voxblox::test::LayerTest { 16 | protected: 17 | virtual void SetUp() { 18 | layer_.reset(new Layer(voxel_size_, voxels_per_side_)); 19 | SetUpLayer(); 20 | } 21 | 22 | void SetUpLayer() const { 23 | voxblox::test::SetUpTestLayer(kBlockVolumeDiameter, layer_.get()); 24 | } 25 | 26 | typename Layer::Ptr layer_; 27 | 28 | const double voxel_size_ = 0.02; 29 | const size_t voxels_per_side_ = 16u; 30 | 31 | static constexpr size_t kBlockVolumeDiameter = 10u; 32 | }; 33 | 34 | typedef LayerTest TsdfLayerTest; 35 | typedef LayerTest EsdfLayerTest; 36 | typedef LayerTest OccupancyLayerTest; 37 | 38 | TEST_F(TsdfLayerTest, DeepCopyConstructor) { 39 | Layer new_layer(*layer_); 40 | EXPECT_TRUE(voxblox::utils::isSameLayer(new_layer, *layer_)); 41 | } 42 | 43 | TEST_F(EsdfLayerTest, DeepCopyConstructor) { 44 | Layer new_layer(*layer_); 45 | EXPECT_TRUE(voxblox::utils::isSameLayer(new_layer, *layer_)); 46 | } 47 | 48 | TEST_F(OccupancyLayerTest, DeepCopyConstructor) { 49 | Layer new_layer(*layer_); 50 | EXPECT_TRUE(voxblox::utils::isSameLayer(new_layer, *layer_)); 51 | } 52 | 53 | int main(int argc, char** argv) { 54 | ::testing::InitGoogleTest(&argc, argv); 55 | google::InitGoogleLogging(argv[0]); 56 | 57 | int result = RUN_ALL_TESTS(); 58 | 59 | return result; 60 | } 61 | -------------------------------------------------------------------------------- /voxblox/test/test_layer_utils.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "voxblox/core/block.h" 5 | #include "voxblox/core/layer.h" 6 | #include "voxblox/core/voxel.h" 7 | #include "voxblox/simulation/simulation_world.h" 8 | #include "voxblox/test/layer_test_utils.h" 9 | #include "voxblox/utils/layer_utils.h" 10 | 11 | using namespace voxblox; // NOLINT 12 | 13 | template 14 | class LayerUtilsTest : public ::testing::Test, 15 | public voxblox::test::LayerTest { 16 | protected: 17 | void initializeSimulatedWorld() { 18 | world_.reset(new Layer(voxel_size_, voxels_per_side_)); 19 | 20 | SimulationWorld simulation; 21 | Point lower_bound, upper_bound; 22 | lower_bound << -0.51, -0.51, -0.51; 23 | upper_bound << 0.9, 0.9, 0.9; 24 | expected_new_coordinate_origin_ << 0.16, 0.16, 0.16; 25 | 26 | simulation.setBounds(lower_bound, upper_bound); 27 | 28 | // Define sphere 1. 29 | Point c1_A; 30 | c1_A << 0.104, 0.104, 0.106; 31 | constexpr FloatingPoint sphere_1_radius_m = 0.05; 32 | 33 | // Define sphere 2. 34 | Point c2_A; 35 | c2_A << -0.104, 0.103, 0.203; 36 | constexpr FloatingPoint sphere_2_radius_m = 0.03; 37 | 38 | // Define sphere 3. 39 | Point c3_A; 40 | c3_A << 0.401, 0.151, 0.203; 41 | constexpr FloatingPoint sphere_3_radius_m = 0.075; 42 | 43 | // Prepare world A 44 | simulation.addObject(std::unique_ptr( 45 | new Sphere(c1_A, sphere_1_radius_m, Color(255u, 0u, 0u)))); 46 | simulation.addObject(std::unique_ptr( 47 | new Sphere(c2_A, sphere_2_radius_m, Color(255u, 0u, 0u)))); 48 | simulation.addObject(std::unique_ptr( 49 | new Sphere(c3_A, sphere_3_radius_m, Color(255u, 0u, 0u)))); 50 | simulation.generateSdfFromWorld(max_distance_world, world_.get()); 51 | } 52 | 53 | virtual void SetUp() { initializeSimulatedWorld(); } 54 | 55 | typename Layer::Ptr world_; 56 | 57 | Point expected_new_coordinate_origin_; 58 | 59 | const double voxel_size_ = 0.005; 60 | const size_t voxels_per_side_ = 16u; 61 | const double max_distance_world = 2.0; 62 | 63 | static constexpr FloatingPoint kPrecision = 1e-6; 64 | }; 65 | 66 | typedef LayerUtilsTest TsdfLayerUtilsTest; 67 | typedef LayerUtilsTest EsdfLayerUtilsTest; 68 | typedef LayerUtilsTest OccupancyLayerUtilsTest; 69 | 70 | TEST_F(TsdfLayerUtilsTest, centerBlocksOfLayerTsdfTest) { 71 | Point new_layer_origin; 72 | utils::centerBlocksOfLayer(world_.get(), &new_layer_origin); 73 | 74 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(new_layer_origin, 75 | expected_new_coordinate_origin_, kPrecision)); 76 | } 77 | 78 | TEST_F(EsdfLayerUtilsTest, centerBlocksOfLayerEsdfTest) { 79 | Point new_layer_origin; 80 | utils::centerBlocksOfLayer(world_.get(), &new_layer_origin); 81 | 82 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(new_layer_origin, 83 | expected_new_coordinate_origin_, kPrecision)); 84 | } 85 | 86 | int main(int argc, char** argv) { 87 | ::testing::InitGoogleTest(&argc, argv); 88 | google::InitGoogleLogging(argv[0]); 89 | 90 | int result = RUN_ALL_TESTS(); 91 | 92 | return result; 93 | } 94 | -------------------------------------------------------------------------------- /voxblox/test/test_load_esdf.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "voxblox/core/block.h" 7 | #include "voxblox/core/layer.h" 8 | #include "voxblox/core/voxel.h" 9 | #include "voxblox/io/layer_io.h" 10 | 11 | using namespace voxblox; // NOLINT 12 | 13 | int main(int argc, char** argv) { 14 | google::InitGoogleLogging(argv[0]); 15 | 16 | if (argc != 2) { 17 | throw std::runtime_error("Args: filename to load"); 18 | } 19 | 20 | const std::string file = argv[1]; 21 | 22 | Layer::Ptr layer_from_file; 23 | io::LoadLayer(file, &layer_from_file); 24 | 25 | LOG(INFO) << "Layer memory size: " << layer_from_file->getMemorySize(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /voxblox/test/tsdf_to_esdf.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include "voxblox/core/block.h" 8 | #include "voxblox/core/esdf_map.h" 9 | #include "voxblox/core/layer.h" 10 | #include "voxblox/core/voxel.h" 11 | #include "voxblox/integrator/esdf_integrator.h" 12 | #include "voxblox/io/layer_io.h" 13 | 14 | using namespace voxblox; // NOLINT 15 | 16 | int main(int argc, char** argv) { 17 | google::InitGoogleLogging(argv[0]); 18 | 19 | if (argc != 7) { 20 | throw std::runtime_error( 21 | std::string("Args: filename to load, filename to save to") + 22 | ", min weight, min fixed distance" + 23 | ", max esdf distance, default esdf distance"); 24 | } 25 | 26 | const std::string file = argv[1]; 27 | const FloatingPoint min_weight = std::stof(argv[3]); 28 | const FloatingPoint min_distance_m = std::stof(argv[4]); 29 | const FloatingPoint max_distance_m = std::stof(argv[5]); 30 | const FloatingPoint default_distance_m = std::stof(argv[6]); 31 | 32 | Layer::Ptr layer_from_file; 33 | io::LoadLayer(file, &layer_from_file); 34 | 35 | LOG(INFO) << "Layer memory size: " << layer_from_file->getMemorySize(); 36 | LOG(INFO) << "Layer voxel size: " << layer_from_file->voxel_size(); 37 | LOG(INFO) << "Layer voxels per side: " << layer_from_file->voxels_per_side(); 38 | 39 | // ESDF maps. 40 | EsdfMap::Config esdf_config; 41 | // Same number of voxels per side for ESDF as with TSDF 42 | esdf_config.esdf_voxels_per_side = layer_from_file->voxels_per_side(); 43 | // Same voxel size for ESDF as with TSDF 44 | esdf_config.esdf_voxel_size = layer_from_file->voxel_size(); 45 | 46 | EsdfIntegrator::Config esdf_integrator_config; 47 | esdf_integrator_config.min_weight = min_weight; 48 | esdf_integrator_config.min_distance_m = min_distance_m; 49 | esdf_integrator_config.max_distance_m = max_distance_m; 50 | esdf_integrator_config.default_distance_m = default_distance_m; 51 | 52 | EsdfMap esdf_map(esdf_config); 53 | EsdfIntegrator esdf_integrator(esdf_integrator_config, layer_from_file.get(), 54 | esdf_map.getEsdfLayerPtr()); 55 | 56 | esdf_integrator.updateFromTsdfLayerBatch(); 57 | 58 | const bool esdf_success = io::SaveLayer(esdf_map.getEsdfLayer(), argv[2]); 59 | 60 | if (esdf_success == false) { 61 | throw std::runtime_error("Failed to save ESDF"); 62 | } 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /voxblox_https.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: https://github.com/ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: eigen_checks 9 | uri: https://github.com/ethz-asl/eigen_checks.git 10 | - git: 11 | local-name: gflags_catkin 12 | uri: https://github.com/ethz-asl/gflags_catkin.git 13 | - git: 14 | local-name: glog_catkin 15 | uri: https://github.com/ethz-asl/glog_catkin.git 16 | - git: 17 | local-name: minkindr 18 | uri: https://github.com/ethz-asl/minkindr.git 19 | - git: 20 | local-name: minkindr_ros 21 | uri: https://github.com/ethz-asl/minkindr_ros.git 22 | - git: 23 | local-name: protobuf_catkin 24 | uri: https://github.com/ethz-asl/protobuf_catkin.git 25 | - git: 26 | local-name: numpy_eigen 27 | uri: https://github.com/ethz-asl/numpy_eigen.git 28 | - git: 29 | local-name: catkin_boost_python_buildtool 30 | uri: https://github.com/ethz-asl/catkin_boost_python_buildtool.git 31 | - git: 32 | local-name: catkin_grpc 33 | uri: https://github.com/CogRob/catkin_grpc.git 34 | -------------------------------------------------------------------------------- /voxblox_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxblox_msgs) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ########## 8 | # EXPORT # 9 | ########## 10 | 11 | cs_install() 12 | cs_export() 13 | -------------------------------------------------------------------------------- /voxblox_msgs/msg/Block.msg: -------------------------------------------------------------------------------- 1 | # Block indices -- as integers, actual position in space is function of 2 | # the voxel_size in the layer and voxels_per_side in the layer. 3 | int32 x_index 4 | int32 y_index 5 | int32 z_index 6 | 7 | # Voxel data packed in 4-byte chunks to better mirror protobuf serialization. 8 | uint32[] data 9 | -------------------------------------------------------------------------------- /voxblox_msgs/msg/Layer.msg: -------------------------------------------------------------------------------- 1 | # Layer definitions 2 | float64 voxel_size 3 | uint32 voxels_per_side 4 | string layer_type # See voxblox::voxel_types 5 | 6 | # Whether to send a full map or an incremental update. 7 | uint8 action # See action defines below 8 | 9 | voxblox_msgs/Block[] blocks 10 | 11 | # Action definitions 12 | # Update all blocks that are part of this message to the new state, 13 | # leave the rest of the map as it was. 14 | uint8 ACTION_UPDATE = 0 15 | # Merge all blocks that are part of this message with the current state of 16 | # the map, leave the rest of the map as it was. 17 | uint8 ACTION_MERGE = 1 18 | # Set the layer to the state described by this message. 19 | uint8 ACTION_RESET = 2 20 | -------------------------------------------------------------------------------- /voxblox_msgs/msg/Mesh.msg: -------------------------------------------------------------------------------- 1 | # vertex positions rounded to nearest BLOCK_EDGE_LENGTH/65535 2 | # no alpha information 3 | 4 | std_msgs/Header header 5 | 6 | float32 block_edge_length 7 | 8 | voxblox_msgs/MeshBlock[] mesh_blocks 9 | -------------------------------------------------------------------------------- /voxblox_msgs/msg/MeshBlock.msg: -------------------------------------------------------------------------------- 1 | # vertex positions rounded to nearest BLOCK_EDGE_LENGTH/65535 2 | # no alpha information 3 | 4 | # Index of meshed points in block map 5 | int64[3] index 6 | 7 | # Triangle information (always in groups of 3) 8 | uint16[] x 9 | uint16[] y 10 | uint16[] z 11 | 12 | # Color information may be missing 13 | uint8[] r 14 | uint8[] g 15 | uint8[] b -------------------------------------------------------------------------------- /voxblox_msgs/msg/VoxelEvaluationDetails.msg: -------------------------------------------------------------------------------- 1 | float64 max_error 2 | float64 min_error 3 | uint32 num_evaluated_voxels 4 | uint32 num_ignored_voxels 5 | uint32 num_overlapping_voxels 6 | uint32 num_non_overlapping_voxels 7 | -------------------------------------------------------------------------------- /voxblox_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxblox_msgs 4 | 0.0.0 5 | Message definitions for Voxblox ROS interface. 6 | 7 | Helen Oleynikova 8 | Marius Fehr 9 | 10 | Helen Oleynikova 11 | Marius Fehr 12 | 13 | BSD 14 | 15 | catkin 16 | catkin_simple 17 | 18 | message_generation 19 | message_runtime 20 | std_msgs 21 | 22 | -------------------------------------------------------------------------------- /voxblox_msgs/srv/FilePath.srv: -------------------------------------------------------------------------------- 1 | string file_path 2 | --- 3 | -------------------------------------------------------------------------------- /voxblox_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxblox_ros) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | add_definitions(-std=c++14 -Wall -Wextra) 8 | 9 | ############# 10 | # LIBRARIES # 11 | ############# 12 | cs_add_library(${PROJECT_NAME} 13 | src/esdf_server.cc 14 | src/interactive_slider.cc 15 | src/simulation_server.cc 16 | src/intensity_server.cc 17 | src/transformer.cc 18 | src/tsdf_server.cc 19 | ) 20 | 21 | ############ 22 | # BINARIES # 23 | ############ 24 | 25 | cs_add_executable(voxblox_eval 26 | src/voxblox_eval.cc 27 | ) 28 | target_link_libraries(voxblox_eval ${PROJECT_NAME}) 29 | 30 | cs_add_executable(tsdf_server 31 | src/tsdf_server_node.cc 32 | ) 33 | target_link_libraries(tsdf_server ${PROJECT_NAME}) 34 | 35 | cs_add_executable(esdf_server 36 | src/esdf_server_node.cc 37 | ) 38 | target_link_libraries(esdf_server ${PROJECT_NAME}) 39 | 40 | cs_add_executable(intensity_server 41 | src/intensity_server_node.cc 42 | ) 43 | target_link_libraries(intensity_server ${PROJECT_NAME}) 44 | 45 | cs_add_executable(simulation_eval 46 | src/simulation_eval.cc 47 | ) 48 | target_link_libraries(simulation_eval ${PROJECT_NAME}) 49 | 50 | cs_add_executable(visualize_tsdf 51 | src/visualize_tsdf.cc 52 | ) 53 | target_link_libraries(visualize_tsdf ${PROJECT_NAME}) 54 | 55 | ########## 56 | # EXPORT # 57 | ########## 58 | cs_install() 59 | cs_export() 60 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/calibrations/euroc_camchain.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | T_cam_imu: 3 | - [0.014067833389924844, 0.9996023098557362, -0.024440094001271714, 0.0657094717307726] 4 | - [-0.9998997399258257, 0.014103075047756597, 0.0012701852087405128, -0.01662153508680676] 5 | - [0.0016143605484757653, 0.0244197748817434, 0.9997004893640622, 0.0014868599890966905] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | cam_overlaps: [1] 8 | camera_model: pinhole 9 | distortion_coeffs: [-0.011602611404146694, 0.05399058892805103, -0.07542693754837938, 10 | 0.03666365316319072] 11 | distortion_model: equidistant 12 | intrinsics: [461.3418503026154, 460.13885656354387, 366.4281018364134, 248.8415565479774] 13 | resolution: [752, 480] 14 | rostopic: /cam0/image_raw 15 | flip_camera: true 16 | cam1: 17 | T_cam_imu: 18 | - [0.011810542390279122, 0.9995951714398011, -0.02588444171084584, -0.04439379107435547] 19 | - [-0.9998052969172723, 0.01221433647773218, 0.015497685006934178, -0.016235309745499065] 20 | - [0.01580757238202092, 0.025696365864524458, 0.9995448051171812, 0.0012030335279962774] 21 | - [0.0, 0.0, 0.0, 1.0] 22 | T_cn_cnm1: 23 | - [0.9999964092230391, 0.0022551294207702314, -0.0014477335121195142, -0.11006339056724425] 24 | - [-0.002234380261272062, 0.9998970009983116, 0.014177268405057909, 0.000510253673254922] 25 | - [0.0014795559720994585, -0.014173982710466103, 0.9998984494078623, -0.0006164896618526655] 26 | - [0.0, 0.0, 0.0, 1.0] 27 | cam_overlaps: [0] 28 | camera_model: pinhole 29 | distortion_coeffs: [0.005730625619095735, -0.009587210461496316, 0.01921165649877144, 30 | -0.010717188529046507] 31 | distortion_model: equidistant 32 | intrinsics: [459.6820431465863, 458.37659300282706, 380.01198798196646, 255.55347731935623] 33 | resolution: [752, 480] 34 | rostopic: /cam1/image_raw 35 | flip_camera: true 36 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/cow_and_lady.yaml: -------------------------------------------------------------------------------- 1 | # actually T_R_C (C = cam0, R = rgbd cam) 2 | T_B_C: 3 | - [1.0, 0.0, 0.0, 0.0] 4 | - [0.0, 1.0, 0.0, 0.0] 5 | - [0.0, 0.0, 1.0, 0.0] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | invert_T_B_C: false 8 | 9 | # actually T_V_C (C = cam0, V = vicon) 10 | T_B_D: 11 | - [0.971048, -0.120915, 0.206023, 0.00114049] 12 | - [0.15701, 0.973037, -0.168959, 0.0450936] 13 | - [-0.180038, 0.196415, 0.96385, 0.0430765] 14 | - [0.0, 0.0, 0.0, 1.0] 15 | invert_T_B_D: true 16 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/cow_dataset.yaml: -------------------------------------------------------------------------------- 1 | # actually T_B_R where R = RGBD & body 2 | T_B_C: 3 | - [1.0, 0.0, 0.0, 0.0] 4 | - [0.0, 1.0, 0.0, 0.0] 5 | - [0.0, 0.0, 1.0, 0.0] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | invert_T_B_C: false 8 | 9 | # actually T_R_V (V = vicon) 10 | T_B_D: 11 | - [ -0.415057, -0.805836, 0.422321, -0.0316981] 12 | - [ -0.909533, 0.378665, -0.171354, 0.0303474] 13 | - [ -0.021835, -0.455236, -0.890102, -0.0558053] 14 | - [ 0, 0, 0, 1] 15 | invert_T_B_D: true 16 | 17 | # This is for the evaluation script. 18 | # Obtained by hand-ICP of the mesh to the GT pointcloud in CloudCompare. 19 | T_V_G: 20 | - [-0.842917, -0.534568, 0.061050, 9.267872] 21 | - [ 0.537996, -0.835894, 0.108821, 5.180613] 22 | - [-0.007141, 0.124572, 0.992185, 0.326235] 23 | - [ 0.000000, 0.000000, 0.000000, 1.000000] 24 | invert_T_V_G: true 25 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/euroc_dataset.yaml: -------------------------------------------------------------------------------- 1 | # actually T_I_C (B = I) for cam0 2 | T_B_C: 3 | - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] 4 | - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] 5 | - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | invert_T_B_C: false 8 | 9 | # If using datasets with GT attached: 10 | #T_B_D: 11 | #- [ 1.0, 0.0, 0.0, 0.0] 12 | #- [ 0.0, 1.0, 0.0, 0.0] 13 | #- [ 0.0, 0.0, 1.0, 0.0] 14 | #- [ 0.0, 0.0, 0.0, 1.0] 15 | #invert_T_B_D: false 16 | 17 | 18 | # For the raw datasets, use below: 19 | # actually T_I_V (V = vicon) 20 | T_B_D: 21 | - [ 0.33638, -0.01749, 0.94156, 0.06901] 22 | - [-0.02078, -0.99972, -0.01114, -0.02781] 23 | - [ 0.94150, -0.01582, -0.33665, -0.12395] 24 | - [ 0.0, 0.0, 0.0, 1.0] 25 | invert_T_B_D: false 26 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/rgbd_dataset.yaml: -------------------------------------------------------------------------------- 1 | # actually T_R_C (C = cam0, R = rgbd cam) 2 | T_B_C: 3 | - [0.999968, 0.003327, -0.007251, -0.001814] 4 | - [-0.003260, 0.999953, 0.009178, -0.028674] 5 | - [0.007281, -0.009154, 0.999932, -0.020372] 6 | - [0.000000, 0.000000, 0.000000, 1.000000] 7 | invert_T_B_C: true 8 | 9 | # actually T_V_C (C = cam0, V = vicon) 10 | T_B_D: 11 | - [0.999885, 0.014491, -0.004467, 0.053458] 12 | - [-0.014337, 0.999362, 0.032704, -0.006089] 13 | - [0.004938, -0.032636, 0.999455, -0.003491] 14 | - [0.000000, 0.000000, 0.000000, 1.000000] 15 | invert_T_B_D: true 16 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/stereo/kitti_stereo.yaml: -------------------------------------------------------------------------------- 1 | stereo_algorithm: 1 2 | pre_filter_cap: 63 3 | disparity_range: 128 4 | speckle_size: 100 5 | texture_threshold: 20 6 | uniqueness_ratio: 30 7 | speckle_range: 32 8 | P1: 900 9 | P2: 7200 10 | correlation_window_size: 15 11 | fullDP: 1 12 | disp12MaxDiff: 0 13 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/stereo/kitti_stereo_bm.yaml: -------------------------------------------------------------------------------- 1 | stereo_algorithm: 0 2 | correlation_window_size: 15 3 | pre_filter_cap: 41 4 | disparity_range: 128 5 | min_disparity: 2 6 | speckle_size: 100 7 | texture_threshold: 5 8 | uniqueness_ratio: 10 9 | speckle_range: 20 10 | # From OpenCV Block Matching [OCV-BM] 11 | -------------------------------------------------------------------------------- /voxblox_ros/cfg/stereo/kitti_stereo_jager.yaml: -------------------------------------------------------------------------------- 1 | stereo_algorithm: 1 2 | correlation_window_size: 9 3 | pre_filter_cap: 31 4 | disparity_range: 128 5 | min_disparity: 2 6 | speckle_size: 200 7 | texture_threshold: 20 8 | uniqueness_ratio: 25 9 | speckle_range: 5 10 | P1: 120 11 | P2: 240 12 | fullDP: 0 13 | disp12MaxDiff: 1 14 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/conversions_inl.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_CONVERSIONS_INL_H_ 2 | #define VOXBLOX_ROS_CONVERSIONS_INL_H_ 3 | 4 | #include 5 | 6 | namespace voxblox { 7 | 8 | template 9 | void serializeLayerAsMsg(const Layer& layer, const bool only_updated, 10 | voxblox_msgs::Layer* msg, 11 | const MapDerializationAction& action) { 12 | CHECK_NOTNULL(msg); 13 | msg->voxels_per_side = layer.voxels_per_side(); 14 | msg->voxel_size = layer.voxel_size(); 15 | 16 | msg->layer_type = getVoxelType(); 17 | 18 | BlockIndexList block_list; 19 | if (only_updated) { 20 | layer.getAllUpdatedBlocks(Update::kMap, &block_list); 21 | } else { 22 | layer.getAllAllocatedBlocks(&block_list); 23 | } 24 | 25 | msg->action = static_cast(action); 26 | 27 | voxblox_msgs::Block block_msg; 28 | msg->blocks.reserve(block_list.size()); 29 | for (const BlockIndex& index : block_list) { 30 | block_msg.x_index = index.x(); 31 | block_msg.y_index = index.y(); 32 | block_msg.z_index = index.z(); 33 | 34 | std::vector data; 35 | layer.getBlockByIndex(index).serializeToIntegers(&data); 36 | 37 | block_msg.data = data; 38 | msg->blocks.push_back(block_msg); 39 | } 40 | } // namespace voxblox 41 | 42 | template 43 | bool deserializeMsgToLayer(const voxblox_msgs::Layer& msg, 44 | Layer* layer) { 45 | CHECK_NOTNULL(layer); 46 | return deserializeMsgToLayer( 47 | msg, static_cast(msg.action), layer); 48 | } 49 | 50 | template 51 | bool deserializeMsgToLayer(const voxblox_msgs::Layer& msg, 52 | const MapDerializationAction& action, 53 | Layer* layer) { 54 | CHECK_NOTNULL(layer); 55 | if (getVoxelType().compare(msg.layer_type) != 0) { 56 | return false; 57 | } 58 | 59 | // So we also need to check if the sizes match. If they don't, we can't 60 | // parse this at all. 61 | constexpr double kVoxelSizeEpsilon = 1e-5; 62 | if (msg.voxels_per_side != layer->voxels_per_side() || 63 | std::abs(msg.voxel_size - layer->voxel_size()) > kVoxelSizeEpsilon) { 64 | LOG(ERROR) << "Sizes don't match!"; 65 | return false; 66 | } 67 | 68 | if (action == MapDerializationAction::kReset) { 69 | LOG(INFO) << "Resetting current layer."; 70 | layer->removeAllBlocks(); 71 | } 72 | 73 | for (const voxblox_msgs::Block& block_msg : msg.blocks) { 74 | BlockIndex index(block_msg.x_index, block_msg.y_index, block_msg.z_index); 75 | 76 | // Either we want to update an existing block or there was no block there 77 | // before. 78 | if (action == MapDerializationAction::kUpdate || !layer->hasBlock(index)) { 79 | // Create a new block if it doesn't exist yet, or get the existing one 80 | // at the correct block index. 81 | typename Block::Ptr block_ptr = 82 | layer->allocateBlockPtrByIndex(index); 83 | 84 | std::vector data = block_msg.data; 85 | block_ptr->deserializeFromIntegers(data); 86 | 87 | } else if (action == MapDerializationAction::kMerge) { 88 | typename Block::Ptr old_block_ptr = 89 | layer->getBlockPtrByIndex(index); 90 | CHECK(old_block_ptr); 91 | 92 | typename Block::Ptr new_block_ptr(new Block( 93 | old_block_ptr->voxels_per_side(), old_block_ptr->voxel_size(), 94 | old_block_ptr->origin())); 95 | 96 | std::vector data = block_msg.data; 97 | new_block_ptr->deserializeFromIntegers(data); 98 | 99 | old_block_ptr->mergeBlock(*new_block_ptr); 100 | } 101 | } 102 | 103 | switch (action) { 104 | case MapDerializationAction::kReset: 105 | CHECK_EQ(layer->getNumberOfAllocatedBlocks(), msg.blocks.size()); 106 | break; 107 | case MapDerializationAction::kUpdate: 108 | // Fall through intended. 109 | case MapDerializationAction::kMerge: 110 | CHECK_GE(layer->getNumberOfAllocatedBlocks(), msg.blocks.size()); 111 | break; 112 | } 113 | 114 | return true; 115 | } 116 | 117 | } // namespace voxblox 118 | 119 | #endif // VOXBLOX_ROS_CONVERSIONS_INL_H_ 120 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/esdf_server.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_ESDF_SERVER_H_ 2 | #define VOXBLOX_ROS_ESDF_SERVER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "voxblox_ros/tsdf_server.h" 12 | 13 | namespace voxblox { 14 | 15 | class EsdfServer : public TsdfServer { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | EsdfServer(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 20 | EsdfServer(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, 21 | const EsdfMap::Config& esdf_config, 22 | const EsdfIntegrator::Config& esdf_integrator_config, 23 | const TsdfMap::Config& tsdf_config, 24 | const TsdfIntegratorBase::Config& tsdf_integrator_config, 25 | const MeshIntegratorConfig& mesh_config); 26 | virtual ~EsdfServer() {} 27 | 28 | bool generateEsdfCallback(std_srvs::Empty::Request& request, // NOLINT 29 | std_srvs::Empty::Response& response); // NOLINT 30 | 31 | void publishAllUpdatedEsdfVoxels(); 32 | virtual void publishSlices(); 33 | void publishTraversable(); 34 | 35 | virtual void publishPointclouds(); 36 | virtual void newPoseCallback(const Transformation& T_G_C); 37 | virtual void publishMap(bool reset_remote_map = false); 38 | virtual bool saveMap(const std::string& file_path); 39 | virtual bool loadMap(const std::string& file_path); 40 | 41 | void updateEsdfEvent(const ros::TimerEvent& event); 42 | 43 | /// Call this to update the ESDF based on latest state of the TSDF map, 44 | /// considering only the newly updated parts of the TSDF map (checked with 45 | /// the ESDF updated bit in Update::Status). 46 | void updateEsdf(); 47 | /// Update the ESDF all at once; clear the existing map. 48 | void updateEsdfBatch(bool full_euclidean = false); 49 | 50 | // Overwrites the layer with what's coming from the topic! 51 | void esdfMapCallback(const voxblox_msgs::Layer& layer_msg); 52 | 53 | inline std::shared_ptr getEsdfMapPtr() { return esdf_map_; } 54 | inline std::shared_ptr getEsdfMapPtr() const { 55 | return esdf_map_; 56 | } 57 | 58 | bool getClearSphere() const { return clear_sphere_for_planning_; } 59 | void setClearSphere(bool clear_sphere_for_planning) { 60 | clear_sphere_for_planning_ = clear_sphere_for_planning; 61 | } 62 | float getEsdfMaxDistance() const; 63 | void setEsdfMaxDistance(float max_distance); 64 | float getTraversabilityRadius() const; 65 | void setTraversabilityRadius(float traversability_radius); 66 | 67 | /** 68 | * These are for enabling or disabling incremental update of the ESDF. Use 69 | * carefully. 70 | */ 71 | void disableIncrementalUpdate() { incremental_update_ = false; } 72 | void enableIncrementalUpdate() { incremental_update_ = true; } 73 | 74 | virtual void clear(); 75 | 76 | protected: 77 | /// Sets up publishing and subscribing. Should only be called from 78 | /// constructor. 79 | void setupRos(); 80 | 81 | /// Publish markers for visualization. 82 | ros::Publisher esdf_pointcloud_pub_; 83 | ros::Publisher esdf_slice_pub_; 84 | ros::Publisher traversable_pub_; 85 | 86 | /// Publish the complete map for other nodes to consume. 87 | ros::Publisher esdf_map_pub_; 88 | 89 | /// Subscriber to subscribe to another node generating the map. 90 | ros::Subscriber esdf_map_sub_; 91 | 92 | /// Services. 93 | ros::ServiceServer generate_esdf_srv_; 94 | 95 | /// Timers. 96 | ros::Timer update_esdf_timer_; 97 | 98 | bool clear_sphere_for_planning_; 99 | bool publish_esdf_map_; 100 | bool publish_traversable_; 101 | float traversability_radius_; 102 | bool incremental_update_; 103 | int num_subscribers_esdf_map_; 104 | 105 | // ESDF maps. 106 | std::shared_ptr esdf_map_; 107 | std::unique_ptr esdf_integrator_; 108 | }; 109 | 110 | } // namespace voxblox 111 | 112 | #endif // VOXBLOX_ROS_ESDF_SERVER_H_ 113 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/intensity_server.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_INTENSITY_SERVER_H_ 2 | #define VOXBLOX_ROS_INTENSITY_SERVER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "voxblox_ros/intensity_vis.h" 14 | #include "voxblox_ros/tsdf_server.h" 15 | 16 | namespace voxblox { 17 | 18 | class IntensityServer : public TsdfServer { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | IntensityServer(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 23 | virtual ~IntensityServer() {} 24 | 25 | virtual void updateMesh(); 26 | virtual void publishPointclouds(); 27 | 28 | void intensityImageCallback(const sensor_msgs::ImageConstPtr& image); 29 | 30 | protected: 31 | /// Subscriber for intensity images. 32 | ros::Subscriber intensity_image_sub_; 33 | 34 | // Publish markers for visualization. 35 | ros::Publisher intensity_pointcloud_pub_; 36 | ros::Publisher intensity_mesh_pub_; 37 | 38 | /// Parameters of the incoming UNDISTORTED intensity images. 39 | double focal_length_px_; 40 | 41 | /** How much to subsample the image by (not proper downsampling, just 42 | * subsampling). 43 | */ 44 | int subsample_factor_; 45 | 46 | // Intensity layer, integrator, and color maps, all related to storing 47 | // and visualizing intensity data. 48 | std::shared_ptr> intensity_layer_; 49 | std::unique_ptr intensity_integrator_; 50 | 51 | // Visualization tools. 52 | std::shared_ptr color_map_; 53 | }; 54 | 55 | } // namespace voxblox 56 | 57 | #endif // VOXBLOX_ROS_INTENSITY_SERVER_H_ 58 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/intensity_vis.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_INTENSITY_VIS_H_ 2 | #define VOXBLOX_ROS_INTENSITY_VIS_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "voxblox_ros/conversions.h" 10 | #include "voxblox_ros/mesh_vis.h" 11 | 12 | namespace voxblox { 13 | 14 | inline void recolorVoxbloxMeshMsgByIntensity( 15 | const Layer& intensity_layer, 16 | const std::shared_ptr& color_map, voxblox_msgs::Mesh* mesh_msg) { 17 | CHECK_NOTNULL(mesh_msg); 18 | CHECK(color_map); 19 | 20 | // Go over all the blocks in the mesh. 21 | for (voxblox_msgs::MeshBlock& mesh_block : mesh_msg->mesh_blocks) { 22 | // Look up verticies in the thermal layer. 23 | for (size_t vert_idx = 0u; vert_idx < mesh_block.x.size(); ++vert_idx) { 24 | // only needed if color information was originally missing 25 | mesh_block.r.resize(mesh_block.x.size()); 26 | mesh_block.g.resize(mesh_block.x.size()); 27 | mesh_block.b.resize(mesh_block.x.size()); 28 | 29 | const IntensityVoxel* voxel = intensity_layer.getVoxelPtrByCoordinates( 30 | Point(mesh_block.x[vert_idx], mesh_block.y[vert_idx], 31 | mesh_block.z[vert_idx])); 32 | if (voxel != nullptr && voxel->weight > 0.0) { 33 | float intensity = voxel->intensity; 34 | Color new_color = color_map->colorLookup(intensity); 35 | mesh_block.r[vert_idx] = new_color.r; 36 | mesh_block.g[vert_idx] = new_color.g; 37 | mesh_block.b[vert_idx] = new_color.b; 38 | } 39 | } 40 | } 41 | } 42 | 43 | } // namespace voxblox 44 | 45 | #endif // VOXBLOX_ROS_INTENSITY_VIS_H_ 46 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/interactive_slider.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_INTERACTIVE_SLIDER_H_ 2 | #define VOXBLOX_ROS_INTERACTIVE_SLIDER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace voxblox { 13 | 14 | /// InteractiveSlider class which can be used for visualizing voxel map slices. 15 | class InteractiveSlider { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | InteractiveSlider( 20 | const std::string& slider_name, 21 | const std::function& slider_callback, 22 | const Point& initial_position, const unsigned int free_plane_index, 23 | const float marker_scale_meters); 24 | virtual ~InteractiveSlider() {} 25 | 26 | private: 27 | const unsigned int free_plane_index_; 28 | interactive_markers::InteractiveMarkerServer interactive_marker_server_; 29 | 30 | /// Processes the feedback after moving the slider. 31 | virtual void interactiveMarkerFeedback( 32 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback, 33 | const std::function& slider_callback); 34 | }; 35 | 36 | } // namespace voxblox 37 | 38 | #endif // VOXBLOX_ROS_INTERACTIVE_SLIDER_H_ 39 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/mesh_pcl.h: -------------------------------------------------------------------------------- 1 | 2 | // The MIT License (MIT) 3 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // Mesh output taken from open_chisel: github.com/personalrobotics/OpenChisel 24 | 25 | #ifndef VOXBLOX_ROS_MESH_PCL_H_ 26 | #define VOXBLOX_ROS_MESH_PCL_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace voxblox { 39 | 40 | inline void toPCLPolygonMesh( 41 | const MeshLayer& mesh_layer, const std::string frame_id, 42 | pcl::PolygonMesh* polygon_mesh_ptr, 43 | const bool simplify_and_connect_mesh = true, 44 | const FloatingPoint vertex_proximity_threshold = 1e-10) { 45 | CHECK_NOTNULL(polygon_mesh_ptr); 46 | 47 | // Constructing the vertices pointcloud 48 | pcl::PointCloud pointcloud; 49 | std::vector polygons; 50 | 51 | Mesh mesh; 52 | convertMeshLayerToMesh(mesh_layer, &mesh, simplify_and_connect_mesh, 53 | vertex_proximity_threshold); 54 | 55 | // add points 56 | pointcloud.reserve(mesh.vertices.size()); 57 | for (const Point& point : mesh.vertices) { 58 | pointcloud.push_back(pcl::PointXYZ(static_cast(point[0]), 59 | static_cast(point[1]), 60 | static_cast(point[2]))); 61 | } 62 | // add triangles 63 | pcl::Vertices vertices_idx; 64 | polygons.reserve(mesh.indices.size() / 3); 65 | for (const VertexIndex& idx : mesh.indices) { 66 | vertices_idx.vertices.push_back(idx); 67 | 68 | if (vertices_idx.vertices.size() == 3) { 69 | polygons.push_back(vertices_idx); 70 | vertices_idx.vertices.clear(); 71 | } 72 | } 73 | 74 | // Converting to the pointcloud binary 75 | pcl::PCLPointCloud2 pointcloud2; 76 | pcl::toPCLPointCloud2(pointcloud, pointcloud2); 77 | // Filling the mesh 78 | polygon_mesh_ptr->header.frame_id = frame_id; 79 | polygon_mesh_ptr->cloud = pointcloud2; 80 | polygon_mesh_ptr->polygons = polygons; 81 | } 82 | 83 | inline void toSimplifiedPCLPolygonMesh( 84 | const MeshLayer& mesh_layer, const std::string frame_id, 85 | const FloatingPoint vertex_proximity_threshold, 86 | pcl::PolygonMesh* polygon_mesh_ptr) { 87 | constexpr bool kSimplifiedAndConnectedMesh = true; 88 | toPCLPolygonMesh(mesh_layer, frame_id, polygon_mesh_ptr, 89 | kSimplifiedAndConnectedMesh, vertex_proximity_threshold); 90 | } 91 | 92 | inline void toConnectedPCLPolygonMesh(const MeshLayer& mesh_layer, 93 | const std::string frame_id, 94 | pcl::PolygonMesh* polygon_mesh_ptr) { 95 | constexpr bool kSimplifiedAndConnectedMesh = true; 96 | constexpr FloatingPoint kVertexThreshold = 1e-10; 97 | toPCLPolygonMesh(mesh_layer, frame_id, polygon_mesh_ptr, 98 | kSimplifiedAndConnectedMesh, kVertexThreshold); 99 | } 100 | 101 | } // namespace voxblox 102 | 103 | #endif // VOXBLOX_ROS_MESH_PCL_H_ 104 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/simulation_server.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_SIMULATION_SERVER_H_ 2 | #define VOXBLOX_ROS_SIMULATION_SERVER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "voxblox_ros/conversions.h" 19 | #include "voxblox_ros/mesh_vis.h" 20 | #include "voxblox_ros/ptcloud_vis.h" 21 | #include "voxblox_ros/ros_params.h" 22 | 23 | namespace voxblox { 24 | 25 | class SimulationServer { 26 | public: 27 | SimulationServer(const ros::NodeHandle& nh, 28 | const ros::NodeHandle& nh_private); 29 | 30 | SimulationServer(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, 31 | const EsdfMap::Config& esdf_config, 32 | const EsdfIntegrator::Config& esdf_integrator_config, 33 | const TsdfMap::Config& tsdf_config, 34 | const TsdfIntegratorBase::Config& tsdf_integrator_config); 35 | 36 | virtual ~SimulationServer() {} 37 | 38 | /// Runs all of the below functions in the correct order: 39 | void run(); 40 | 41 | /// Creates a new world, and prepares ground truth SDF(s). 42 | virtual void prepareWorld() = 0; 43 | 44 | /// Generates a SDF by generating random poses and putting them into an SDF. 45 | void generateSDF(); 46 | 47 | /// Evaluate errors... 48 | void evaluate(); 49 | 50 | /// Visualize results. :) 51 | void visualize(); 52 | 53 | protected: 54 | void getServerConfigFromRosParam(const ros::NodeHandle& nh_private); 55 | 56 | /// Convenience function to generate valid viewpoints. 57 | bool generatePlausibleViewpoint(FloatingPoint min_distance, Point* ray_origin, 58 | Point* ray_direction) const; 59 | 60 | ros::NodeHandle nh_; 61 | ros::NodeHandle nh_private_; 62 | 63 | // A bunch of publishers :) 64 | ros::Publisher sim_pub_; 65 | ros::Publisher tsdf_gt_pub_; 66 | ros::Publisher esdf_gt_pub_; 67 | ros::Publisher tsdf_gt_mesh_pub_; 68 | ros::Publisher tsdf_test_pub_; 69 | ros::Publisher esdf_test_pub_; 70 | ros::Publisher tsdf_test_mesh_pub_; 71 | ros::Publisher view_ptcloud_pub_; 72 | 73 | // Settings 74 | FloatingPoint voxel_size_; 75 | int voxels_per_side_; 76 | std::string world_frame_; 77 | bool generate_occupancy_; 78 | bool visualize_; 79 | FloatingPoint visualization_slice_level_; 80 | bool generate_mesh_; 81 | bool incremental_; 82 | bool add_robot_pose_; 83 | FloatingPoint truncation_distance_; 84 | FloatingPoint esdf_max_distance_; 85 | size_t max_attempts_to_generate_viewpoint_; 86 | 87 | // Camera settings 88 | Eigen::Vector2i depth_camera_resolution_; 89 | FloatingPoint fov_h_rad_; 90 | FloatingPoint max_dist_; 91 | FloatingPoint min_dist_; 92 | int num_viewpoints_; 93 | 94 | // Actual simulation server. 95 | std::unique_ptr world_; 96 | 97 | // Maps (GT and generates from sensors) generated here. 98 | std::unique_ptr > tsdf_gt_; 99 | std::unique_ptr > esdf_gt_; 100 | 101 | // Generated maps: 102 | std::unique_ptr > tsdf_test_; 103 | std::unique_ptr > esdf_test_; 104 | std::unique_ptr > occ_test_; 105 | 106 | // Integrators: 107 | std::unique_ptr tsdf_integrator_; 108 | std::unique_ptr esdf_integrator_; 109 | std::unique_ptr occ_integrator_; 110 | std::unique_ptr esdf_occ_integrator_; 111 | }; 112 | 113 | } // namespace voxblox 114 | 115 | #endif // VOXBLOX_ROS_SIMULATION_SERVER_H_ 116 | -------------------------------------------------------------------------------- /voxblox_ros/include/voxblox_ros/transformer.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_ROS_TRANSFORMER_H_ 2 | #define VOXBLOX_ROS_TRANSFORMER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace voxblox { 12 | 13 | /** 14 | * Class that binds to either the TF tree or resolves transformations from the 15 | * ROS parameter server, depending on settings loaded from ROS params. 16 | */ 17 | class Transformer { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | Transformer(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 22 | 23 | bool lookupTransform(const std::string& from_frame, 24 | const std::string& to_frame, const ros::Time& timestamp, 25 | Transformation* transform); 26 | 27 | void transformCallback(const geometry_msgs::TransformStamped& transform_msg); 28 | 29 | private: 30 | bool lookupTransformTf(const std::string& from_frame, 31 | const std::string& to_frame, 32 | const ros::Time& timestamp, Transformation* transform); 33 | 34 | bool lookupTransformQueue(const ros::Time& timestamp, 35 | Transformation* transform); 36 | 37 | ros::NodeHandle nh_; 38 | ros::NodeHandle nh_private_; 39 | 40 | /** 41 | * Global/map coordinate frame. Will always look up TF transforms to this 42 | * frame. 43 | */ 44 | std::string world_frame_; 45 | /// If set, overwrite sensor frame with this value. If empty, unused. 46 | std::string sensor_frame_; 47 | /** 48 | * Whether to use TF transform resolution (true) or fixed transforms from 49 | * parameters and transform topics (false). 50 | */ 51 | bool use_tf_transforms_; 52 | int64_t timestamp_tolerance_ns_; 53 | /** 54 | * B is the body frame of the robot, C is the camera/sensor frame creating 55 | * the pointclouds, and D is the 'dynamic' frame; i.e., incoming messages 56 | * are assumed to be T_G_D. 57 | */ 58 | Transformation T_B_C_; 59 | Transformation T_B_D_; 60 | /** 61 | * If we use topic transforms, we have 2 parts: a dynamic transform from a 62 | * topic and a static transform from parameters. 63 | * Static transform should be T_G_D (where D is whatever sensor the 64 | * dynamic coordinate frame is in) and the static should be T_D_C (where 65 | * C is the sensor frame that produces the depth data). It is possible to 66 | * specify T_C_D and set invert_static_transform to true. 67 | */ 68 | 69 | /** 70 | * To be replaced (at least optionally) with odometry + static transform 71 | * from IMU to visual frame. 72 | */ 73 | tf::TransformListener tf_listener_; 74 | 75 | // l Only used if use_tf_transforms_ set to false. 76 | ros::Subscriber transform_sub_; 77 | 78 | // l Transform queue, used only when use_tf_transforms is false. 79 | AlignedDeque transform_queue_; 80 | }; 81 | 82 | } // namespace voxblox 83 | 84 | #endif // VOXBLOX_ROS_TRANSFORMER_H_ 85 | -------------------------------------------------------------------------------- /voxblox_ros/launch/basement_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /voxblox_ros/launch/cow_and_lady_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /voxblox_ros/launch/euroc_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /voxblox_ros/launch/eval/eval_cow.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /voxblox_ros/launch/eval/eval_cow_and_lady.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /voxblox_ros/launch/eval/eval_euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /voxblox_ros/launch/eval/eval_kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /voxblox_ros/launch/kitti_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /voxblox_ros/launch/rgbd_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /voxblox_ros/launch/sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /voxblox_ros/mesh_results/.keep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/voxblox/c8066b04075d2fee509de295346b1c0b788c4f38/voxblox_ros/mesh_results/.keep -------------------------------------------------------------------------------- /voxblox_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxblox_ros 4 | 0.0.0 5 | Voxblox ROS interface 6 | 7 | Helen Oleynikova 8 | Marius Fehr 9 | 10 | Helen Oleynikova 11 | Marius Fehr 12 | 13 | BSD 14 | 15 | catkin 16 | catkin_simple 17 | 18 | cv_bridge 19 | gflags_catkin 20 | interactive_markers 21 | minkindr_conversions 22 | pcl_conversions 23 | pcl_ros 24 | sensor_msgs 25 | tf 26 | voxblox 27 | voxblox_msgs 28 | voxblox_rviz_plugin 29 | 30 | -------------------------------------------------------------------------------- /voxblox_ros/src/esdf_server_node.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox_ros/esdf_server.h" 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | ros::init(argc, argv, "voxblox"); 7 | google::InitGoogleLogging(argv[0]); 8 | google::ParseCommandLineFlags(&argc, &argv, false); 9 | google::InstallFailureSignalHandler(); 10 | ros::NodeHandle nh; 11 | ros::NodeHandle nh_private("~"); 12 | 13 | voxblox::EsdfServer node(nh, nh_private); 14 | 15 | ros::spin(); 16 | return 0; 17 | } 18 | -------------------------------------------------------------------------------- /voxblox_ros/src/intensity_server.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox_ros/intensity_server.h" 2 | 3 | namespace voxblox { 4 | 5 | IntensityServer::IntensityServer(const ros::NodeHandle& nh, 6 | const ros::NodeHandle& nh_private) 7 | : TsdfServer(nh, nh_private), 8 | focal_length_px_(400.0f), 9 | subsample_factor_(12) { 10 | cache_mesh_ = true; 11 | 12 | intensity_layer_.reset( 13 | new Layer(tsdf_map_->getTsdfLayer().voxel_size(), 14 | tsdf_map_->getTsdfLayer().voxels_per_side())); 15 | intensity_integrator_.reset(new IntensityIntegrator(tsdf_map_->getTsdfLayer(), 16 | intensity_layer_.get())); 17 | 18 | // Get ROS params: 19 | nh_private_.param("intensity_focal_length", focal_length_px_, 20 | focal_length_px_); 21 | nh_private_.param("subsample_factor", subsample_factor_, subsample_factor_); 22 | 23 | float intensity_min_value = 10.0f; 24 | float intensity_max_value = 40.0f; 25 | nh_private_.param("intensity_min_value", intensity_min_value, 26 | intensity_min_value); 27 | nh_private_.param("intensity_max_value", intensity_max_value, 28 | intensity_max_value); 29 | 30 | FloatingPoint intensity_max_distance = 31 | intensity_integrator_->getMaxDistance(); 32 | nh_private_.param("intensity_max_distance", intensity_max_distance, 33 | intensity_max_distance); 34 | intensity_integrator_->setMaxDistance(intensity_max_distance); 35 | 36 | // Publishers for output. 37 | intensity_pointcloud_pub_ = 38 | nh_private_.advertise >( 39 | "intensity_pointcloud", 1, true); 40 | intensity_mesh_pub_ = 41 | nh_private_.advertise("intensity_mesh", 1, true); 42 | 43 | color_map_.reset(new IronbowColorMap()); 44 | color_map_->setMinValue(intensity_min_value); 45 | color_map_->setMaxValue(intensity_max_value); 46 | 47 | // Set up subscriber. 48 | intensity_image_sub_ = nh_private_.subscribe( 49 | "intensity_image", 1, &IntensityServer::intensityImageCallback, this); 50 | } 51 | 52 | void IntensityServer::updateMesh() { 53 | TsdfServer::updateMesh(); 54 | 55 | // Now recolor the mesh... 56 | timing::Timer publish_mesh_timer("intensity_mesh/publish"); 57 | recolorVoxbloxMeshMsgByIntensity(*intensity_layer_, color_map_, 58 | &cached_mesh_msg_); 59 | intensity_mesh_pub_.publish(cached_mesh_msg_); 60 | publish_mesh_timer.Stop(); 61 | } 62 | 63 | void IntensityServer::publishPointclouds() { 64 | // Create a pointcloud with temperature = intensity. 65 | pcl::PointCloud pointcloud; 66 | 67 | createIntensityPointcloudFromIntensityLayer(*intensity_layer_, &pointcloud); 68 | 69 | pointcloud.header.frame_id = world_frame_; 70 | intensity_pointcloud_pub_.publish(pointcloud); 71 | 72 | TsdfServer::publishPointclouds(); 73 | } 74 | 75 | void IntensityServer::intensityImageCallback( 76 | const sensor_msgs::ImageConstPtr& image) { 77 | CHECK(intensity_layer_); 78 | CHECK(intensity_integrator_); 79 | CHECK(image); 80 | // Look up transform first... 81 | Transformation T_G_C; 82 | if (!transformer_.lookupTransform(image->header.frame_id, world_frame_, 83 | image->header.stamp, &T_G_C)) { 84 | ROS_WARN_THROTTLE(10, "Failed to look up intensity transform!"); 85 | return; 86 | } 87 | 88 | cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image); 89 | 90 | CHECK(cv_ptr); 91 | 92 | const size_t num_pixels = 93 | cv_ptr->image.rows * cv_ptr->image.cols / subsample_factor_; 94 | 95 | float half_row = cv_ptr->image.rows / 2.0; 96 | float half_col = cv_ptr->image.cols / 2.0; 97 | 98 | // Pre-allocate the bearing vectors and intensities. 99 | Pointcloud bearing_vectors; 100 | bearing_vectors.reserve(num_pixels + 1); 101 | std::vector intensities; 102 | intensities.reserve(num_pixels + 1); 103 | 104 | size_t k = 0; 105 | size_t m = 0; 106 | for (int i = 0; i < cv_ptr->image.rows; i++) { 107 | const float* image_row = cv_ptr->image.ptr(i); 108 | for (int j = 0; j < cv_ptr->image.cols; j++) { 109 | if (m % subsample_factor_ == 0) { 110 | // Rotates the vector pointing from the camera center to the pixel 111 | // into the global frame, and normalizes it. 112 | bearing_vectors.push_back( 113 | T_G_C.getRotation().toImplementation() * 114 | Point(j - half_col, i - half_row, focal_length_px_).normalized()); 115 | intensities.push_back(image_row[j]); 116 | k++; 117 | } 118 | m++; 119 | } 120 | } 121 | 122 | // Put this into the integrator. 123 | intensity_integrator_->addIntensityBearingVectors( 124 | T_G_C.getPosition(), bearing_vectors, intensities); 125 | } 126 | 127 | } // namespace voxblox 128 | -------------------------------------------------------------------------------- /voxblox_ros/src/intensity_server_node.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox_ros/intensity_server.h" 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | ros::init(argc, argv, "voxblox"); 7 | google::InitGoogleLogging(argv[0]); 8 | google::ParseCommandLineFlags(&argc, &argv, false); 9 | google::InstallFailureSignalHandler(); 10 | ros::NodeHandle nh; 11 | ros::NodeHandle nh_private("~"); 12 | 13 | voxblox::IntensityServer node(nh, nh_private); 14 | 15 | ros::spin(); 16 | return 0; 17 | } 18 | -------------------------------------------------------------------------------- /voxblox_ros/src/interactive_slider.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox_ros/interactive_slider.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace voxblox { 7 | 8 | InteractiveSlider::InteractiveSlider( 9 | const std::string& slider_name, 10 | const std::function& slider_callback, 11 | const Point& initial_position, const unsigned int free_plane_index, 12 | const float marker_scale_meters) 13 | : free_plane_index_(free_plane_index), 14 | interactive_marker_server_(slider_name) { 15 | CHECK(!slider_name.empty()); 16 | CHECK_LT(free_plane_index, 3u); 17 | 18 | // Create an interactive marker. 19 | visualization_msgs::InteractiveMarker interactive_marker; 20 | interactive_marker.header.frame_id = "map"; 21 | interactive_marker.header.stamp = ros::Time::now(); 22 | interactive_marker.pose.position.x = 23 | static_cast(initial_position.x()); 24 | interactive_marker.pose.position.y = 25 | static_cast(initial_position.y()); 26 | interactive_marker.pose.position.z = 27 | static_cast(initial_position.z()); 28 | 29 | // Create a marker. 30 | visualization_msgs::Marker marker; 31 | marker.type = visualization_msgs::Marker::CUBE; 32 | marker.scale.x = marker_scale_meters; 33 | marker.scale.y = marker_scale_meters; 34 | marker.scale.z = marker_scale_meters; 35 | constexpr float kGreyValue = 0.1f; 36 | marker.color.r = kGreyValue; 37 | marker.color.g = kGreyValue; 38 | marker.color.b = kGreyValue; 39 | marker.color.a = 1.0f; 40 | 41 | // Control for moving the marker in the direction of the free plane index. 42 | visualization_msgs::InteractiveMarkerControl control; 43 | control.interaction_mode = 44 | visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 45 | control.orientation.w = 1.0; 46 | control.orientation.x = 0.0; 47 | control.orientation.y = 0.0; 48 | control.orientation.z = 0.0; 49 | switch (free_plane_index_) { 50 | case 0u: 51 | marker.scale.x *= 0.1f; 52 | control.orientation.x = 1.0; 53 | break; 54 | case 1u: 55 | marker.scale.y *= 0.1f; 56 | control.orientation.z = 1.0; 57 | break; 58 | case 2u: 59 | marker.scale.z *= 0.1f; 60 | control.orientation.y = 1.0; 61 | } 62 | interactive_marker.controls.push_back(control); 63 | 64 | // Control for moving the marker in the plane which is orthogonal to the free 65 | // plane index direction. 66 | control.interaction_mode = 67 | visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; 68 | control.markers.push_back(marker); 69 | interactive_marker.controls.push_back(control); 70 | 71 | // Add interactive marker to server. 72 | interactive_marker_server_.insert( 73 | interactive_marker, 74 | std::bind(&InteractiveSlider::interactiveMarkerFeedback, this, 75 | std::placeholders::_1, slider_callback)); 76 | interactive_marker_server_.applyChanges(); 77 | 78 | // Initial callback. 79 | switch (free_plane_index_) { 80 | case 0u: 81 | slider_callback(interactive_marker.pose.position.x); 82 | break; 83 | case 1u: 84 | slider_callback(interactive_marker.pose.position.y); 85 | break; 86 | case 2u: 87 | slider_callback(interactive_marker.pose.position.z); 88 | } 89 | } 90 | 91 | void InteractiveSlider::interactiveMarkerFeedback( 92 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback, 93 | const std::function& slider_callback) { 94 | if (feedback->event_type == 95 | visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) { 96 | switch (free_plane_index_) { 97 | case 0u: 98 | slider_callback(feedback->pose.position.x); 99 | break; 100 | case 1u: 101 | slider_callback(feedback->pose.position.y); 102 | break; 103 | case 2u: 104 | slider_callback(feedback->pose.position.z); 105 | } 106 | } 107 | } 108 | 109 | } // namespace voxblox 110 | -------------------------------------------------------------------------------- /voxblox_ros/src/simulation_eval.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "voxblox_ros/simulation_server.h" 6 | 7 | namespace voxblox { 8 | class SimulationServerImpl : public voxblox::SimulationServer { 9 | public: 10 | SimulationServerImpl(const ros::NodeHandle& nh, 11 | const ros::NodeHandle& nh_private) 12 | : SimulationServer(nh, nh_private) {} 13 | 14 | void prepareWorld() { 15 | CHECK_NOTNULL(world_); 16 | world_->addObject(std::unique_ptr( 17 | new Sphere(Point(0.0, 0.0, 2.0), 2.0, Color::Red()))); 18 | 19 | world_->addObject(std::unique_ptr(new PlaneObject( 20 | Point(-2.0, -4.0, 2.0), Point(0, 1, 0), Color::White()))); 21 | 22 | world_->addObject(std::unique_ptr( 23 | new PlaneObject(Point(4.0, 0.0, 0.0), Point(-1, 0, 0), Color::Pink()))); 24 | 25 | world_->addObject(std::unique_ptr( 26 | new Cube(Point(-4.0, 4.0, 2.0), Point(4, 4, 4), Color::Green()))); 27 | 28 | world_->addGroundLevel(0.03); 29 | 30 | world_->generateSdfFromWorld(truncation_distance_, tsdf_gt_.get()); 31 | world_->generateSdfFromWorld(esdf_max_distance_, esdf_gt_.get()); 32 | } 33 | }; 34 | 35 | } // namespace voxblox 36 | 37 | int main(int argc, char** argv) { 38 | ros::init(argc, argv, "voxblox_sim"); 39 | google::InitGoogleLogging(argv[0]); 40 | google::ParseCommandLineFlags(&argc, &argv, false); 41 | google::InstallFailureSignalHandler(); 42 | ros::NodeHandle nh; 43 | ros::NodeHandle nh_private("~"); 44 | 45 | voxblox::SimulationServerImpl sim_eval(nh, nh_private); 46 | 47 | sim_eval.run(); 48 | 49 | ROS_INFO("Done."); 50 | ros::spin(); 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /voxblox_ros/src/tsdf_server_node.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox_ros/tsdf_server.h" 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | ros::init(argc, argv, "voxblox"); 7 | google::InitGoogleLogging(argv[0]); 8 | google::ParseCommandLineFlags(&argc, &argv, false); 9 | google::InstallFailureSignalHandler(); 10 | ros::NodeHandle nh; 11 | ros::NodeHandle nh_private("~"); 12 | 13 | voxblox::TsdfServer node(nh, nh_private); 14 | 15 | ros::spin(); 16 | return 0; 17 | } 18 | -------------------------------------------------------------------------------- /voxblox_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxblox_rviz_plugin) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | set(CMAKE_MACOSX_RPATH 0) 8 | add_definitions(-std=c++11 -Wall) 9 | 10 | ## This setting causes Qt's "MOC" generation to happen automatically. 11 | set(CMAKE_AUTOMOC ON) 12 | 13 | ## This plugin includes Qt widgets, so we must include Qt. 14 | ## We'll use the version that rviz used so they are compatible. 15 | if(rviz_QT_VERSION VERSION_LESS "5") 16 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 17 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 18 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 19 | include(${QT_USE_FILE}) 20 | qt4_wrap_cpp(MOC_FILES 21 | ${INCLUDE_FILES_QT} 22 | ) 23 | else() 24 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 25 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 26 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 27 | set(QT_LIBRARIES Qt5::Widgets) 28 | QT5_WRAP_CPP(MOC_FILES 29 | ${INCLUDE_FILES_QT} 30 | ) 31 | endif() 32 | 33 | ## Avoid Qt signals and slots defining "emit", "slots", etc. 34 | add_definitions(-DQT_NO_KEYWORDS) 35 | 36 | set(HEADER_FILES include/voxblox_rviz_plugin/voxblox_mesh_display.h include/voxblox_rviz_plugin/voxblox_mesh_visual.h) 37 | 38 | set(SRC_FILES src/voxblox_mesh_display.cc src/voxblox_mesh_visual.cc ) 39 | 40 | cs_add_library(${PROJECT_NAME} 41 | ${SRC_FILES} 42 | ${HEADER_FILES} 43 | ${MOC_FILES} 44 | ) 45 | 46 | target_link_libraries(${PROJECT_NAME} 47 | ${QT_LIBRARIES} 48 | ) 49 | 50 | 51 | ########## 52 | # EXPORT # 53 | ########## 54 | 55 | install( 56 | FILES plugin_description.xml 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 58 | 59 | install( 60 | DIRECTORY icons/ 61 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 62 | 63 | cs_install() 64 | cs_export() 65 | -------------------------------------------------------------------------------- /voxblox_rviz_plugin/icons/classes/VoxbloxMesh.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/voxblox/c8066b04075d2fee509de295346b1c0b788c4f38/voxblox_rviz_plugin/icons/classes/VoxbloxMesh.png -------------------------------------------------------------------------------- /voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_mesh_display.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_DISPLAY_H_ 2 | #define VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_DISPLAY_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "voxblox_rviz_plugin/voxblox_mesh_visual.h" 10 | 11 | namespace voxblox_rviz_plugin { 12 | 13 | class VoxbloxMeshVisual; 14 | 15 | class VoxbloxMeshDisplay 16 | : public rviz::MessageFilterDisplay { 17 | Q_OBJECT 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | VoxbloxMeshDisplay(); 21 | virtual ~VoxbloxMeshDisplay(); 22 | 23 | protected: 24 | virtual void onInitialize(); 25 | 26 | virtual void reset(); 27 | 28 | private: 29 | void processMessage(const voxblox_msgs::Mesh::ConstPtr& msg); 30 | 31 | std::unique_ptr visual_; 32 | }; 33 | 34 | } // namespace voxblox_rviz_plugin 35 | 36 | #endif // VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_DISPLAY_H_ 37 | -------------------------------------------------------------------------------- /voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_mesh_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H_ 2 | #define VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace voxblox_rviz_plugin { 10 | 11 | /// Visualizes a single voxblox_msgs::Mesh message. 12 | class VoxbloxMeshVisual { 13 | public: 14 | VoxbloxMeshVisual(Ogre::SceneManager* scene_manager, 15 | Ogre::SceneNode* parent_node); 16 | virtual ~VoxbloxMeshVisual(); 17 | 18 | void setMessage(const voxblox_msgs::Mesh::ConstPtr& msg); 19 | 20 | /// Set the coordinate frame pose. 21 | void setFramePosition(const Ogre::Vector3& position); 22 | void setFrameOrientation(const Ogre::Quaternion& orientation); 23 | 24 | private: 25 | Ogre::SceneNode* frame_node_; 26 | Ogre::SceneManager* scene_manager_; 27 | 28 | unsigned int instance_number_; 29 | static unsigned int instance_counter_; 30 | 31 | voxblox::AnyIndexHashMapType::type object_map_; 32 | }; 33 | 34 | } // namespace voxblox_rviz_plugin 35 | 36 | #endif // VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H_ 37 | -------------------------------------------------------------------------------- /voxblox_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxblox_rviz_plugin 4 | 0.0.0 5 | RViz plugin for displaying incremental voxblox mesh messages. 6 | Zachary Taylor 7 | BSD 8 | Zachary Taylor 9 | 10 | catkin 11 | catkin_simple 12 | 13 | rviz 14 | voxblox 15 | voxblox_msgs 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /voxblox_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | Displays incremental voxblox mesh messages. 6 | voxblox_msgs/Mesh 7 | 8 | 9 | -------------------------------------------------------------------------------- /voxblox_rviz_plugin/src/voxblox_mesh_display.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox_rviz_plugin/voxblox_mesh_display.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace voxblox_rviz_plugin { 12 | 13 | VoxbloxMeshDisplay::VoxbloxMeshDisplay() {} 14 | 15 | void VoxbloxMeshDisplay::onInitialize() { MFDClass::onInitialize(); } 16 | 17 | VoxbloxMeshDisplay::~VoxbloxMeshDisplay() {} 18 | 19 | void VoxbloxMeshDisplay::reset() { 20 | MFDClass::reset(); 21 | visual_.reset(); 22 | } 23 | 24 | void VoxbloxMeshDisplay::processMessage( 25 | const voxblox_msgs::Mesh::ConstPtr& msg) { 26 | // Here we call the rviz::FrameManager to get the transform from the 27 | // fixed frame to the frame in the header of this Imu message. If 28 | // it fails, we can't do anything else so we return. 29 | Ogre::Quaternion orientation; 30 | Ogre::Vector3 position; 31 | if (!context_->getFrameManager()->getTransform( 32 | msg->header.frame_id, msg->header.stamp, position, orientation)) { 33 | ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", 34 | msg->header.frame_id.c_str(), qPrintable(fixed_frame_)); 35 | return; 36 | } 37 | 38 | if (visual_ == nullptr) { 39 | visual_.reset( 40 | new VoxbloxMeshVisual(context_->getSceneManager(), scene_node_)); 41 | } 42 | 43 | // Now set or update the contents of the chosen visual. 44 | visual_->setMessage(msg); 45 | visual_->setFramePosition(position); 46 | visual_->setFrameOrientation(orientation); 47 | } 48 | 49 | } // namespace voxblox_rviz_plugin 50 | 51 | #include 52 | PLUGINLIB_EXPORT_CLASS(voxblox_rviz_plugin::VoxbloxMeshDisplay, rviz::Display) 53 | -------------------------------------------------------------------------------- /voxblox_ssh.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: git@github.com:catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: git@github.com:ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: eigen_checks 9 | uri: git@github.com:ethz-asl/eigen_checks.git 10 | - git: 11 | local-name: gflags_catkin 12 | uri: git@github.com:ethz-asl/gflags_catkin.git 13 | - git: 14 | local-name: glog_catkin 15 | uri: git@github.com:ethz-asl/glog_catkin.git 16 | - git: 17 | local-name: minkindr 18 | uri: git@github.com:ethz-asl/minkindr.git 19 | - git: 20 | local-name: minkindr_ros 21 | uri: git@github.com:ethz-asl/minkindr_ros.git 22 | - git: 23 | local-name: protobuf_catkin 24 | uri: git@github.com:ethz-asl/protobuf_catkin.git 25 | - git: 26 | local-name: numpy_eigen 27 | uri: git@github.com:ethz-asl/numpy_eigen.git 28 | - git: 29 | local-name: catkin_boost_python_buildtool 30 | uri: git@github.com:ethz-asl/catkin_boost_python_buildtool.git 31 | - git: 32 | local-name: catkin_grpc 33 | uri: git@github.com:CogRob/catkin_grpc.git 34 | --------------------------------------------------------------------------------