├── .flake8 ├── .github └── workflows │ ├── format.yaml │ ├── release.yaml │ └── test.yaml ├── .gitignore ├── .gitmodules ├── .isort.cfg ├── .pyproject.toml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── action.sh ├── example ├── dataset │ ├── .gitignore │ ├── collect.py │ └── download_dataset.sh ├── ground_truth_cylinder.py ├── incremetal_update.py ├── real_data.py └── ros │ ├── .gitignore │ ├── download_rosbag.sh │ ├── example_node.py │ └── save_rosbag.sh ├── format.sh ├── mypy.ini ├── python └── voxbloxpy │ ├── __init__.py │ ├── core.py │ ├── py.typed │ └── ros.py ├── release ├── .gitignore ├── Dockerfile ├── check_glibcxx.sh └── python_versions.txt ├── setup.py ├── tests ├── test_all.py └── utils.py ├── voxblox ├── CMakeLists.txt ├── generated_proto │ └── .gitignore ├── 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 ├── 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 └── wrapper.cpp /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | ignore = 3 | I, H # follow isort 4 | E501 # max length should be checked by black 5 | E203 # https://github.com/psf/black/issues/315 6 | W503 # https://github.com/psf/black/issues/52 7 | A, B, C, CNL, D, Q # external plugins if installed 8 | -------------------------------------------------------------------------------- /.github/workflows/format.yaml: -------------------------------------------------------------------------------- 1 | name: Check format 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | branches: 9 | - master 10 | 11 | jobs: 12 | lint: 13 | runs-on: ubuntu-latest 14 | steps: 15 | - name: Checkout Code 16 | uses: actions/checkout@v2 17 | - name: pip install formatters 18 | run: | 19 | pip3 install black 20 | pip3 install isort 21 | pip3 install flake8 22 | 23 | - name: check by black 24 | run: | 25 | black --version 26 | black --config .pyproject.toml --check ./python ./example 27 | - name: check by isort 28 | run: | 29 | isort --version-number 30 | isort --check-only --quiet ./python ./example 31 | - name: check by flake8 32 | run: | 33 | flake8 --version 34 | flake8 ./python ./example 35 | -------------------------------------------------------------------------------- /.github/workflows/release.yaml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | push: 5 | tags: 6 | - 'v*' 7 | 8 | # https://stackoverflow.com/questions/63887031/build-docker-image-locally-in-github-actions-using-docker-build-push-action 9 | 10 | jobs: 11 | pypi: 12 | name: Release To PyPi 13 | runs-on: ubuntu-latest 14 | steps: 15 | - name: Checkout 16 | uses: actions/checkout@v2 17 | - name: Setup QEMU 18 | uses: docker/setup-qemu-action@v1 19 | - name: Set up Docker Buildx 20 | uses: docker/setup-buildx-action@v1 21 | with: 22 | driver: docker 23 | - name: build 24 | uses: docker/build-push-action@v2 25 | with: 26 | context: release 27 | push: false 28 | tags: pypi_release:latest 29 | file: release/Dockerfile 30 | 31 | - name: upload to pypi 32 | run: | 33 | docker run pypi_release:latest /bin/bash -i -c 'source ~/.bashrc; twine upload --skip-existing -u __token__ -p ${{ secrets.PYPI_TOKEN_VOXBLOXPY }} $HOME/voxbloxpy/dist/*' 34 | -------------------------------------------------------------------------------- /.github/workflows/test.yaml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | branches: 9 | - master 10 | 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | steps: 15 | - name: Checkout Code 16 | uses: actions/checkout@v2 17 | 18 | - name: update submodule 19 | run: git submodule update --init --depth=1 20 | 21 | - name: Install apt packages 22 | run: | 23 | sudo apt-get install libeigen3-dev libgtest-dev 24 | sudo apt-get install libgflags-dev libgoogle-glog-dev libprotobuf-dev protobuf-compiler 25 | 26 | - name: Build 27 | run: | 28 | export PKG_CONFIG_PATH="$PKG_CONFIG_PATH:/usr/lib/x86_64-linux-gnu/pkgconfig" 29 | mkdir build 30 | cd build 31 | cmake -DBUILD_TEST=ON .. 32 | make 33 | 34 | - name: Test c++ 35 | run: | 36 | cd build 37 | ctest --verbose 38 | 39 | - name: Build python module 40 | run: | 41 | export PKG_CONFIG_PATH="$PKG_CONFIG_PATH:/usr/lib/x86_64-linux-gnu/pkgconfig" 42 | pip3 install scikit-build 43 | pip3 install . 44 | - name: Run mypy check 45 | run: | 46 | pip3 install mypy 47 | mypy --version 48 | mypy . 49 | 50 | - name: Test python module 51 | run: | 52 | python3 example/ground_truth_cylinder.py 53 | python3 example/incremetal_update.py 54 | pip3 install gdown 55 | cd example/dataset && bash ./download_dataset.sh && cd ../.. 56 | python3 example/real_data.py 57 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /_skbuild 2 | /build 3 | /.cache 4 | compile_commands.json 5 | 6 | # other stuff 7 | *.new 8 | *.bak 9 | 10 | # Byte-compiled / optimized / DLL files 11 | __pycache__/ 12 | *.py[cod] 13 | *$py.class 14 | 15 | # C extensions 16 | *.so 17 | 18 | # Distribution / packaging 19 | .Python 20 | build/ 21 | develop-eggs/ 22 | dist/ 23 | downloads/ 24 | eggs/ 25 | .eggs/ 26 | lib/ 27 | lib64/ 28 | parts/ 29 | sdist/ 30 | var/ 31 | wheels/ 32 | share/python-wheels/ 33 | *.egg-info/ 34 | .installed.cfg 35 | *.egg 36 | MANIFEST 37 | 38 | # PyInstaller 39 | # Usually these files are written by a python script from a template 40 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 41 | *.manifest 42 | *.spec 43 | 44 | # Installer logs 45 | pip-log.txt 46 | pip-delete-this-directory.txt 47 | 48 | # Unit test / coverage reports 49 | htmlcov/ 50 | .tox/ 51 | .nox/ 52 | .coverage 53 | .coverage.* 54 | .cache 55 | nosetests.xml 56 | coverage.xml 57 | *.cover 58 | *.py,cover 59 | .hypothesis/ 60 | .pytest_cache/ 61 | cover/ 62 | 63 | # Translations 64 | *.mo 65 | *.pot 66 | 67 | # Django stuff: 68 | *.log 69 | local_settings.py 70 | db.sqlite3 71 | db.sqlite3-journal 72 | 73 | # Flask stuff: 74 | instance/ 75 | .webassets-cache 76 | 77 | # Scrapy stuff: 78 | .scrapy 79 | 80 | # Sphinx documentation 81 | docs/_build/ 82 | 83 | # PyBuilder 84 | .pybuilder/ 85 | target/ 86 | 87 | # Jupyter Notebook 88 | .ipynb_checkpoints 89 | 90 | # IPython 91 | profile_default/ 92 | ipython_config.py 93 | 94 | # pyenv 95 | # For a library or package, you might want to ignore these files since the code is 96 | # intended to run in multiple environments; otherwise, check them in: 97 | # .python-version 98 | 99 | # pipenv 100 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 101 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 102 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 103 | # install all needed dependencies. 104 | #Pipfile.lock 105 | 106 | # poetry 107 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 108 | # This is especially recommended for binary packages to ensure reproducibility, and is more 109 | # commonly ignored for libraries. 110 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 111 | #poetry.lock 112 | 113 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 114 | __pypackages__/ 115 | 116 | # Celery stuff 117 | celerybeat-schedule 118 | celerybeat.pid 119 | 120 | # SageMath parsed files 121 | *.sage.py 122 | 123 | # Environments 124 | .env 125 | .venv 126 | env/ 127 | venv/ 128 | ENV/ 129 | env.bak/ 130 | venv.bak/ 131 | 132 | # Spyder project settings 133 | .spyderproject 134 | .spyproject 135 | 136 | # Rope project settings 137 | .ropeproject 138 | 139 | # mkdocs documentation 140 | /site 141 | 142 | # mypy 143 | .mypy_cache/ 144 | .dmypy.json 145 | dmypy.json 146 | 147 | # Pyre type checker 148 | .pyre/ 149 | 150 | # pytype static type analyzer 151 | .pytype/ 152 | 153 | # Cython debug symbols 154 | cython_debug/ 155 | 156 | # PyCharm 157 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 158 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 159 | # and can be added to the global gitignore or merged into this file. For a more nuclear 160 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 161 | #.idea/ 162 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pybind11"] 2 | path = pybind11 3 | url = https://github.com/pybind/pybind11.git 4 | [submodule "voxblox/eigen_checks"] 5 | path = voxblox/eigen_checks 6 | url = https://github.com/HiroIshida/eigen_checks.git 7 | [submodule "voxblox/minkindr"] 8 | path = voxblox/minkindr 9 | url = https://github.com/HiroIshida/minkindr.git 10 | -------------------------------------------------------------------------------- /.isort.cfg: -------------------------------------------------------------------------------- 1 | [settings] 2 | profile = black 3 | -------------------------------------------------------------------------------- /.pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | line-length = 100 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10 FATAL_ERROR) 2 | set(PROJECT_NAME "voxbloxpy") 3 | 4 | if(NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif(NOT CMAKE_BUILD_TYPE) 7 | 8 | set(CMAKE_CXX_STANDARD 14) 9 | set(CMAKE_CXX_FLAGS " ${CMAKE_CXX_FLAGS_INIT} -Wall -fPIC") 10 | 11 | find_package(Eigen3 REQUIRED) 12 | ADD_DEFINITIONS(-DEIGEN_NO_DEBUG) 13 | include_directories(${EIGEN3_INCLUDE_DIR}) 14 | 15 | include_directories(voxblox/include/ voxblox/generated_proto/) 16 | include_directories(voxblox/minkindr/minkindr/include/) 17 | 18 | add_subdirectory(voxblox) 19 | add_subdirectory(pybind11) 20 | pybind11_add_module(_voxbloxpy wrapper.cpp) 21 | target_link_libraries(_voxbloxpy PRIVATE voxblox) 22 | 23 | set(CMD_SITE_PKG "import site; import sys;sys.stdout.write(site.getusersitepackages())") 24 | 25 | execute_process( 26 | COMMAND ${PYTHON_EXECUTABLE} -c "${CMD_SITE_PKG}" 27 | OUTPUT_VARIABLE PYTHON_SITE_PACKAGES_INSTALL_DIR 28 | ) 29 | 30 | set(PIP_INSTALL ON) 31 | if(PIP_INSTALL) 32 | install(TARGETS _voxbloxpy DESTINATION .) 33 | else() 34 | # install to the site pakcage 35 | install(TARGETS _voxbloxpy DESTINATION ${PYTHON_SITE_PACKAGES_INSTALL_DIR}) 36 | message("destination site-packages:" ${PYTHON_SITE_PACKAGES_INSTALL_DIR}) 37 | endif() 38 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Hirokazu Ishida 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # voxbloxpy [![CI](https://github.com/HiroIshida/voxbloxpy/actions/workflows/test.yaml/badge.svg)](https://github.com/HiroIshida/voxbloxpy/actions/workflows/test.yaml) [![pypi-version](https://img.shields.io/pypi/v/voxbloxpy.svg)](https://pypi.org/project/voxbloxpy/) 2 | 3 | [voxblox](https://github.com/ethz-asl/voxblox) is a ROS-based revolutionary project for online creation of signed distance field. This repository `voxbloxpy` provides the python-wrapper of the [voxblox](https://github.com/ethz-asl/voxblox) and some utils for volumetric rendering using `plotly`. This python package is **standalone**, that is, the package is ros-dependencies-free and can be installed from pypi. 4 | 5 | The wrapper's core source code can be found in [`wrapper.cpp`](wrapper.cpp) (lower-level) and [`python/voxbloxpy`](python/voxbloxpy) (higher-level) directory. 6 | 7 | The following animation is created using point cloud collected by a robot rotating in our lab. 8 | 9 | https://user-images.githubusercontent.com/38597814/199616755-02bcaed4-1170-4d88-87d2-995a67663d41.mp4 10 | 11 | This project is just a wrapper. So, please cite the paper (Oleynikova+, IROS 2017) for the original project when you use this in your research. 12 | ```latex 13 | @inproceedings{oleynikova2017voxblox, 14 | author={Oleynikova, Helen and Taylor, Zachary and Fehr, Marius and Siegwart, Roland and Nieto, Juan}, 15 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 16 | title={Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning}, 17 | year={2017} 18 | } 19 | ``` 20 | 21 | # Usage 22 | ```python 23 | from voxbloxpy import EsdfMap, CameraPose 24 | 25 | esdf = EsdfMap(0.02) # voxel size in meter 26 | camera_pose = CameraPose(pos, quat_wxyz) # set camera pose wrt world 27 | esdf.udpate(camera_pose, cloud_wrt_camera) # able to update as many as you want with different camera-cloud pair 28 | sd_values = esdf.get_sd_batch(pts_wrt_world) 29 | ``` 30 | 31 | # installation 32 | **NOTE: ROS not required** 33 | 34 | ## Install from pypi 35 | ```bash 36 | sudo apt-get install libgoogle-glog-dev 37 | pip3 install voxbloxpy 38 | ``` 39 | Note: Dependency on `libgoogle-glog-dev` is kind of a pain, though it can be resolved by apt install. I'm planning to remove this dependency by building glog from source and build a static library. 40 | 41 | ## Source build 42 | ```bash 43 | git clone https://github.com/HiroIshida/voxbloxpy.git 44 | cd voxbloxpy 45 | git submodule update --init 46 | sudo apt-get install libeigen3-dev libgtest-dev libgflags-dev libgoogle-glog-dev libprotobuf-dev protobuf-compiler 47 | pip3 install -e . 48 | ``` 49 | 50 | # Run demo (real dataset) 51 | download dataset (pickled pointcloud and camera poses) 52 | ```bash 53 | pip3 install gdown # if not installed yet 54 | cd example/dataset/ 55 | ./download_dataset.sh 56 | ``` 57 | The dataset is created by the scan when PR2 robot is directing toward the fridge with the opened door. 58 | 59 | 60 | Then run esdf creation demo 61 | ```bash 62 | python3 example/real_data.py --visualize 63 | ``` 64 | The bottom left figure shows the rviz-image at the scan time, and the bottom right figure shows the output of the visualization. 65 | 66 | 67 | 68 | # Run demo playing rosbag (you need to have ROS) 69 | ``` 70 | cd example/ros 71 | bash download_rosbag.sh 72 | roscore # run in different terminal 73 | rosbag play pr2_jsk_73b2_movearound.bag # run in different terminal 74 | python3 example_node.py 75 | ``` 76 | The sequence of figures and interactive html will be dumped in `/example/ros/figs`. 77 | See the mp4 video at the top of this README for the visualization of the result. 78 | -------------------------------------------------------------------------------- /action.sh: -------------------------------------------------------------------------------- 1 | cd build 2 | cmake .. -DBUILD_TEST=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DPYTHON_EXECUTABLE=$(which python3) 3 | if [ $? == 0 ]; then 4 | make -j16 5 | fi 6 | if [ $? == 0 ]; then 7 | make install 8 | fi 9 | -------------------------------------------------------------------------------- /example/dataset/.gitignore: -------------------------------------------------------------------------------- 1 | *.pkl 2 | -------------------------------------------------------------------------------- /example/dataset/collect.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import time 3 | 4 | import numpy as np 5 | import ros_numpy 6 | import rospy 7 | import tf 8 | from sensor_msgs.msg import PointCloud2 9 | 10 | from voxbloxpy import CameraPose 11 | 12 | rospy.init_node("pointcloud collector") 13 | 14 | 15 | class Collector: 16 | listener: tf.TransformListener 17 | finish: bool 18 | 19 | def __init__(self): 20 | self.listener = tf.TransformListener() 21 | rospy.Subscriber("/kinect_head/depth_registered/points", PointCloud2, self.callback) 22 | self.finish = False 23 | 24 | def callback(self, pcloud: PointCloud2): 25 | if self.finish: 26 | return 27 | target_frame = "base_link" 28 | source_frame = pcloud.header.frame_id 29 | 30 | self.listener.waitForTransform( 31 | target_frame, source_frame, rospy.Time(), rospy.Duration(4.0) 32 | ) 33 | pos, quat_xyzw = self.listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) 34 | quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) 35 | camera_pose = CameraPose(np.array(pos), quat_wxyz) 36 | 37 | pts = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pcloud) 38 | 39 | file_name = "pcloud_with_camera_pose-{}.pkl".format(time.strftime("%Y%m%d%H%M%S")) 40 | with open(file_name, "wb") as f: 41 | pickle.dump((pts, camera_pose), f) 42 | self.finish = True 43 | 44 | 45 | col = Collector() 46 | while not col.finish: 47 | time.sleep(2) 48 | -------------------------------------------------------------------------------- /example/dataset/download_dataset.sh: -------------------------------------------------------------------------------- 1 | gdown https://drive.google.com/uc?id=1jXyWaCdKfEABgzObY3MXjjQlB4_8OfE6 2 | tar xf real_data.tar 3 | rm real_data.tar 4 | -------------------------------------------------------------------------------- /example/ground_truth_cylinder.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import numpy as np 4 | 5 | from voxbloxpy import Grid, get_test_esdf 6 | 7 | if __name__ == "__main__": 8 | parser = argparse.ArgumentParser() 9 | parser.add_argument("--visualize", action="store_true", help="visualize") 10 | args = parser.parse_args() 11 | visualize: bool = args.visualize 12 | 13 | esdf = get_test_esdf(0.05, 6, 640, 480) 14 | print("finish creating esdf") 15 | 16 | N = 60 17 | lb = np.array([-3.0, -3.0, -2.0]) 18 | ub = np.array([3.0, 3.0, 4.0]) 19 | grid = Grid(lb, ub, (N, N, N)) 20 | gridsdf = esdf.get_grid_sdf(grid) 21 | 22 | if visualize: 23 | gridsdf.render_volume() 24 | -------------------------------------------------------------------------------- /example/incremetal_update.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import numpy as np 4 | 5 | from voxbloxpy import CameraPose, EsdfMap, Grid 6 | 7 | 8 | def create_esdf(sphere: bool = True, debug_view: bool = True): 9 | quat_wxyz = np.array([1.0, 0.0, 0.0, 0.0]) 10 | pos = np.array([-1.0, 0.6, 0.0]) 11 | camera_pose = CameraPose(pos, quat_wxyz) 12 | 13 | r_sphere = 0.5 14 | ylin = np.linspace(-1.0, 1.0, 400) 15 | zlin = np.linspace(-1.0, 1.0, 400) 16 | y_grid, z_grid = np.meshgrid(ylin, zlin) 17 | 18 | if sphere: 19 | pts_yz_plane = np.array(list(zip(y_grid.flatten(), z_grid.flatten()))) 20 | inside_circle = np.sum(pts_yz_plane**2, axis=1) < r_sphere - 0.02 21 | pts_yz_plane_inner = pts_yz_plane[inside_circle] 22 | pts_x = -np.sqrt(1.0 - np.sum(pts_yz_plane_inner**2, axis=1)) + 1.0 # 1.0 for camera 23 | 24 | pts = np.zeros((len(pts_x), 3)) 25 | pts[:, 0] = pts_x 26 | pts[:, 1:] = pts_yz_plane_inner 27 | else: 28 | pts_yz_plane = np.array(list(zip(y_grid.flatten(), z_grid.flatten()))) 29 | pts_x = np.zeros(len(pts_yz_plane)) 30 | pts = np.zeros((len(pts_x), 3)) 31 | pts[:, 0] = pts_x 32 | pts[:, 1:] = pts_yz_plane 33 | 34 | esdfmap = EsdfMap.create(0.05, 32) 35 | esdfmap.update(camera_pose, pts) 36 | 37 | return esdfmap 38 | 39 | 40 | if __name__ == "__main__": 41 | parser = argparse.ArgumentParser() 42 | parser.add_argument("--visualize", action="store_true", help="visualize") 43 | args = parser.parse_args() 44 | visualize: bool = args.visualize 45 | 46 | esdf = create_esdf(sphere=True, debug_view=False) 47 | 48 | if visualize: 49 | grid = Grid(np.array([-1.0, -1.0, -1.0]), np.array([1.0, 1.0, 0.5]), (60, 60, 60)) 50 | grid_sdf = esdf.get_grid_sdf(grid) 51 | grid_sdf.render_volume() 52 | -------------------------------------------------------------------------------- /example/real_data.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import pickle 3 | import time 4 | from pathlib import Path 5 | 6 | import numpy as np 7 | 8 | from voxbloxpy import EsdfMap, Grid 9 | 10 | if __name__ == "__main__": 11 | parser = argparse.ArgumentParser() 12 | parser.add_argument("--visualize", action="store_true", help="visualize") 13 | args = parser.parse_args() 14 | visualize: bool = args.visualize 15 | 16 | source_path = Path(__file__).resolve().parent.expanduser() 17 | dataset_path = source_path / "dataset" 18 | 19 | cloud_with_camera_list = [] 20 | for file_path in dataset_path.iterdir(): 21 | if file_path.name.endswith(".pkl"): 22 | print("load pkl: {}".format(file_path)) 23 | with file_path.open(mode="rb") as f: 24 | cloud_with_camera_list.append(pickle.load(f)) 25 | 26 | esdf = EsdfMap.create(0.02) 27 | for cloud_with_camera in cloud_with_camera_list: 28 | pts, camera_pose = cloud_with_camera 29 | ts = time.time() 30 | esdf.update(camera_pose, pts) 31 | print("elapsed time to update esdf {}".format(time.time() - ts)) 32 | 33 | grid = Grid(np.array([0.3, -1.0, 0.0]), np.array([2.0, 1.0, 2.0]), (100, 100, 100)) 34 | gridsdf = esdf.get_grid_sdf(grid) 35 | 36 | if visualize: 37 | gridsdf.render_volume(isomin=-0.1, isomax=0.2) 38 | gridsdf_comp = gridsdf.get_quantized() 39 | gridsdf_comp.render_volume(isomin=-0.1, isomax=0.2) 40 | -------------------------------------------------------------------------------- /example/ros/.gitignore: -------------------------------------------------------------------------------- 1 | /figs 2 | *.bag 3 | -------------------------------------------------------------------------------- /example/ros/download_rosbag.sh: -------------------------------------------------------------------------------- 1 | gdown https://drive.google.com/uc?id=13s3Kg3oHVV6htc7s9uIv9h7NVqgnQctg 2 | -------------------------------------------------------------------------------- /example/ros/example_node.py: -------------------------------------------------------------------------------- 1 | import time 2 | from pathlib import Path 3 | from typing import List 4 | 5 | import rospy 6 | 7 | try: 8 | import kaleido # noqa 9 | except ImportError: 10 | message = "kaleido is required for plotly figure export. >> pip3 install kaleido" 11 | raise ImportError(message) 12 | 13 | from voxbloxpy import EsdfMap, GridSDF 14 | from voxbloxpy.ros import EsdfNode, EsdfNodeConfig 15 | 16 | rospy.init_node("esdf_node") 17 | 18 | topic = "/kinect_head/depth_registered/points" 19 | world_frame = "/map" 20 | config = EsdfNodeConfig(point_cloud_topic=topic, world_frame=world_frame) 21 | 22 | grid_sdf_list: List[GridSDF] = [] 23 | 24 | 25 | def hook(esdf_map: EsdfMap): 26 | # this function might be too heavy. 27 | # I just do the following to make an animation. but do not do that 28 | # in the realtime application! 29 | ts = time.time() 30 | info = esdf_map.get_voxel_info() 31 | measure_grid = info.get_boundary_grid(grid_size=0.1) 32 | grid_sdf = esdf_map.get_grid_sdf(measure_grid, create_itp_lazy=True) 33 | grid_sdf_list.append(grid_sdf) 34 | te = time.time() 35 | rospy.loginfo("elapsed time for getting gridsdf {} sec".format(te - ts)) 36 | 37 | 38 | node = EsdfNode(config, hook=hook) 39 | 40 | time.sleep(20) # stop node after a while 41 | node.callback_running = False 42 | 43 | # save figures as png 44 | fig_path = Path(__file__).resolve().parent / "figs" 45 | fig_path.mkdir(exist_ok=True) 46 | for i, grid_sdf in enumerate(grid_sdf_list): 47 | fig = grid_sdf.render_volume(isomin=-0.2, isomax=2.0, show=False) 48 | name = fig_path / "sdfplot-{:03d}.png".format(i) 49 | rospy.loginfo("writing figure to {}".format(name)) 50 | fig.write_image(name) 51 | 52 | # save final figure as html 53 | file_name = fig_path / "final_sdf.html" 54 | fig = grid_sdf_list[-1].render_volume(isomin=-0.2, isomax=2.0, show=False) 55 | fig.write_html(file_name) 56 | -------------------------------------------------------------------------------- /example/ros/save_rosbag.sh: -------------------------------------------------------------------------------- 1 | rosbag record /kinect_head/depth_registered/points /tf -O pr2_jsk_73b2_movearound.bag 2 | -------------------------------------------------------------------------------- /format.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | find . -name "*.py" ! -path "*/pybind11/*" ! -path "*/voxblox/*" | xargs -i{} python3 -m autoflake -i --remove-all-unused-imports --remove-unused-variables --ignore-init-module-imports {} 3 | python3 -m isort ./python ./example 4 | python3 -m black ./python ./example --config .pyproject.toml 5 | python3 -m flake8 ./python ./example 6 | -------------------------------------------------------------------------------- /mypy.ini: -------------------------------------------------------------------------------- 1 | [mypy] 2 | python_version = 3.8 3 | exclude = setup.py|pybind11|voxblox 4 | show_error_codes = True 5 | warn_unused_ignores = False 6 | check_untyped_defs = True 7 | 8 | [mypy-cv2] 9 | ignore_missing_imports = True 10 | [mypy-tqdm] 11 | ignore_missing_imports = True 12 | [mypy-matplotlib] 13 | ignore_missing_imports = True 14 | [mypy-matplotlib.pyplot] 15 | ignore_missing_imports = True 16 | [mypy-torchvision.*] 17 | ignore_missing_imports = True 18 | [mypy-psutil] 19 | ignore_missing_imports = True 20 | [mypy-ros_numpy] 21 | ignore_missing_imports = True 22 | [mypy-rospy] 23 | ignore_missing_imports = True 24 | [mypy-tf] 25 | ignore_missing_imports = True 26 | [mypy-sensor_msgs.msg] 27 | ignore_missing_imports = True 28 | [mypy-kaleido] 29 | ignore_missing_imports = True 30 | [mypy-skrobot.*] 31 | ignore_missing_imports = True 32 | [mypy-pytest] 33 | ignore_missing_imports = True 34 | -------------------------------------------------------------------------------- /python/voxbloxpy/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | from voxbloxpy.core import ( 3 | CameraPose, 4 | EsdfMap, 5 | Grid, 6 | GridSDF, 7 | IntegratorType, 8 | VoxelInfos, 9 | get_test_esdf, 10 | ) 11 | -------------------------------------------------------------------------------- /python/voxbloxpy/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HiroIshida/voxbloxpy/f4e2070d003fc0bf72e3b4aa1eaee7c13a240530/python/voxbloxpy/py.typed -------------------------------------------------------------------------------- /python/voxbloxpy/ros.py: -------------------------------------------------------------------------------- 1 | import time 2 | from dataclasses import dataclass 3 | from typing import Callable, Optional 4 | 5 | import numpy as np 6 | import ros_numpy 7 | import rospy 8 | from sensor_msgs.msg import PointCloud2 9 | from tf import TransformListener 10 | 11 | from voxbloxpy import CameraPose, EsdfMap 12 | 13 | 14 | @dataclass 15 | class EsdfNodeConfig: 16 | point_cloud_topic: str 17 | world_frame: str = "map" 18 | voxel_size: float = 0.05 19 | voxel_per_side: int = 16 20 | 21 | 22 | class EsdfNode: 23 | esdf: EsdfMap 24 | listener: TransformListener 25 | hook: Callable[[EsdfMap], None] 26 | config: EsdfNodeConfig 27 | callback_running: bool 28 | 29 | def __init__(self, config: EsdfNodeConfig, hook: Optional[Callable[[EsdfMap], None]] = None): 30 | self.esdf = EsdfMap.create(config.voxel_size, config.voxel_per_side) 31 | self.listener = TransformListener() 32 | rospy.Subscriber(config.point_cloud_topic, PointCloud2, self.point_cloud_cb) 33 | if hook is None: 34 | 35 | def hook(esdf: EsdfMap): 36 | pass 37 | 38 | self.hook = hook 39 | self.config = config 40 | self.callback_running = True 41 | 42 | def point_cloud_cb(self, pcloud: PointCloud2) -> None: 43 | if not self.callback_running: 44 | return 45 | target_frame = self.config.world_frame 46 | source_frame = pcloud.header.frame_id 47 | self.listener.waitForTransform( 48 | target_frame, source_frame, rospy.Time(), rospy.Duration(4.0) 49 | ) 50 | pos, quat_xyzw = self.listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) 51 | quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) 52 | camera_pose = CameraPose(np.array(pos), quat_wxyz) 53 | 54 | pts = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pcloud) 55 | 56 | # update esdf_map 57 | ts = time.time() 58 | self.esdf.update(camera_pose, pts) 59 | elapsed = time.time() - ts 60 | message = "esdf map is updated. elapsd time is {} sec".format(elapsed) 61 | rospy.loginfo(message) 62 | 63 | self.hook(self.esdf) 64 | -------------------------------------------------------------------------------- /release/.gitignore: -------------------------------------------------------------------------------- 1 | .ssh/ 2 | -------------------------------------------------------------------------------- /release/Dockerfile: -------------------------------------------------------------------------------- 1 | # For better backword compatibility... 2 | FROM ubuntu:16.04 3 | ENV DEBIAN_FRONTEND=noninteractive 4 | 5 | RUN apt update 6 | # install pyenv deps 7 | RUN apt-get install -y make build-essential libssl-dev zlib1g-dev libbz2-dev libreadline-dev libsqlite3-dev wget curl llvm libncurses5-dev libncursesw5-dev xz-utils tk-dev libffi-dev liblzma-dev python-openssl git sudo 8 | 9 | RUN \ 10 | useradd user && \ 11 | echo "user ALL=(root) NOPASSWD:ALL" > /etc/sudoers.d/user && \ 12 | chmod 0440 /etc/sudoers.d/user && \ 13 | mkdir -p /home/user && \ 14 | chown user:user /home/user && \ 15 | chsh -s /bin/bash user 16 | 17 | WORKDIR /home/user 18 | USER user 19 | CMD /bin/bash 20 | 21 | ENV HOME /home/user 22 | ENV PYENV_ROOT $HOME/.pyenv 23 | ENV PATH $PYENV_ROOT/shims:$PYENV_ROOT/bin:$PATH 24 | RUN \ 25 | curl https://pyenv.run|bash && \ 26 | echo 'eval "$(pyenv init -)"' >> ~/.bashrc 27 | COPY python_versions.txt $HOME/ 28 | RUN cat python_versions.txt | while read version; do pyenv install $version ; done 29 | 30 | # update cmake to 3.20 31 | RUN sudo apt-get remove cmake 32 | RUN wget -nv https://cmake.org/files/v3.20/cmake-3.20.0-rc1-linux-$(uname -m).tar.gz 33 | RUN tar -xf cmake-3.20.0-rc1-linux-$(uname -m).tar.gz 34 | RUN sudo ln -sf ~/cmake-3.20.0-rc1-linux-$(uname -m)/bin/* /usr/bin 35 | 36 | # Prebuild 37 | RUN sudo apt-get install -y libeigen3-dev libgtest-dev libgflags-dev libgoogle-glog-dev libprotobuf-dev protobuf-compiler 38 | #COPY --chown=user .ssh /home/user/.ssh 39 | #RUN git clone git@github.com:HiroIshida/voxbloxpy.git 40 | RUN git clone https://github.com/HiroIshida/voxbloxpy.git 41 | RUN cat python_versions.txt | while read version; do pyenv global $version && pip install scikit-build; done 42 | RUN cd voxbloxpy && git submodule update --init 43 | 44 | # ad hoc src install gflags (this extra step required because build on legacy ubuntu 16.04) 45 | RUN \ 46 | git clone https://github.com/gflags/gflags.git && \ 47 | cd gflags && mkdir build && \ 48 | cd build && cmake .. && make -j2 && sudo make install 49 | 50 | RUN \ 51 | cd voxbloxpy && \ 52 | cat ../python_versions.txt | while read version; do pyenv global $version && python setup.py bdist_wheel -p manylinux2014_$(uname -m); done 53 | 54 | COPY check_glibcxx.sh $HOME/ 55 | RUN bash ./check_glibcxx.sh 56 | 57 | RUN pyenv global $(tail -n1 python_versions.txt) 58 | RUN pip install twine 59 | -------------------------------------------------------------------------------- /release/check_glibcxx.sh: -------------------------------------------------------------------------------- 1 | current=$(strings $(ldconfig -p |grep libstd|awk '{print $4}')|grep GLIBCXX|sort --version-sort | tail -2| head -1| sed "s/^.*GLIBCXX_\([0-9.]*\).*/\1/") 2 | maxaddmissible="3.4.21" # default in ubuntu 16.04 3 | echo "Current version is $current" 4 | echo "Maximum admissible version is $maxaddmissible" 5 | if [ "$(printf '%s\n' "$maxaddmissible" "$current" | sort -V --reverse| head -n1)" = "$maxaddmissible" ]; then 6 | echo "Less than or equal to ${maxaddmissible}" 7 | else 8 | echo "Error: Greater than ${maxaddmissible}. Requiring older version." 9 | exit 1 10 | fi 11 | -------------------------------------------------------------------------------- /release/python_versions.txt: -------------------------------------------------------------------------------- 1 | 3.6.15 2 | 3.7.11 3 | 3.8.11 4 | 3.9.10 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from skbuild import setup 2 | 3 | install_requires = [ 4 | "numpy", 5 | "plotly", 6 | "scipy" 7 | ] 8 | 9 | setup( 10 | name="voxbloxpy", 11 | version="0.0.3", 12 | description="standalone voxblox python", 13 | author="Hirokazu Ishida", 14 | license="MIT", 15 | packages=["voxbloxpy"], 16 | package_dir={"": "python"}, 17 | cmake_install_dir="python/voxbloxpy/", 18 | install_requires=install_requires, 19 | extra_requires = {"test": ["scikit-robot"]}, 20 | package_data={"voxbloxpy": ["py.typed"]}, 21 | ) 22 | -------------------------------------------------------------------------------- /tests/test_all.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | from voxbloxpy import EsdfMap, IntegratorType 4 | from utils import Camera, CameraConfig, RayMarchingConfig, SDF 5 | 6 | from skrobot.model import Sphere 7 | 8 | from typing import List, Tuple 9 | 10 | import numpy as np 11 | import tqdm 12 | from skrobot.model.primitives import Sphere 13 | 14 | 15 | class TestGroup: 16 | radius: float = 0.4 17 | resol: float = 0.02 18 | 19 | @staticmethod 20 | def generate_camera_pcloud_pairs( 21 | sdf: SDF, camera_config: CameraConfig, rm_config: RayMarchingConfig, nx: int = 20, nz: int = 3 22 | ) -> List[Tuple[Camera, np.ndarray]]: 23 | thetas = np.linspace(0, 2 * np.pi, nx) 24 | phis = np.linspace(-0.7 * np.pi, 0.7 * np.pi, nz) 25 | 26 | pairs = [] 27 | for theta in thetas: 28 | for phi in phis: 29 | pos = np.array([np.cos(theta) * np.cos(phi), np.sin(theta) * np.cos(phi), np.sin(phi)]) 30 | camera = Camera(pos, config=camera_config) 31 | camera.look_at(np.zeros(3), horizontal=True) 32 | pts_global = camera.generate_point_cloud(sdf, rm_config, hit_only=True) 33 | pairs.append((camera, pts_global)) 34 | return pairs 35 | 36 | @staticmethod 37 | def create_sphere_points(radius: float) -> np.ndarray: 38 | pts = [] 39 | for theta in np.linspace(0, 2 * np.pi, 100): 40 | for phi in np.linspace(-0.4 * np.pi, 0.4 * np.pi, 10): 41 | pos = ( 42 | np.array( 43 | [np.cos(theta) * np.cos(phi), np.sin(theta) * np.cos(phi), np.sin(phi)] 44 | ) 45 | * radius 46 | ) 47 | pts.append(pos) 48 | pts_sphere = np.array(pts) 49 | return pts_sphere 50 | 51 | @pytest.fixture() 52 | def esdf(self): 53 | sphere = Sphere(self.radius, with_sdf=True) 54 | pairs = self.generate_camera_pcloud_pairs( 55 | sphere.sdf, CameraConfig(resolx=128, resoly=96), RayMarchingConfig(max_dist=3.0), nz=5 56 | ) 57 | esdf = EsdfMap.create(self.resol, integrator_type=IntegratorType.SIMPLE) 58 | for camera, cloud_global in tqdm.tqdm(pairs): 59 | cloud_camera = camera.inverse_transform_vector(cloud_global) 60 | esdf.update(camera.get_voxbloxpy_camera_pose(), cloud_camera) 61 | yield esdf 62 | 63 | def test_generate_point_cloud(self): # test of utils (not main code) 64 | sphere = Sphere(0.2, pos=(0.2, 0.2, 0.2), with_sdf=True) 65 | pairs = self.generate_camera_pcloud_pairs( 66 | sphere.sdf, CameraConfig(resolx=64, resoly=48), RayMarchingConfig(max_dist=3.0) 67 | ) 68 | pts_concat = np.concatenate([p[1] for p in pairs]) 69 | 70 | sphere.sdf(pts_concat) 71 | 72 | def test_synthetic_esdf_value(self, esdf: EsdfMap): 73 | 74 | def check(dist_from_surface: float, admissible_rate: float): 75 | pts_sphere = self.create_sphere_points(self.radius + dist_from_surface) 76 | sd_values = esdf.get_sd_batch(pts_sphere) 77 | success_rate = np.sum(np.abs(sd_values - dist_from_surface) < self.resol * 2) / len(pts_sphere) 78 | assert success_rate > admissible_rate 79 | 80 | check(-0.04, 0.99) 81 | check(0.0, 0.99) 82 | check(0.05, 0.95) 83 | check(0.1, 0.9) 84 | check(0.2, 0.7) 85 | 86 | def test_quantization(self, esdf: EsdfMap): 87 | grid = esdf.get_voxel_info().get_boundary_grid(grid_size=0.01) 88 | grid_sdf = esdf.get_grid_sdf(grid) 89 | grid_sdf.get_quantized() 90 | -------------------------------------------------------------------------------- /tests/utils.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from dataclasses import dataclass 3 | from typing import Optional, Protocol 4 | 5 | import numpy as np 6 | from skrobot.coordinates.geo import orient_coords_to_axis 7 | from skrobot.coordinates.math import matrix2quaternion, rotation_matrix_from_axis 8 | from skrobot.model.primitives import Axis, Coordinates 9 | from voxbloxpy.core import CameraPose 10 | 11 | 12 | class SDF(Protocol): 13 | def __call__(self, pts: np.ndarray) -> np.ndarray: 14 | ... 15 | 16 | 17 | @dataclass 18 | class CameraConfig: 19 | resolx: int = 640 20 | resoly: int = 480 21 | fovx: float = np.deg2rad(57.0) 22 | fovy: float = np.deg2rad(43.0) 23 | 24 | 25 | @dataclass 26 | class RayMarchingConfig: 27 | surface_closeness_threashold: float = 1e-3 28 | max_iter: int = 30 29 | max_dist: float = 2.0 30 | terminate_active_rate_threshold: float = 0.005 31 | 32 | 33 | class Camera(Axis): 34 | config: CameraConfig 35 | 36 | def __init__( 37 | self, camera_pos: Optional[np.ndarray] = None, config: Optional[CameraConfig] = None 38 | ): 39 | if config is None: 40 | config = CameraConfig() 41 | super().__init__(axis_radius=0.02, axis_length=0.3, pos=camera_pos) 42 | self.config = config 43 | 44 | def look_at(self, p: np.ndarray, horizontal: bool = True) -> None: 45 | if np.all(p == self.worldpos()): 46 | return 47 | diff = p - self.worldpos() 48 | orient_coords_to_axis(self, diff, axis="x") 49 | if horizontal: 50 | co = Coordinates( 51 | pos=self.worldpos(), rot=rotation_matrix_from_axis(diff, [0, 0, 1], axes="xz") 52 | ) 53 | self.newcoords(co) 54 | 55 | def generate_point_cloud( 56 | self, 57 | sdf: SDF, 58 | rm_config: RayMarchingConfig, 59 | hit_only: bool = False, 60 | ) -> np.ndarray: 61 | """Generate a point cloud wrt global coordinate""" 62 | co_proj_plane = self.copy_worldcoords() 63 | co_proj_plane.translate(np.array([1.0, 0.0, 0.0])) 64 | 65 | half_width = np.tan(self.config.fovx * 0.5) 66 | half_height = np.tan(self.config.fovy * 0.5) 67 | 68 | wlin = np.linspace(-half_width, half_width, self.config.resolx) 69 | hlin = np.linspace(-half_height, half_height, self.config.resoly) 70 | W, H = np.meshgrid(wlin, hlin) 71 | 72 | # First, create a points w.r.t projection plane 73 | grid_wh_local = np.array(list(zip(W.flatten(), H.flatten()))) 74 | grid_local = np.hstack((np.zeros((len(grid_wh_local), 1)), grid_wh_local)) 75 | grid_global = co_proj_plane.transform_vector(grid_local) 76 | 77 | ray_start = self.worldpos() 78 | ray_starts = np.tile(np.expand_dims(ray_start, axis=0), (len(grid_global), 1)) 79 | ray_directions = grid_global - ray_start 80 | 81 | norms = np.linalg.norm(ray_directions, axis=1) 82 | ray_directions_unit = ray_directions / norms[:, None] 83 | 84 | dists = self.ray_marching(ray_starts, ray_directions_unit, sdf, rm_config) 85 | clouds = ray_starts + dists[:, None] * ray_directions_unit 86 | 87 | if hit_only: 88 | hit_indices = dists < rm_config.max_dist - 1e-3 89 | clouds = clouds[hit_indices] 90 | return clouds 91 | 92 | @staticmethod 93 | def ray_marching( 94 | pts_starts, direction_arr_unit, f_sdf, rm_config: RayMarchingConfig 95 | ) -> np.ndarray: 96 | pts_starts = copy.deepcopy(pts_starts) 97 | ray_tips = pts_starts 98 | n_point = len(pts_starts) 99 | active_flags = np.ones(n_point).astype(bool) 100 | frying_dists = np.zeros(n_point) 101 | for _ in range(rm_config.max_iter): 102 | active_indices = np.where(active_flags)[0] 103 | 104 | sd_vals_active = f_sdf(ray_tips[active_indices]) 105 | diff = direction_arr_unit[active_flags] * sd_vals_active[:, None] 106 | ray_tips[active_indices] += diff 107 | frying_dists[active_indices] += sd_vals_active 108 | 109 | is_close_enough = sd_vals_active < rm_config.surface_closeness_threashold 110 | active_flags[active_indices[is_close_enough]] = False 111 | 112 | is_far_enough = frying_dists[active_indices] > rm_config.max_dist 113 | active_flags[active_indices[is_far_enough]] = False 114 | frying_dists[active_indices[is_far_enough]] = rm_config.max_dist 115 | ray_tips[active_indices[is_far_enough]] = np.inf 116 | 117 | active_ratio = np.sum(active_flags) / n_point 118 | if active_ratio < rm_config.terminate_active_rate_threshold: 119 | break 120 | 121 | frying_dists[active_indices] = rm_config.max_dist # type: ignore 122 | modified_dists = np.minimum(frying_dists, rm_config.max_dist) 123 | return modified_dists 124 | 125 | def get_voxbloxpy_camera_pose(self) -> CameraPose: 126 | pos = self.worldpos() 127 | rot = self.worldrot() 128 | quat = matrix2quaternion(rot) 129 | return CameraPose(pos, quat) 130 | -------------------------------------------------------------------------------- /voxblox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10 FATAL_ERROR) 2 | project(voxblox) 3 | 4 | if(NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif(NOT CMAKE_BUILD_TYPE) 7 | 8 | set(CMAKE_CXX_STANDARD 14) 9 | set(CMAKE_CXX_FLAGS " ${CMAKE_CXX_FLAGS_INIT} -Wall -fPIC") 10 | 11 | find_package(Eigen3 REQUIRED) 12 | ADD_DEFINITIONS(-DEIGEN_NO_DEBUG) 13 | include_directories(${EIGEN3_INCLUDE_DIR}) 14 | 15 | find_package(PkgConfig) 16 | find_package(Protobuf REQUIRED) 17 | pkg_check_modules(LIBGLOG REQUIRED libglog) 18 | pkg_check_modules(LIBGFLAGS REQUIRED gflags) 19 | 20 | # TODO: cmake has protobuf_generate_cpp function actually 21 | execute_process(COMMAND bash -c "protoc proto/voxblox/* --cpp_out generated_proto" WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}) 22 | 23 | include_directories(include/) 24 | include_directories(generated_proto/) 25 | include_directories(minkindr/minkindr/include/) 26 | include_directories(eigen_checks/include/) 27 | 28 | add_library(voxblox STATIC 29 | src/alignment/icp.cc 30 | src/core/block.cc 31 | src/core/esdf_map.cc 32 | src/core/tsdf_map.cc 33 | src/integrator/esdf_integrator.cc 34 | src/integrator/esdf_occ_integrator.cc 35 | src/integrator/integrator_utils.cc 36 | src/integrator/intensity_integrator.cc 37 | src/integrator/tsdf_integrator.cc 38 | src/io/mesh_ply.cc 39 | src/io/sdf_ply.cc 40 | src/mesh/marching_cubes.cc 41 | src/simulation/objects.cc 42 | src/simulation/simulation_world.cc 43 | src/utils/camera_model.cc 44 | src/utils/evaluation_utils.cc 45 | src/utils/layer_utils.cc 46 | src/utils/neighbor_tools.cc 47 | src/utils/protobuf_utils.cc 48 | src/utils/timing.cc 49 | src/utils/voxel_utils.cc 50 | generated_proto/proto/voxblox/Block.pb.cc 51 | generated_proto/proto/voxblox/Layer.pb.cc 52 | ) 53 | target_link_libraries(voxblox PRIVATE glog) 54 | target_link_libraries(voxblox PRIVATE gflags) 55 | target_link_libraries(voxblox PRIVATE protobuf) 56 | 57 | option(BUILD_TEST "build google test" OFF) 58 | if(BUILD_TEST) 59 | find_package(GTest REQUIRED) 60 | enable_testing() 61 | 62 | function(setup_voxblox_test test_name test_src) 63 | add_executable(${test_name} ${test_src}) 64 | target_link_libraries(${test_name} voxblox ${GTEST_LIBRARIES} pthread) 65 | target_include_directories(${test_name} PUBLIC ${GTEST_INCLUDE_DIRS}) 66 | gtest_discover_tests(${test_name}) 67 | endfunction() 68 | 69 | setup_voxblox_test(test_approx_hash_array test/test_approx_hash_array.cc) 70 | setup_voxblox_test(test_bucket_queue test/test_bucket_queue.cc) 71 | setup_voxblox_test(test_clear_spheres test/test_clear_spheres.cc) 72 | setup_voxblox_test(test_layer test/test_layer.cc) 73 | setup_voxblox_test(test_layer_utils test/test_layer_utils.cc) 74 | #setup_voxblox_test(test_load_esdf test/test_load_esdf.cc) 75 | setup_voxblox_test(test_merge_integration test/test_merge_integration.cc) 76 | setup_voxblox_test(test_protobuf test/test_protobuf.cc) 77 | setup_voxblox_test(test_sdf_integration test/test_sdf_integrators.cc) 78 | setup_voxblox_test(test_tsdf_interpolator test/test_tsdf_interpolator.cc) 79 | setup_voxblox_test(test_tsdf_map test/test_tsdf_map.cc) 80 | endif(BUILD_TEST) 81 | -------------------------------------------------------------------------------- /voxblox/generated_proto/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | */ 3 | !.gitignore 4 | -------------------------------------------------------------------------------- /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 "proto/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/esdf_map.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_CORE_ESDF_MAP_H_ 2 | #define VOXBLOX_CORE_ESDF_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 | #include "voxblox/io/layer_io.h" 15 | 16 | namespace voxblox { 17 | /** 18 | * Map holding a Euclidean Signed Distance Field Layer. Contains functions for 19 | * interacting with the layer and getting gradient and distance information. 20 | */ 21 | class EsdfMap { 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | typedef std::shared_ptr Ptr; 26 | 27 | struct Config { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | FloatingPoint esdf_voxel_size = 0.2; 31 | size_t esdf_voxels_per_side = 16u; 32 | }; 33 | 34 | explicit EsdfMap(const Config& config) 35 | : esdf_layer_(new Layer(config.esdf_voxel_size, 36 | config.esdf_voxels_per_side)), 37 | interpolator_(esdf_layer_.get()) { 38 | block_size_ = config.esdf_voxel_size * config.esdf_voxels_per_side; 39 | } 40 | 41 | /// Creates a new EsdfMap based on a COPY of this layer. 42 | explicit EsdfMap(const Layer& layer) 43 | : EsdfMap(aligned_shared>(layer)) {} 44 | 45 | /// Creates a new EsdfMap that contains this layer. 46 | explicit EsdfMap(Layer::Ptr layer) 47 | : esdf_layer_(layer), interpolator_(CHECK_NOTNULL(esdf_layer_.get())) { 48 | block_size_ = layer->block_size(); 49 | } 50 | 51 | virtual ~EsdfMap() {} 52 | 53 | Layer* getEsdfLayerPtr() { return esdf_layer_.get(); } 54 | const Layer* getEsdfLayerConstPtr() const { 55 | return esdf_layer_.get(); 56 | } 57 | 58 | const Layer& getEsdfLayer() const { return *esdf_layer_; } 59 | 60 | FloatingPoint block_size() const { return block_size_; } 61 | FloatingPoint voxel_size() const { return esdf_layer_->voxel_size(); } 62 | 63 | /** 64 | * Specific accessor functions for esdf maps. 65 | * Returns true if the point exists in the map AND is observed. 66 | * These accessors use Vector3d and doubles explicitly rather than 67 | * FloatingPoint to have a standard, cast-free interface to planning 68 | * functions. 69 | */ 70 | bool getDistanceAtPosition(const Eigen::Vector3d& position, 71 | double* distance) const; 72 | bool getDistanceAtPosition(const Eigen::Vector3d& position, bool interpolate, 73 | double* distance) const; 74 | 75 | bool getDistanceAndGradientAtPosition(const Eigen::Vector3d& position, 76 | double* distance, 77 | Eigen::Vector3d* gradient) const; 78 | bool getDistanceAndGradientAtPosition(const Eigen::Vector3d& position, 79 | bool interpolate, double* distance, 80 | Eigen::Vector3d* gradient) const; 81 | 82 | bool isObserved(const Eigen::Vector3d& position) const; 83 | 84 | // NOTE(mereweth@jpl.nasa.gov) 85 | // EigenDRef is fully dynamic stride type alias for Numpy array slices 86 | // Use column-major matrices; column-by-column traversal is faster 87 | // Convenience alias borrowed from pybind11 88 | using EigenDStride = Eigen::Stride; 89 | template 90 | using EigenDRef = Eigen::Ref; 91 | 92 | // Convenience functions for querying many points at once from Python 93 | void batchGetDistanceAtPosition( 94 | EigenDRef>& positions, 95 | Eigen::Ref distances, 96 | Eigen::Ref observed) const; 97 | 98 | void batchGetDistanceAndGradientAtPosition( 99 | EigenDRef>& positions, 100 | Eigen::Ref distances, 101 | EigenDRef>& gradients, 102 | Eigen::Ref observed) const; 103 | 104 | void batchIsObserved( 105 | EigenDRef>& positions, 106 | Eigen::Ref observed) const; 107 | 108 | unsigned int coordPlaneSliceGetCount(unsigned int free_plane_index, 109 | double free_plane_val) const; 110 | 111 | /** 112 | * Extract all voxels on a slice plane that is parallel to one of the 113 | * axis-aligned planes. free_plane_index specifies the free coordinate 114 | * (zero-based; x, y, z order) free_plane_val specifies the plane intercept 115 | * coordinate along that axis 116 | */ 117 | unsigned int coordPlaneSliceGetDistance( 118 | unsigned int free_plane_index, double free_plane_val, 119 | EigenDRef>& positions, 120 | Eigen::Ref distances, unsigned int max_points) const; 121 | 122 | protected: 123 | FloatingPoint block_size_; 124 | 125 | // The layers. 126 | Layer::Ptr esdf_layer_; 127 | 128 | // Interpolator for the layer. 129 | Interpolator interpolator_; 130 | }; 131 | 132 | } // namespace voxblox 133 | 134 | #endif // VOXBLOX_CORE_ESDF_MAP_H_ 135 | -------------------------------------------------------------------------------- /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_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_INTEGRATOR_ESDF_INTEGRATOR_H_ 2 | #define VOXBLOX_INTEGRATOR_ESDF_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/neighbor_tools.h" 17 | #include "voxblox/utils/timing.h" 18 | 19 | namespace voxblox { 20 | 21 | /** 22 | * Builds an ESDF layer out of a given TSDF layer. For a description of this 23 | * algorithm, please see: https://arxiv.org/abs/1611.03631 24 | */ 25 | class EsdfIntegrator { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | struct Config { 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 31 | 32 | /** 33 | * Whether to use full euclidean distance (true) or quasi-euclidean (false). 34 | * Full euclidean is slightly more accurate (up to 8% in the worst case) but 35 | * slower. 36 | */ 37 | bool full_euclidean_distance = false; 38 | /** 39 | * Maximum distance to calculate the actual distance to. 40 | * Any values above this will be set to default_distance_m. 41 | */ 42 | FloatingPoint max_distance_m = 2.0; 43 | /** 44 | * Should mirror (or be smaller than) truncation distance in tsdf 45 | * integrator. 46 | */ 47 | FloatingPoint min_distance_m = 0.2; 48 | /// Default distance set for unknown values and values > max_distance_m. 49 | FloatingPoint default_distance_m = 2.0; 50 | /** 51 | * For cheaper but less accurate map updates: the minimum difference in 52 | * a voxel distance, before the change is propagated. 53 | */ 54 | FloatingPoint min_diff_m = 0.001; 55 | /// Minimum weight to consider a TSDF value seen at. 56 | float min_weight = 1e-6; 57 | /// Number of buckets for the bucketed priority queue. 58 | int num_buckets = 20; 59 | /** 60 | * Whether to push stuff to the open queue multiple times, with updated 61 | * distances. 62 | */ 63 | bool multi_queue = false; 64 | /** 65 | * Whether to add an outside layer of occupied voxels. Basically just sets 66 | * all unknown voxels in the allocated blocks to occupied. 67 | * Try to only use this for batch processing, otherwise look into 68 | * addNewRobotPosition below, which uses clear spheres. 69 | */ 70 | bool add_occupied_crust = false; 71 | 72 | /** 73 | * For marking unknown space around a robot as free or occupied, these are 74 | * the radiuses used around each robot position. 75 | */ 76 | FloatingPoint clear_sphere_radius = 1.5; 77 | FloatingPoint occupied_sphere_radius = 5.0; 78 | }; 79 | 80 | EsdfIntegrator(const Config& config, Layer* tsdf_layer, 81 | Layer* esdf_layer); 82 | 83 | /** 84 | *Used for planning - allocates sphere around as observed but occupied, 85 | * and clears space in a smaller sphere around current position. 86 | * Points added this way are marked as "hallucinated," and can subsequently 87 | * be cleared based on this. 88 | */ 89 | void addNewRobotPosition(const Point& position); 90 | 91 | /** 92 | *Update from a TSDF layer in batch, clearing the current ESDF layer in the 93 | * process. 94 | */ 95 | void updateFromTsdfLayerBatch(); 96 | /** 97 | * Incrementally update from the TSDF layer, optionally clearing the updated 98 | * flag of all changed TSDF voxels. 99 | */ 100 | void updateFromTsdfLayer(bool clear_updated_flag); 101 | 102 | /** 103 | * Short-cut for pushing neighbors (i.e., incremental update) by default. 104 | * Not necessary in batch. 105 | */ 106 | void updateFromTsdfBlocks(const BlockIndexList& tsdf_blocks, 107 | bool incremental = false); 108 | 109 | /** 110 | * For incremental updates, the raise set contains all fixed voxels whose 111 | * distances have INCREASED since last iteration. This means that all voxels 112 | * that have these voxels as parents need to be invalidated and assigned 113 | * new values. 114 | * The raise set is always empty in batch operations. 115 | */ 116 | void processRaiseSet(); 117 | 118 | /** 119 | * The core of ESDF updates: processes a queue of voxels to get the minimum 120 | * possible distance value in each voxel, by checking the values of its 121 | * neighbors and distance to neighbors. The update is done once the open 122 | * set is empty. 123 | */ 124 | void processOpenSet(); 125 | 126 | /** 127 | * For new voxels, etc. -- update its value from its neighbors. Sort of the 128 | * inverse of what the open set does (pushes the value of voxels *TO* its 129 | * neighbors). Used mostly for adding new voxels in. 130 | */ 131 | bool updateVoxelFromNeighbors(const GlobalIndex& global_index); 132 | 133 | // Convenience functions. 134 | inline bool isFixed(FloatingPoint dist_m) const { 135 | return std::abs(dist_m) < config_.min_distance_m; 136 | } 137 | /// Clears the state of the integrator, in case robot pose clearance is used. 138 | void clear() { 139 | updated_blocks_.clear(); 140 | open_.clear(); 141 | raise_ = AlignedQueue(); 142 | } 143 | /// Update some specific settings. 144 | float getEsdfMaxDistance() const { return config_.max_distance_m; } 145 | void setEsdfMaxDistance(float max_distance) { 146 | config_.max_distance_m = max_distance; 147 | if (config_.default_distance_m < max_distance) { 148 | config_.default_distance_m = max_distance; 149 | } 150 | } 151 | bool getFullEuclidean() const { return config_.full_euclidean_distance; } 152 | void setFullEuclidean(bool full_euclidean) { 153 | config_.full_euclidean_distance = full_euclidean; 154 | } 155 | 156 | protected: 157 | Config config_; 158 | 159 | Layer* tsdf_layer_; 160 | Layer* esdf_layer_; 161 | 162 | /** 163 | * Open Queue for incremental updates. Contains global voxel indices 164 | * for the ESDF layer. 165 | */ 166 | BucketQueue open_; 167 | 168 | /** 169 | * Raise set for updates; these are values that used to be in the fixed 170 | * frontier and now have a higher value, or their children which need to 171 | * have their values invalidated. 172 | */ 173 | AlignedQueue raise_; 174 | 175 | size_t voxels_per_side_; 176 | FloatingPoint voxel_size_; 177 | 178 | IndexSet updated_blocks_; 179 | }; 180 | 181 | } // namespace voxblox 182 | 183 | #endif // VOXBLOX_INTEGRATOR_ESDF_INTEGRATOR_H_ 184 | -------------------------------------------------------------------------------- /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/integrator_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_INTEGRATOR_INTEGRATOR_UTILS_H_ 2 | #define VOXBLOX_INTEGRATOR_INTEGRATOR_UTILS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "voxblox/core/block_hash.h" 13 | #include "voxblox/core/common.h" 14 | #include "voxblox/utils/timing.h" 15 | 16 | namespace voxblox { 17 | 18 | /** 19 | * Small class that can be used by multiple threads that need mutually exclusive 20 | * indexes to the same array, while still covering all elements. 21 | */ 22 | class ThreadSafeIndex { 23 | public: 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | 26 | // NOTE: The ThreadSafeIndex base destructor must be marked virtual. 27 | // Otherwise the destructors of derived classes don't get called when 28 | // derived class instances are destructed through base class pointers. 29 | // This would result leaking memory due to derived class member 30 | // variables not being freed. 31 | virtual ~ThreadSafeIndex() = default; 32 | 33 | /// returns true if index is valid, false otherwise 34 | bool getNextIndex(size_t* idx); 35 | 36 | void reset(); 37 | 38 | protected: 39 | explicit ThreadSafeIndex(size_t number_of_points); 40 | 41 | virtual size_t getNextIndexImpl(size_t base_idx) = 0; 42 | 43 | std::atomic atomic_idx_; 44 | const size_t number_of_points_; 45 | }; 46 | 47 | /* 48 | * The class attempts to ensure that the points are read in an order that gives 49 | * good coverage over the pointcloud very quickly. This is so that the 50 | * integrator can be terminated before all points have been read (due to time 51 | * constraints) and still capture most of the geometry. 52 | */ 53 | class MixedThreadSafeIndex : public ThreadSafeIndex { 54 | public: 55 | explicit MixedThreadSafeIndex(size_t number_of_points); 56 | 57 | protected: 58 | virtual size_t getNextIndexImpl(size_t sequential_idx); 59 | 60 | private: 61 | const size_t number_of_groups_; 62 | 63 | /// 1024 bins 64 | static constexpr size_t num_bits = 10; 65 | static constexpr size_t step_size_ = 1 << num_bits; 66 | static constexpr size_t bit_mask_ = step_size_ - 1; 67 | }; 68 | 69 | /* 70 | * This class will sort the indices such that the nearest points are integrated 71 | * first. This has two favorable effects. First, in case the integration reaches 72 | * the time limit, we are more likely to have integratedg geometry in the robots 73 | * immediate vacinity, which is more relevant for planning/collision avoidance. 74 | * The other reason is that the FastTsdfIntegrator will behave nicer and is less 75 | * likely to destroy small features in the geometry. 76 | */ 77 | class SortedThreadSafeIndex : public ThreadSafeIndex { 78 | public: 79 | explicit SortedThreadSafeIndex(const Pointcloud& points_C); 80 | 81 | protected: 82 | virtual size_t getNextIndexImpl(size_t sequential_idx); 83 | 84 | private: 85 | std::vector> indices_and_squared_norms_; 86 | }; 87 | 88 | class ThreadSafeIndexFactory { 89 | public: 90 | static ThreadSafeIndex* get(const std::string& mode, 91 | const Pointcloud& points_C); 92 | }; 93 | 94 | /** 95 | * Generates the indexes of all voxels a given ray will pass through. This class 96 | * assumes PRE-SCALED coordinates, where one unit = one voxel size. The indices 97 | * are also returned in this scales coordinate system, which should map to voxel 98 | * indexes. 99 | */ 100 | class RayCaster { 101 | public: 102 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 103 | 104 | RayCaster(const Point& origin, const Point& point_G, 105 | const bool is_clearing_ray, const bool voxel_carving_enabled, 106 | const FloatingPoint max_ray_length_m, 107 | const FloatingPoint voxel_size_inv, 108 | const FloatingPoint truncation_distance, 109 | const bool cast_from_origin = true); 110 | 111 | RayCaster(const Point& start_scaled, const Point& end_scaled); 112 | 113 | /// returns false if ray terminates at ray_index, true otherwise 114 | bool nextRayIndex(GlobalIndex* ray_index); 115 | 116 | private: 117 | void setupRayCaster(const Point& start_scaled, const Point& end_scaled); 118 | 119 | Ray t_to_next_boundary_; 120 | GlobalIndex curr_index_; 121 | AnyIndex ray_step_signs_; 122 | Ray t_step_size_; 123 | 124 | uint ray_length_in_steps_; 125 | uint current_step_; 126 | }; 127 | 128 | /** 129 | * This function assumes PRE-SCALED coordinates, where one unit = one voxel 130 | * size. The indices are also returned in this scales coordinate system, which 131 | * should map to voxel indices. 132 | */ 133 | inline void castRay(const Point& start_scaled, const Point& end_scaled, 134 | AlignedVector* indices) { 135 | CHECK_NOTNULL(indices); 136 | 137 | RayCaster ray_caster(start_scaled, end_scaled); 138 | 139 | GlobalIndex ray_index; 140 | while (ray_caster.nextRayIndex(&ray_index)) { 141 | indices->push_back(ray_index); 142 | } 143 | } 144 | 145 | /** 146 | * Takes start and end in WORLD COORDINATES, does all pre-scaling and 147 | * sorting into hierarhical index. 148 | */ 149 | inline void getHierarchicalIndexAlongRay( 150 | const Point& start, const Point& end, size_t voxels_per_side, 151 | FloatingPoint voxel_size, FloatingPoint truncation_distance, 152 | bool voxel_carving_enabled, HierarchicalIndexMap* hierarchical_idx_map) { 153 | hierarchical_idx_map->clear(); 154 | 155 | FloatingPoint voxels_per_side_inv = 1.0 / voxels_per_side; 156 | FloatingPoint voxel_size_inv = 1.0 / voxel_size; 157 | 158 | const Ray unit_ray = (end - start).normalized(); 159 | 160 | const Point ray_end = end + unit_ray * truncation_distance; 161 | const Point ray_start = 162 | voxel_carving_enabled ? start : (end - unit_ray * truncation_distance); 163 | 164 | const Point start_scaled = ray_start * voxel_size_inv; 165 | const Point end_scaled = ray_end * voxel_size_inv; 166 | 167 | AlignedVector global_voxel_index; 168 | timing::Timer cast_ray_timer("integrate/cast_ray"); 169 | castRay(start_scaled, end_scaled, &global_voxel_index); 170 | cast_ray_timer.Stop(); 171 | 172 | timing::Timer create_index_timer("integrate/create_hi_index"); 173 | for (const GlobalIndex& global_voxel_idx : global_voxel_index) { 174 | BlockIndex block_idx = getBlockIndexFromGlobalVoxelIndex( 175 | global_voxel_idx, voxels_per_side_inv); 176 | VoxelIndex local_voxel_idx = 177 | getLocalFromGlobalVoxelIndex(global_voxel_idx, voxels_per_side); 178 | 179 | if (local_voxel_idx.x() < 0) { 180 | local_voxel_idx.x() += voxels_per_side; 181 | } 182 | if (local_voxel_idx.y() < 0) { 183 | local_voxel_idx.y() += voxels_per_side; 184 | } 185 | if (local_voxel_idx.z() < 0) { 186 | local_voxel_idx.z() += voxels_per_side; 187 | } 188 | 189 | (*hierarchical_idx_map)[block_idx].push_back(local_voxel_idx); 190 | } 191 | create_index_timer.Stop(); 192 | } 193 | 194 | } // namespace voxblox 195 | 196 | #endif // VOXBLOX_INTEGRATOR_INTEGRATOR_UTILS_H_ 197 | -------------------------------------------------------------------------------- /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/mesh/marching_cubes.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef VOXBLOX_MESH_MARCHING_CUBES_H_ 24 | #define VOXBLOX_MESH_MARCHING_CUBES_H_ 25 | 26 | #include "voxblox/mesh/mesh.h" 27 | 28 | namespace voxblox { 29 | 30 | /** 31 | * Performs the marching cubes algorithm to generate a mesh layer from a TSDF. 32 | * Implementation taken from Open Chisel 33 | * https://github.com/personalrobotics/OpenChisel 34 | */ 35 | class MarchingCubes { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | 39 | static const int kTriangleTable[256][16]; 40 | static const int kEdgeIndexPairs[12][2]; 41 | 42 | MarchingCubes() {} 43 | virtual ~MarchingCubes() {} 44 | 45 | static void meshCube( 46 | const Eigen::Matrix& vertex_coordinates, 47 | const Eigen::Matrix& vertex_sdf, 48 | TriangleVector* triangles) { 49 | DCHECK(triangles != NULL); 50 | 51 | const int index = calculateVertexConfiguration(vertex_sdf); 52 | 53 | Eigen::Matrix edge_coords; 54 | interpolateEdgeVertices(vertex_coordinates, vertex_sdf, &edge_coords); 55 | 56 | const int* table_row = kTriangleTable[index]; 57 | 58 | int edge_index = 0; 59 | int table_col = 0; 60 | while ((edge_index = table_row[table_col]) != -1) { 61 | Triangle triangle; 62 | triangle.col(0) = edge_coords.col(edge_index); 63 | edge_index = table_row[table_col + 1]; 64 | triangle.col(1) = edge_coords.col(edge_index); 65 | edge_index = table_row[table_col + 2]; 66 | triangle.col(2) = edge_coords.col(edge_index); 67 | triangles->push_back(triangle); 68 | table_col += 3; 69 | } 70 | } 71 | 72 | static void meshCube(const Eigen::Matrix& vertex_coords, 73 | const Eigen::Matrix& vertex_sdf, 74 | VertexIndex* next_index, Mesh* mesh) { 75 | DCHECK(next_index != NULL); 76 | DCHECK(mesh != NULL); 77 | const int index = calculateVertexConfiguration(vertex_sdf); 78 | 79 | // No edges in this cube. 80 | if (index == 0) { 81 | return; 82 | } 83 | 84 | Eigen::Matrix edge_vertex_coordinates; 85 | interpolateEdgeVertices(vertex_coords, vertex_sdf, 86 | &edge_vertex_coordinates); 87 | 88 | const int* table_row = kTriangleTable[index]; 89 | 90 | int table_col = 0; 91 | while (table_row[table_col] != -1) { 92 | mesh->vertices.emplace_back( 93 | edge_vertex_coordinates.col(table_row[table_col + 2])); 94 | mesh->vertices.emplace_back( 95 | edge_vertex_coordinates.col(table_row[table_col + 1])); 96 | mesh->vertices.emplace_back( 97 | edge_vertex_coordinates.col(table_row[table_col])); 98 | mesh->indices.push_back(*next_index); 99 | mesh->indices.push_back((*next_index) + 1); 100 | mesh->indices.push_back((*next_index) + 2); 101 | const Point& p0 = mesh->vertices[*next_index]; 102 | const Point& p1 = mesh->vertices[*next_index + 1]; 103 | const Point& p2 = mesh->vertices[*next_index + 2]; 104 | Point px = (p1 - p0); 105 | Point py = (p2 - p0); 106 | Point n = px.cross(py).normalized(); 107 | mesh->normals.push_back(n); 108 | mesh->normals.push_back(n); 109 | mesh->normals.push_back(n); 110 | *next_index += 3; 111 | table_col += 3; 112 | } 113 | } 114 | 115 | static int calculateVertexConfiguration( 116 | const Eigen::Matrix& vertex_sdf) { 117 | return (vertex_sdf(0) < 0 ? (1 << 0) : 0) | 118 | (vertex_sdf(1) < 0 ? (1 << 1) : 0) | 119 | (vertex_sdf(2) < 0 ? (1 << 2) : 0) | 120 | (vertex_sdf(3) < 0 ? (1 << 3) : 0) | 121 | (vertex_sdf(4) < 0 ? (1 << 4) : 0) | 122 | (vertex_sdf(5) < 0 ? (1 << 5) : 0) | 123 | (vertex_sdf(6) < 0 ? (1 << 6) : 0) | 124 | (vertex_sdf(7) < 0 ? (1 << 7) : 0); 125 | } 126 | 127 | static void interpolateEdgeVertices( 128 | const Eigen::Matrix& vertex_coords, 129 | const Eigen::Matrix& vertex_sdf, 130 | Eigen::Matrix* edge_coords) { 131 | DCHECK(edge_coords != NULL); 132 | for (std::size_t i = 0; i < 12; ++i) { 133 | const int* pairs = kEdgeIndexPairs[i]; 134 | const int edge0 = pairs[0]; 135 | const int edge1 = pairs[1]; 136 | // Only interpolate along edges where there is a zero crossing. 137 | if ((vertex_sdf(edge0) < 0 && vertex_sdf(edge1) >= 0) || 138 | (vertex_sdf(edge0) >= 0 && vertex_sdf(edge1) < 0)) 139 | edge_coords->col(i) = interpolateVertex( 140 | vertex_coords.col(edge0), vertex_coords.col(edge1), 141 | vertex_sdf(edge0), vertex_sdf(edge1)); 142 | } 143 | } 144 | 145 | /** 146 | * Performs linear interpolation on two cube corners to find the approximate 147 | * zero crossing (surface) value. 148 | */ 149 | static inline Point interpolateVertex(const Point& vertex1, 150 | const Point& vertex2, float sdf1, 151 | float sdf2) { 152 | static constexpr FloatingPoint kMinSdfDifference = 1e-6; 153 | const FloatingPoint sdf_diff = sdf1 - sdf2; 154 | // Only compute the actual interpolation value if the sdf_difference is not 155 | // too small, this is to counteract issues with floating point precision. 156 | if (std::abs(sdf_diff) >= kMinSdfDifference) { 157 | const FloatingPoint t = sdf1 / sdf_diff; 158 | return Point(vertex1 + t * (vertex2 - vertex1)); 159 | } else { 160 | return Point(0.5 * (vertex1 + vertex2)); 161 | } 162 | } 163 | }; 164 | 165 | } // namespace voxblox 166 | 167 | #endif // VOXBLOX_MESH_MARCHING_CUBES_H_ 168 | -------------------------------------------------------------------------------- /voxblox/include/voxblox/mesh/mesh.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef VOXBLOX_MESH_MESH_H_ 24 | #define VOXBLOX_MESH_MESH_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include "voxblox/core/common.h" 30 | 31 | namespace voxblox { 32 | 33 | /** 34 | * Holds the vertex, normals, color and triangle index information of a mesh. 35 | */ 36 | struct Mesh { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | typedef std::shared_ptr Ptr; 41 | typedef std::shared_ptr ConstPtr; 42 | 43 | static constexpr FloatingPoint kInvalidBlockSize = -1.0; 44 | 45 | Mesh() 46 | : block_size(kInvalidBlockSize), origin(Point::Zero()), updated(false) { 47 | // Do nothing. 48 | } 49 | 50 | Mesh(FloatingPoint _block_size, const Point& _origin) 51 | : block_size(_block_size), origin(_origin), updated(false) { 52 | CHECK_GT(block_size, 0.0); 53 | } 54 | virtual ~Mesh() {} 55 | 56 | inline bool hasVertices() const { return !vertices.empty(); } 57 | inline bool hasNormals() const { return !normals.empty(); } 58 | inline bool hasColors() const { return !colors.empty(); } 59 | inline bool hasTriangles() const { return !indices.empty(); } 60 | 61 | inline size_t size() const { return vertices.size(); } 62 | inline size_t getMemorySize() const { 63 | size_t size_bytes = 0u; 64 | size_bytes += sizeof(Pointcloud) + vertices.size() * sizeof(Point); 65 | size_bytes += sizeof(Pointcloud) + normals.size() * sizeof(Point); 66 | size_bytes += sizeof(Colors) + vertices.size() * sizeof(Color); 67 | size_bytes += 68 | sizeof(VertexIndexList) + indices.size() * sizeof(VertexIndex); 69 | 70 | size_bytes += sizeof(block_size); 71 | size_bytes += sizeof(origin); 72 | size_bytes += sizeof(updated); 73 | return size_bytes; 74 | } 75 | 76 | inline void clear() { 77 | vertices.clear(); 78 | normals.clear(); 79 | colors.clear(); 80 | indices.clear(); 81 | } 82 | 83 | inline void clearTriangles() { indices.clear(); } 84 | inline void clearNormals() { normals.clear(); } 85 | inline void clearColors() { colors.clear(); } 86 | 87 | inline void resize(const size_t size, const bool has_normals = true, 88 | const bool has_colors = true, 89 | const bool has_indices = true) { 90 | vertices.resize(size); 91 | 92 | if (has_normals) { 93 | normals.resize(size); 94 | } 95 | 96 | if (has_colors) { 97 | colors.resize(size); 98 | } 99 | 100 | if (has_indices) { 101 | indices.resize(size); 102 | } 103 | } 104 | 105 | inline void reserve(const size_t size, const bool has_normals = true, 106 | const bool has_colors = true, 107 | const bool has_triangles = true) { 108 | vertices.reserve(size); 109 | 110 | if (has_normals) { 111 | normals.reserve(size); 112 | } 113 | 114 | if (has_colors) { 115 | colors.reserve(size); 116 | } 117 | 118 | if (has_triangles) { 119 | indices.reserve(size); 120 | } 121 | } 122 | 123 | void colorizeMesh(const Color& new_color) { 124 | colors.clear(); 125 | colors.resize(vertices.size(), new_color); 126 | } 127 | 128 | void concatenateMesh(const Mesh& other_mesh) { 129 | CHECK_EQ(other_mesh.hasColors(), hasColors()); 130 | CHECK_EQ(other_mesh.hasNormals(), hasNormals()); 131 | CHECK_EQ(other_mesh.hasTriangles(), hasTriangles()); 132 | 133 | reserve(size() + other_mesh.size(), hasNormals(), hasColors(), 134 | hasTriangles()); 135 | 136 | const size_t num_vertices_before = vertices.size(); 137 | 138 | for (const Point& vertex : other_mesh.vertices) { 139 | vertices.push_back(vertex); 140 | } 141 | for (const Color& color : other_mesh.colors) { 142 | colors.push_back(color); 143 | } 144 | for (const Point& normal : other_mesh.normals) { 145 | normals.push_back(normal); 146 | } 147 | for (const size_t index : other_mesh.indices) { 148 | indices.push_back(index + num_vertices_before); 149 | } 150 | } 151 | 152 | Pointcloud vertices; 153 | VertexIndexList indices; 154 | Pointcloud normals; 155 | Colors colors; 156 | 157 | FloatingPoint block_size; 158 | Point origin; 159 | 160 | bool updated; 161 | }; 162 | 163 | } // namespace voxblox 164 | 165 | #endif // VOXBLOX_MESH_MESH_H_ 166 | -------------------------------------------------------------------------------- /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 | inline 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 | inline 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/approx_hash_array.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_APPROX_HASH_ARRAY_H_ 2 | #define VOXBLOX_UTILS_APPROX_HASH_ARRAY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "voxblox/core/common.h" 9 | 10 | /** 11 | * These classes allocate a fixed size array and index it with a hash that is 12 | * masked so that only its first N bits are non zero. This can be 13 | * thought of as a fast rough approximation of a hash table. 14 | * There are several advantages and some very significant disadvantages 15 | * Advantages- 16 | * - Simple and blazing fast lockless thread-safe approximate sets 17 | * - Can be used to provide more fine grain locking of blocks for threading then 18 | * simply locking the entire layer 19 | * Disadvantages- 20 | * - Highly inefficient use of memory (allocates 2^N elements) 21 | * - Cannot discern between two different elements with the same hash 22 | * - If the hash of two elements have the same first N elements of their hash, 23 | * only one can be stored. 24 | */ 25 | 26 | namespace voxblox { 27 | 28 | /** 29 | * Basic container, give in an index and get the element that was stored there. 30 | * There are 2^unmasked_bits elements in the container, which element is 31 | * returned depends on your hashing function. 32 | * Uses at least 2^unmasked_bits * sizeof(StoreElement) bytes of ram 33 | */ 34 | template 36 | class ApproxHashArray { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | StoredElement& get(const size_t& hash) { 41 | return pseudo_map_[hash & bit_mask_]; 42 | } 43 | 44 | StoredElement& get(const IndexType& index, size_t* hash) { 45 | DCHECK(hash); 46 | *hash = hasher_(index); 47 | return get(*hash); 48 | } 49 | 50 | StoredElement& get(const IndexType& index) { 51 | size_t hash = hasher_(index); 52 | return get(hash); 53 | } 54 | 55 | private: 56 | static constexpr size_t pseudo_map_size_ = (1 << unmasked_bits); 57 | static constexpr size_t bit_mask_ = (1 << unmasked_bits) - 1; 58 | 59 | std::array pseudo_map_; 60 | IndexTypeHasher hasher_; 61 | }; 62 | 63 | /** 64 | * Acts as a fast and thread safe set, with the serious limitation of both 65 | * false-negatives and false positives being possible. 66 | * A false positive occurs if two different elements have the same hash, and the 67 | * other element was already added to the set. 68 | * A false negative occurs if an element was removed to add another element with 69 | * the same masked hash. The chance of this happening is inversely proportional 70 | * to 2^unmasked_bits. 71 | * Uses at least (2^unmasked_bits + full_reset_threshold) * sizeof(StoreElement) 72 | * bytes of ram. 73 | * Note that the reset function is not thread safe. 74 | */ 75 | template 77 | class ApproxHashSet { 78 | public: 79 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 80 | 81 | ApproxHashSet() : offset_(0), pseudo_set_(pseudo_set_size_) { 82 | for (std::atomic& value : pseudo_set_) { 83 | value.store(0, std::memory_order_relaxed); 84 | } 85 | // The array used for storing values is initialized with zeros. However, the 86 | // zeroth bin can actually store the 0 hash. Because of this to prevent a 87 | // false positive on looking up a 0 hash this bin needs to initially store a 88 | // different value. 89 | pseudo_set_[offset_].store(std::numeric_limits::max()); 90 | } 91 | 92 | /** 93 | * Returns true if an element with the same hash is currently in the set, 94 | * false otherwise. 95 | * Note due to the masking of bits, many elements that were previously 96 | * inserted into the ApproxHashSet have been overwritten by other values. 97 | */ 98 | inline bool isHashCurrentlyPresent(const size_t& hash) { 99 | const size_t array_index = (hash & bit_mask_) + offset_; 100 | 101 | return (pseudo_set_[array_index].load(std::memory_order_relaxed) == hash); 102 | } 103 | 104 | inline bool isHashCurrentlyPresent(const IndexType& index, size_t* hash) { 105 | DCHECK(hash); 106 | *hash = hasher_(index); 107 | return isHashCurrentlyPresent(*hash); 108 | } 109 | 110 | inline bool isHashCurrentlyPresent(const IndexType& index) { 111 | size_t hash = hasher_(index); 112 | return isHashCurrentlyPresent(hash); 113 | } 114 | /** 115 | * Returns true if it replaced the element in the masked_hash's with the hash 116 | * of the given element. 117 | * Returns false if this hash was already there and no replacement was needed. 118 | * THIS IS THE MOST EXPENSIVE FUNCTION IN ALL OF VOXBLOX. 119 | * PROILE AND TEST AFTER EVEN THE MOST SUPERFICIAL CHANGE 120 | * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n 121 | * Also note that while the below layout of ifs and variables may not appear 122 | * the most efficient the compiler seems to do some black magic with it that 123 | * makes it come out ahead of other formulations. 124 | */ 125 | inline bool replaceHash(const size_t& hash) { 126 | const size_t array_index = (hash & bit_mask_) + offset_; 127 | 128 | if (pseudo_set_[array_index].load(std::memory_order_relaxed) == hash) { 129 | return false; 130 | } else { 131 | pseudo_set_[array_index].store(hash, std::memory_order_relaxed); 132 | return true; 133 | } 134 | } 135 | 136 | inline bool replaceHash(const IndexType& index, size_t* hash) { 137 | DCHECK(hash); 138 | *hash = hasher_(index); 139 | return replaceHash(*hash); 140 | } 141 | 142 | inline bool replaceHash(const IndexType& index) { 143 | const size_t hash = hasher_(index); 144 | return replaceHash(hash); 145 | } 146 | 147 | /** 148 | * If unmasked_bits is large, the array takes a lot of memory, this makes 149 | * clearing it slow. 150 | * However offsetting which bin hashes are placed into has the same effect. 151 | * Once we run out of room to offset by (determined by full_reset_threshold) 152 | * we clear the memory). 153 | * This function is not thread safe. 154 | */ 155 | void resetApproxSet() { 156 | if (++offset_ >= full_reset_threshold) { 157 | for (std::atomic& value : pseudo_set_) { 158 | value.store(0, std::memory_order_relaxed); 159 | } 160 | offset_ = 0; 161 | 162 | // The array used for storing values is initialized with zeros. However, 163 | // the zeroth bin can actually store the 0 hash. Because of this to 164 | // prevent a false positive on looking up a 0 hash this bin needs to 165 | // initially store a different value. 166 | pseudo_set_[offset_].store(std::numeric_limits::max()); 167 | } 168 | } 169 | 170 | private: 171 | static constexpr size_t pseudo_set_size_ = 172 | (1 << unmasked_bits) + full_reset_threshold; 173 | static constexpr size_t bit_mask_ = (1 << unmasked_bits) - 1; 174 | 175 | size_t offset_; 176 | std::vector> pseudo_set_; 177 | 178 | IndexTypeHasher hasher_; 179 | }; 180 | } // namespace voxblox 181 | 182 | #endif // VOXBLOX_UTILS_APPROX_HASH_ARRAY_H_ 183 | -------------------------------------------------------------------------------- /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/color_maps.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXBLOX_UTILS_COLOR_MAPS_H_ 2 | #define VOXBLOX_UTILS_COLOR_MAPS_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 | class ColorMap { 13 | public: 14 | ColorMap() : min_value_(0.0), max_value_(1.0) {} 15 | virtual ~ColorMap() {} 16 | 17 | void setMinValue(float min_value) { min_value_ = min_value; } 18 | 19 | void setMaxValue(float max_value) { max_value_ = max_value; } 20 | 21 | virtual Color colorLookup(float value) const = 0; 22 | 23 | protected: 24 | float min_value_; 25 | float max_value_; 26 | }; 27 | 28 | class GrayscaleColorMap : public ColorMap { 29 | public: 30 | virtual Color colorLookup(float value) const { 31 | float new_value = std::min(max_value_, std::max(min_value_, value)); 32 | new_value = (new_value - min_value_) / (max_value_ - min_value_); 33 | return grayColorMap(new_value); 34 | } 35 | }; 36 | 37 | class InverseGrayscaleColorMap : public ColorMap { 38 | public: 39 | virtual Color colorLookup(float value) const { 40 | float new_value = std::min(max_value_, std::max(min_value_, value)); 41 | new_value = (new_value - min_value_) / (max_value_ - min_value_); 42 | return grayColorMap(1.0 - new_value); 43 | } 44 | }; 45 | 46 | class RainbowColorMap : public ColorMap { 47 | public: 48 | virtual Color colorLookup(float value) const { 49 | float new_value = std::min(max_value_, std::max(min_value_, value)); 50 | new_value = (new_value - min_value_) / (max_value_ - min_value_); 51 | return rainbowColorMap(new_value); 52 | } 53 | }; 54 | 55 | class InverseRainbowColorMap : public ColorMap { 56 | public: 57 | virtual Color colorLookup(float value) const { 58 | float new_value = std::min(max_value_, std::max(min_value_, value)); 59 | new_value = (new_value - min_value_) / (max_value_ - min_value_); 60 | return rainbowColorMap(1.0 - new_value); 61 | } 62 | }; 63 | 64 | class IronbowColorMap : public ColorMap { 65 | public: 66 | IronbowColorMap() : ColorMap() { 67 | palette_colors_.push_back(Color(0, 0, 0)); 68 | palette_colors_.push_back(Color(145, 20, 145)); 69 | palette_colors_.push_back(Color(255, 138, 0)); 70 | palette_colors_.push_back(Color(255, 230, 40)); 71 | palette_colors_.push_back(Color(255, 255, 255)); 72 | // Add an extra to avoid overflow. 73 | palette_colors_.push_back(Color(255, 255, 255)); 74 | 75 | increment_ = 1.0 / (palette_colors_.size() - 2); 76 | } 77 | 78 | virtual Color colorLookup(float value) const { 79 | float new_value = std::min(max_value_, std::max(min_value_, value)); 80 | new_value = (new_value - min_value_) / (max_value_ - min_value_); 81 | 82 | size_t index = static_cast(std::floor(new_value / increment_)); 83 | 84 | return Color::blendTwoColors( 85 | palette_colors_[index], increment_ * (index + 1) - new_value, 86 | palette_colors_[index + 1], new_value - increment_ * (index)); 87 | } 88 | 89 | protected: 90 | std::vector palette_colors_; 91 | float increment_; 92 | }; 93 | 94 | class IdColorMap { 95 | public: 96 | IdColorMap() {} 97 | virtual ~IdColorMap() {} 98 | 99 | virtual Color colorLookup(size_t value) const = 0; 100 | }; 101 | /** 102 | * Map unique IDs from [0, Inf[ to unique colors that have constant (irrational) 103 | * spacing and high spread on the color wheel. 104 | * An important advantage of this color map is that the colors are independent 105 | * of the max ID value, e.g. previous colors don't change when adding new IDs. 106 | */ 107 | class IrrationalIdColorMap : IdColorMap { 108 | public: 109 | IrrationalIdColorMap() : irrational_base_(3.f * M_PI) {} 110 | 111 | void setIrrationalBase(float value) { irrational_base_ = value; } 112 | 113 | virtual Color colorLookup(const size_t value) const { 114 | const float normalized_color = std::fmod(value / irrational_base_, 1.f); 115 | return rainbowColorMap(normalized_color); 116 | } 117 | 118 | private: 119 | float irrational_base_; 120 | }; 121 | /** 122 | * Map unique IDs from [0, Inf[ to unique colors 123 | * with piecewise constant spacing and higher spread on the color wheel. 124 | * An important advantage of this color map is that the colors are independent 125 | * of the max ID value, e.g. previous colors don't change when adding new IDs. 126 | */ 127 | class ExponentialOffsetIdColorMap : IdColorMap { 128 | public: 129 | ExponentialOffsetIdColorMap() : items_per_revolution_(10u) {} 130 | 131 | void setItemsPerRevolution(uint value) { items_per_revolution_ = value; } 132 | 133 | virtual Color colorLookup(const size_t value) const { 134 | const size_t revolution = value / items_per_revolution_; 135 | const float progress_along_revolution = 136 | std::fmod(value / items_per_revolution_, 1.f); 137 | // NOTE: std::modf could be used to simultaneously get the integer and 138 | // fractional parts, but the above code is assumed to be more readable 139 | 140 | // Calculate the offset if appropriate 141 | float offset = 0; 142 | if (items_per_revolution_ < value + 1u) { 143 | const size_t current_episode = std::floor(std::log2(revolution)); 144 | const size_t episode_start = std::exp2(current_episode); 145 | const size_t episode_num_subdivisions = episode_start; 146 | const size_t current_subdivision = revolution - episode_start; 147 | const float subdivision_step_size = 148 | 1 / (items_per_revolution_ * 2 * episode_num_subdivisions); 149 | offset = (2 * current_subdivision + 1) * subdivision_step_size; 150 | } 151 | 152 | const float normalized_color = progress_along_revolution + offset; 153 | return rainbowColorMap(normalized_color); 154 | } 155 | 156 | private: 157 | float items_per_revolution_; 158 | }; 159 | 160 | } // namespace voxblox 161 | 162 | #endif // VOXBLOX_UTILS_COLOR_MAPS_H_ 163 | -------------------------------------------------------------------------------- /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/timing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland 3 | * You can contact the author at 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* Adapted from Paul Furgale Schweizer Messer sm_timing */ 19 | 20 | #ifndef VOXBLOX_UTILS_TIMING_H_ 21 | #define VOXBLOX_UTILS_TIMING_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "voxblox/core/common.h" 32 | 33 | namespace voxblox { 34 | 35 | namespace timing { 36 | 37 | template 38 | class Accumulator { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | Accumulator() 43 | : window_samples_(0), 44 | totalsamples_(0), 45 | window_sum_(0), 46 | sum_(0), 47 | min_(std::numeric_limits::max()), 48 | max_(std::numeric_limits::min()) {} 49 | 50 | void Add(T sample) { 51 | if (window_samples_ < N) { 52 | samples_[window_samples_++] = sample; 53 | window_sum_ += sample; 54 | } else { 55 | T& oldest = samples_[window_samples_++ % N]; 56 | window_sum_ += sample - oldest; 57 | oldest = sample; 58 | } 59 | sum_ += sample; 60 | ++totalsamples_; 61 | if (sample > max_) { 62 | max_ = sample; 63 | } 64 | if (sample < min_) { 65 | min_ = sample; 66 | } 67 | } 68 | 69 | int TotalSamples() const { return totalsamples_; } 70 | 71 | double Sum() const { return sum_; } 72 | 73 | double Mean() const { return sum_ / totalsamples_; } 74 | 75 | double RollingMean() const { 76 | return window_sum_ / std::min(window_samples_, N); 77 | } 78 | 79 | double Max() const { return max_; } 80 | 81 | double Min() const { return min_; } 82 | 83 | double LazyVariance() const { 84 | if (window_samples_ == 0) { 85 | return 0.0; 86 | } 87 | double var = 0; 88 | double mean = RollingMean(); 89 | for (int i = 0; i < std::min(window_samples_, N); ++i) { 90 | var += (samples_[i] - mean) * (samples_[i] - mean); 91 | } 92 | var /= std::min(window_samples_, N); 93 | return var; 94 | } 95 | 96 | private: 97 | int window_samples_; 98 | int totalsamples_; 99 | Total window_sum_; 100 | Total sum_; 101 | T min_; 102 | T max_; 103 | T samples_[N]; 104 | }; 105 | 106 | struct TimerMapValue { 107 | TimerMapValue() {} 108 | 109 | /// Create an accumulator with specified window size. 110 | Accumulator acc_; 111 | }; 112 | 113 | /** 114 | * A class that has the timer interface but does nothing. Swapping this in in 115 | * place of the Timer class (say with a typedef) should allow one to disable 116 | * timing. Because all of the functions are inline, they should just disappear. 117 | */ 118 | class DummyTimer { 119 | public: 120 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 121 | 122 | explicit DummyTimer(size_t /*handle*/, bool /*constructStopped*/ = false) {} 123 | explicit DummyTimer(std::string const& /*tag*/, 124 | bool /*constructStopped*/ = false) {} 125 | ~DummyTimer() {} 126 | 127 | void Start() {} 128 | void Stop() {} 129 | bool IsTiming() { return false; } 130 | }; 131 | 132 | class Timer { 133 | public: 134 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 135 | 136 | explicit Timer(size_t handle, bool constructStopped = false); 137 | explicit Timer(std::string const& tag, bool constructStopped = false); 138 | ~Timer(); 139 | 140 | void Start(); 141 | void Stop(); 142 | bool IsTiming() const; 143 | 144 | private: 145 | std::chrono::time_point time_; 146 | 147 | bool timing_; 148 | size_t handle_; 149 | }; 150 | 151 | class Timing { 152 | public: 153 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 154 | 155 | typedef std::map map_t; 156 | friend class Timer; 157 | // Definition of static functions to query the timers. 158 | static size_t GetHandle(std::string const& tag); 159 | static std::string GetTag(size_t handle); 160 | static double GetTotalSeconds(size_t handle); 161 | static double GetTotalSeconds(std::string const& tag); 162 | static double GetMeanSeconds(size_t handle); 163 | static double GetMeanSeconds(std::string const& tag); 164 | static size_t GetNumSamples(size_t handle); 165 | static size_t GetNumSamples(std::string const& tag); 166 | static double GetVarianceSeconds(size_t handle); 167 | static double GetVarianceSeconds(std::string const& tag); 168 | static double GetMinSeconds(size_t handle); 169 | static double GetMinSeconds(std::string const& tag); 170 | static double GetMaxSeconds(size_t handle); 171 | static double GetMaxSeconds(std::string const& tag); 172 | static double GetHz(size_t handle); 173 | static double GetHz(std::string const& tag); 174 | static void Print(std::ostream& out); 175 | static std::string Print(); 176 | static std::string SecondsToTimeString(double seconds); 177 | static void Reset(); 178 | static const map_t& GetTimers() { return Instance().tagMap_; } 179 | 180 | private: 181 | void AddTime(size_t handle, double seconds); 182 | 183 | static Timing& Instance(); 184 | 185 | Timing(); 186 | ~Timing(); 187 | 188 | typedef AlignedVector list_t; 189 | 190 | list_t timers_; 191 | map_t tagMap_; 192 | size_t maxTagLength_; 193 | std::mutex mutex_; 194 | }; 195 | 196 | #if ENABLE_MSF_TIMING 197 | typedef Timer DebugTimer; 198 | #else 199 | typedef DummyTimer DebugTimer; 200 | #endif 201 | 202 | } // namespace timing 203 | } // namespace voxblox 204 | 205 | #endif // VOXBLOX_UTILS_TIMING_H_ 206 | -------------------------------------------------------------------------------- /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/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/integrator_utils.cc: -------------------------------------------------------------------------------- 1 | #include "voxblox/integrator/integrator_utils.h" 2 | 3 | namespace voxblox { 4 | 5 | ThreadSafeIndex* ThreadSafeIndexFactory::get(const std::string& mode, 6 | const Pointcloud& points_C) { 7 | if (mode == "mixed") { 8 | return new MixedThreadSafeIndex(points_C.size()); 9 | } else if (mode == "sorted") { 10 | return new SortedThreadSafeIndex(points_C); 11 | } else { 12 | LOG(FATAL) << "Unknown integration order mode: '" << mode << "'!"; 13 | } 14 | return nullptr; 15 | } 16 | 17 | ThreadSafeIndex::ThreadSafeIndex(size_t number_of_points) 18 | : atomic_idx_(0), number_of_points_(number_of_points) {} 19 | 20 | MixedThreadSafeIndex::MixedThreadSafeIndex(size_t number_of_points) 21 | : ThreadSafeIndex(number_of_points), 22 | number_of_groups_(number_of_points / step_size_) {} 23 | 24 | SortedThreadSafeIndex::SortedThreadSafeIndex(const Pointcloud& points_C) 25 | : ThreadSafeIndex(points_C.size()) { 26 | indices_and_squared_norms_.reserve(points_C.size()); 27 | size_t idx = 0; 28 | for (const Point& point_C : points_C) { 29 | indices_and_squared_norms_.emplace_back(idx, point_C.squaredNorm()); 30 | ++idx; 31 | } 32 | 33 | std::sort( 34 | indices_and_squared_norms_.begin(), indices_and_squared_norms_.end(), 35 | [](const std::pair& a, 36 | const std::pair& b) { return a.second < b.second; }); 37 | } 38 | 39 | // returns true if index is valid, false otherwise 40 | bool ThreadSafeIndex::getNextIndex(size_t* idx) { 41 | DCHECK(idx != nullptr); 42 | size_t sequential_idx = atomic_idx_.fetch_add(1); 43 | 44 | if (sequential_idx >= number_of_points_) { 45 | return false; 46 | } else { 47 | *idx = getNextIndexImpl(sequential_idx); 48 | return true; 49 | } 50 | } 51 | 52 | void ThreadSafeIndex::reset() { atomic_idx_.store(0); } 53 | 54 | size_t MixedThreadSafeIndex::getNextIndexImpl(size_t sequential_idx) { 55 | if (number_of_groups_ * step_size_ <= sequential_idx) { 56 | return sequential_idx; 57 | } 58 | 59 | const size_t group_num = sequential_idx % number_of_groups_; 60 | const size_t position_in_group = sequential_idx / number_of_groups_; 61 | 62 | return group_num * step_size_ + position_in_group; 63 | } 64 | 65 | size_t SortedThreadSafeIndex::getNextIndexImpl(size_t sequential_idx) { 66 | return indices_and_squared_norms_[sequential_idx].first; 67 | } 68 | 69 | // This class assumes PRE-SCALED coordinates, where one unit = one voxel size. 70 | // The indices are also returned in this scales coordinate system, which should 71 | // map to voxel indices. 72 | RayCaster::RayCaster(const Point& origin, const Point& point_G, 73 | const bool is_clearing_ray, 74 | const bool voxel_carving_enabled, 75 | const FloatingPoint max_ray_length_m, 76 | const FloatingPoint voxel_size_inv, 77 | const FloatingPoint truncation_distance, 78 | const bool cast_from_origin) { 79 | const Ray unit_ray = (point_G - origin).normalized(); 80 | 81 | Point ray_start, ray_end; 82 | if (is_clearing_ray) { 83 | FloatingPoint ray_length = (point_G - origin).norm(); 84 | ray_length = std::min(std::max(ray_length - truncation_distance, 85 | static_cast(0.0)), 86 | max_ray_length_m); 87 | ray_end = origin + unit_ray * ray_length; 88 | ray_start = voxel_carving_enabled ? origin : ray_end; 89 | } else { 90 | ray_end = point_G + unit_ray * truncation_distance; 91 | ray_start = voxel_carving_enabled 92 | ? origin 93 | : (point_G - unit_ray * truncation_distance); 94 | } 95 | 96 | const Point start_scaled = ray_start * voxel_size_inv; 97 | const Point end_scaled = ray_end * voxel_size_inv; 98 | 99 | if (cast_from_origin) { 100 | setupRayCaster(start_scaled, end_scaled); 101 | } else { 102 | setupRayCaster(end_scaled, start_scaled); 103 | } 104 | } 105 | 106 | RayCaster::RayCaster(const Point& start_scaled, const Point& end_scaled) { 107 | setupRayCaster(start_scaled, end_scaled); 108 | } 109 | 110 | // returns false if ray terminates at ray_index, true otherwise 111 | bool RayCaster::nextRayIndex(GlobalIndex* ray_index) { 112 | if (current_step_++ > ray_length_in_steps_) { 113 | return false; 114 | } 115 | 116 | DCHECK(ray_index != nullptr); 117 | *ray_index = curr_index_; 118 | 119 | int t_min_idx; 120 | t_to_next_boundary_.minCoeff(&t_min_idx); 121 | curr_index_[t_min_idx] += ray_step_signs_[t_min_idx]; 122 | t_to_next_boundary_[t_min_idx] += t_step_size_[t_min_idx]; 123 | 124 | return true; 125 | } 126 | 127 | void RayCaster::setupRayCaster(const Point& start_scaled, 128 | const Point& end_scaled) { 129 | if (std::isnan(start_scaled.x()) || std::isnan(start_scaled.y()) || 130 | std::isnan(start_scaled.z()) || std::isnan(end_scaled.x()) || 131 | std::isnan(end_scaled.y()) || std::isnan(end_scaled.z())) { 132 | ray_length_in_steps_ = 0; 133 | return; 134 | } 135 | 136 | curr_index_ = getGridIndexFromPoint(start_scaled); 137 | const GlobalIndex end_index = getGridIndexFromPoint(end_scaled); 138 | const GlobalIndex diff_index = end_index - curr_index_; 139 | 140 | current_step_ = 0; 141 | 142 | ray_length_in_steps_ = std::abs(diff_index.x()) + std::abs(diff_index.y()) + 143 | std::abs(diff_index.z()); 144 | 145 | const Ray ray_scaled = end_scaled - start_scaled; 146 | 147 | ray_step_signs_ = AnyIndex(signum(ray_scaled.x()), signum(ray_scaled.y()), 148 | signum(ray_scaled.z())); 149 | 150 | const AnyIndex corrected_step(std::max(0, ray_step_signs_.x()), 151 | std::max(0, ray_step_signs_.y()), 152 | std::max(0, ray_step_signs_.z())); 153 | 154 | const Point start_scaled_shifted = 155 | start_scaled - curr_index_.cast(); 156 | 157 | Ray distance_to_boundaries(corrected_step.cast() - 158 | start_scaled_shifted); 159 | 160 | t_to_next_boundary_ = Ray((std::abs(ray_scaled.x()) < 0.0) 161 | ? 2.0 162 | : distance_to_boundaries.x() / ray_scaled.x(), 163 | (std::abs(ray_scaled.y()) < 0.0) 164 | ? 2.0 165 | : distance_to_boundaries.y() / ray_scaled.y(), 166 | (std::abs(ray_scaled.z()) < 0.0) 167 | ? 2.0 168 | : distance_to_boundaries.z() / ray_scaled.z()); 169 | 170 | // Distance to cross one grid cell along the ray in t. 171 | // Same as absolute inverse value of delta_coord. 172 | t_step_size_ = Ray( 173 | (std::abs(ray_scaled.x()) < 0.0) ? 2.0 174 | : ray_step_signs_.x() / ray_scaled.x(), 175 | (std::abs(ray_scaled.y()) < 0.0) ? 2.0 176 | : ray_step_signs_.y() / ray_scaled.y(), 177 | (std::abs(ray_scaled.z()) < 0.0) ? 2.0 178 | : ray_step_signs_.z() / ray_scaled.z()); 179 | } 180 | 181 | } // namespace voxblox 182 | -------------------------------------------------------------------------------- /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 "proto/voxblox/Block.pb.h" 4 | #include "proto/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/test_tsdf_interpolator.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //#include "voxblox/core/tsdf_map.h" 6 | #include "voxblox/core/common.h" 7 | #include "voxblox/core/layer.h" 8 | #include "voxblox/core/voxel.h" 9 | #include "voxblox/interpolator/interpolator.h" 10 | 11 | using namespace voxblox; // NOLINT 12 | 13 | class TsdfMergeIntegratorTest : public ::testing::Test { 14 | protected: 15 | virtual void SetUp() { 16 | tsdf_voxels_per_side_ = 10u; 17 | tsdf_voxel_size_ = 1.0f; 18 | compare_tol_ = 0.0000001f; 19 | } 20 | 21 | size_t tsdf_voxels_per_side_; 22 | float tsdf_voxel_size_; 23 | float compare_tol_; 24 | 25 | void setBlockSameValues(const float distance, 26 | Block::Ptr block_ptr) { 27 | // Looping and setting all voxels 28 | for (size_t x_idx = 0; x_idx < tsdf_voxels_per_side_; x_idx++) { 29 | for (size_t y_idx = 0; y_idx < tsdf_voxels_per_side_; y_idx++) { 30 | for (size_t z_idx = 0; z_idx < tsdf_voxels_per_side_; z_idx++) { 31 | // Creating the point 32 | VoxelIndex index(x_idx, y_idx, z_idx); 33 | TsdfVoxel& voxel_ref = block_ptr->getVoxelByVoxelIndex(index); 34 | // Setting voxel 35 | voxel_ref.distance = distance; 36 | voxel_ref.weight = 1.0; 37 | } 38 | } 39 | } 40 | // Setting has data flag 41 | block_ptr->has_data() = true; 42 | } 43 | }; 44 | 45 | TEST_F(TsdfMergeIntegratorTest, WithinBlock) { 46 | // New layer 47 | Layer tsdf_layer(tsdf_voxel_size_, tsdf_voxels_per_side_); 48 | 49 | // Allocating the block; 50 | Point origin(0.0, 0.0, 0.0); 51 | Block::Ptr block_ptr = 52 | tsdf_layer.allocateBlockPtrByCoordinates(origin); 53 | 54 | // This is a block where the distance of each voxel indicates height 55 | for (float x = 0.5; x < tsdf_voxels_per_side_; x++) { 56 | for (float y = 0.5; y < tsdf_voxels_per_side_; y++) { 57 | for (float z = 0.5; z < tsdf_voxels_per_side_; z++) { 58 | // Creating the point 59 | Point point(x, y, z); 60 | // Getting a voxel 61 | TsdfVoxel& voxel_ref = block_ptr->getVoxelByCoordinates(point); 62 | // Setting voxel 63 | voxel_ref.distance = z; 64 | voxel_ref.weight = 1.0; 65 | block_ptr->has_data() = true; 66 | } 67 | } 68 | } 69 | 70 | // Creating the interpolator 71 | Interpolator interpolator(&tsdf_layer); 72 | 73 | // A test output voxel 74 | TsdfVoxel voxel; 75 | 76 | // Interpolating below the block 77 | Point point_out_1(1.0, 1.0, 0.1); 78 | Point point_out_2(1.0, 1.0, 0.4); 79 | // Expecting this to not work 80 | EXPECT_FALSE(interpolator.getVoxel(point_out_1, &voxel, true)); 81 | EXPECT_FALSE(interpolator.getVoxel(point_out_2, &voxel, true)); 82 | 83 | // Interpolating within the block 84 | bool failed_to_interpolate = false; 85 | bool wrong_answer = false; 86 | for (float x = 1.0; x < tsdf_voxels_per_side_ - 0.5; x = x + 0.1) { 87 | for (float y = 1.0; y < tsdf_voxels_per_side_ - 0.5; y = y + 0.1) { 88 | for (float z = 1.0; z < tsdf_voxels_per_side_ - 0.5; z = z + 0.1) { 89 | // The point to interpolate at 90 | Point point(x, y, z); 91 | // Interpolating at this point 92 | if (interpolator.getVoxel(point, &voxel, true)) { 93 | if (fabs(voxel.distance - z) > compare_tol_) { 94 | wrong_answer = true; 95 | } 96 | } else { 97 | failed_to_interpolate = true; 98 | } 99 | } 100 | } 101 | } 102 | // Testing the results 103 | EXPECT_FALSE(wrong_answer); 104 | EXPECT_FALSE(failed_to_interpolate); 105 | } 106 | 107 | TEST_F(TsdfMergeIntegratorTest, BetweenBlocks) { 108 | // New layer 109 | Layer tsdf_layer(tsdf_voxel_size_, tsdf_voxels_per_side_); 110 | 111 | // Allocating some blocks (in a sort of 3D plus pattern) 112 | Block::Ptr block_ptr_0 = 113 | tsdf_layer.allocateBlockPtrByCoordinates(Point(0.0, 0.0, 0.0)); 114 | Block::Ptr block_ptr_x_p1 = 115 | tsdf_layer.allocateBlockPtrByCoordinates( 116 | Point(tsdf_voxels_per_side_, 0.0, 0.0)); 117 | Block::Ptr block_ptr_x_n1 = 118 | tsdf_layer.allocateBlockPtrByCoordinates( 119 | Point(-tsdf_voxel_size_, 0.0, 0.0)); 120 | Block::Ptr block_ptr_y_p1 = 121 | tsdf_layer.allocateBlockPtrByCoordinates( 122 | Point(0.0, tsdf_voxels_per_side_, 0.0)); 123 | Block::Ptr block_ptr_y_n1 = 124 | tsdf_layer.allocateBlockPtrByCoordinates( 125 | Point(0.0, -tsdf_voxel_size_, 0.0)); 126 | Block::Ptr block_ptr_z_p1 = 127 | tsdf_layer.allocateBlockPtrByCoordinates( 128 | Point(0.0, 0.0, tsdf_voxels_per_side_)); 129 | Block::Ptr block_ptr_z_n1 = 130 | tsdf_layer.allocateBlockPtrByCoordinates( 131 | Point(0.0, 0.0, -tsdf_voxel_size_)); 132 | 133 | // Assigning the blocks some values 134 | setBlockSameValues(0.0, block_ptr_0); 135 | setBlockSameValues(1.0, block_ptr_x_p1); 136 | setBlockSameValues(-1.0, block_ptr_x_n1); 137 | setBlockSameValues(1.0, block_ptr_y_p1); 138 | setBlockSameValues(-1.0, block_ptr_y_n1); 139 | setBlockSameValues(1.0, block_ptr_z_p1); 140 | setBlockSameValues(-1.0, block_ptr_z_n1); 141 | 142 | // Interpolating some bullshit 143 | Interpolator interpolator(&tsdf_layer); 144 | 145 | // Points to interpolate (below origin block) 146 | AlignedVector points_below; 147 | points_below.push_back(Point(1.0, 1.0, 0.5)); 148 | points_below.push_back(Point(1.0, 1.0, 0.25)); 149 | points_below.push_back(Point(1.0, 1.0, 0.0)); 150 | points_below.push_back(Point(1.0, 1.0, -0.25)); 151 | points_below.push_back(Point(1.0, 1.0, -0.5)); 152 | 153 | // Expected answers (hand calculated) 154 | float expected_answers_below[5] = {0, -0.25, -0.5, -0.75, -1.0}; 155 | 156 | // Looping and testing 157 | TsdfVoxel voxel; 158 | for (size_t point_index = 0; point_index < points_below.size(); 159 | point_index++) { 160 | // Getting the interpolated point 161 | Point point = points_below[point_index]; 162 | interpolator.getVoxel(point, &voxel, true); 163 | // Testing 164 | EXPECT_NEAR(voxel.distance, expected_answers_below[point_index], 165 | compare_tol_); 166 | } 167 | } 168 | 169 | int main(int argc, char** argv) { 170 | ::testing::InitGoogleTest(&argc, argv); 171 | google::InitGoogleLogging(argv[0]); 172 | 173 | int result = RUN_ALL_TESTS(); 174 | 175 | return result; 176 | } 177 | -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------