├── .devcontainer ├── devcontainer.json └── docker-compose.yml ├── .github └── workflows │ └── main.yml ├── .gitignore ├── Dockerfile ├── LICENSE ├── README.md ├── apps └── kfusion │ ├── CMakeLists.txt │ ├── Makefile │ ├── cmake │ ├── FindOpenCL.cmake │ ├── FindOpenNI2.cmake │ ├── FindPAPI.cmake │ └── FindTooN.cmake │ ├── demo │ ├── carrot.sh │ ├── cup_and_bottle.sh │ ├── multi_moving.sh │ ├── rotate_book.sh │ └── syn_2objs.sh │ ├── include │ ├── PowerMonitor.h │ ├── commons.h │ ├── config.h │ ├── constant_parameters.h │ ├── default_parameters.h │ ├── draw.h │ ├── interface.h │ ├── kernels.h │ ├── marching_cubes.h │ ├── tick.h │ └── vtk-io.h │ ├── moc_ApplicationWindow.cpp │ ├── moc_MainWindow.cpp │ ├── moc_OpenGLWidget.cpp │ ├── moc_qcustomplot.cpp │ ├── qt │ ├── ApplicationWindow.cpp │ ├── ApplicationWindow.h │ ├── MainWindow.cpp │ ├── MainWindow.h │ ├── OpenGLWidget.cpp │ ├── OpenGLWidget.h │ ├── SlamBenchQt.cpp │ ├── images.qrc │ ├── images.qrc.depends │ ├── images │ │ ├── camera.png │ │ ├── clear24.png │ │ ├── film.png │ │ ├── live.png │ │ ├── pause.png │ │ ├── play.png │ │ ├── restart.png │ │ ├── rotccw24.png │ │ ├── rotcw24.png │ │ ├── rotd24.png │ │ ├── rotu24.png │ │ ├── step.png │ │ └── tripod.png │ ├── licenses │ │ ├── GPL.txt │ │ ├── lgpl.txt │ │ └── products.txt │ ├── qcustomplot.cpp │ └── qcustomplot.h │ ├── scripts │ ├── README.md │ ├── __init__.py │ ├── _dataset.py │ ├── _run.py │ ├── _util.py │ ├── associate.py │ ├── associate.txt │ ├── datasets.py │ ├── evaluate_ate.py │ ├── evaluate_rpe.py │ ├── run_kf.py │ └── systemsettings.py │ ├── src │ ├── PowerMonitor.cpp │ ├── benchmark.cpp │ ├── cpp │ │ ├── bfusion │ │ │ ├── alloc_impl.hpp │ │ │ ├── bspline_lookup.cc │ │ │ ├── mapping_impl.hpp │ │ │ └── rendering_impl.hpp │ │ ├── continuous │ │ │ ├── volume_instance.hpp │ │ │ ├── volume_template.hpp │ │ │ └── volume_traits.hpp │ │ ├── kfusion.cpp │ │ ├── kfusion │ │ │ ├── alloc_impl.hpp │ │ │ ├── mapping_impl.hpp │ │ │ └── rendering_impl.hpp │ │ ├── mapping.hpp │ │ ├── object │ │ │ ├── object.cpp │ │ │ └── object.h │ │ ├── preprocessing │ │ │ ├── preprocessing.cpp │ │ │ └── preprocessing.h │ │ ├── rendering │ │ │ ├── rendering.cpp │ │ │ └── rendering.h │ │ ├── segmentation │ │ │ ├── segmentation.cpp │ │ │ └── segmentation.h │ │ └── tracking │ │ │ ├── tracking.cpp │ │ │ └── tracking.h │ ├── mainQt.cpp │ └── reader.cpp │ ├── test │ ├── CMakeLists.txt │ ├── Makefile │ ├── callSegmentationUnitTest │ ├── callTrackingUnitTest │ ├── preprocessing │ │ ├── CMakeLists.txt │ │ └── interp_unittest.cpp │ ├── segmentation │ │ ├── 1341846436.889894.png │ │ ├── 1341846436.889927.png │ │ ├── 1341846436.922139.png │ │ ├── 1341846436.922153.png │ │ ├── CMakeLists.txt │ │ ├── classid │ │ │ ├── 0064.npy │ │ │ └── 0065.npy │ │ ├── mask │ │ │ ├── 0064.npy │ │ │ └── 0065.npy │ │ ├── pose │ │ └── segmentation_unittest.cpp │ └── tracking │ │ ├── 1341846436.889894.png │ │ ├── 1341846436.889927.png │ │ ├── 1341846436.922139.png │ │ ├── 1341846436.922153.png │ │ ├── 612.png │ │ ├── 613.png │ │ ├── 613_depth.png │ │ ├── 720.png │ │ ├── 720_depth.png │ │ ├── 721.png │ │ ├── 721_depth.png │ │ ├── CMakeLists.txt │ │ ├── jacobians_unittest.cpp │ │ ├── tum pose │ │ └── tum unit test.zip │ └── thirdparty │ ├── CMakeLists.txt │ ├── Makefile │ ├── OCLWrapper.cpp │ ├── checkKernels.py │ ├── checkPos.py │ ├── cnpy │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cnpy.cpp │ ├── cnpy.h │ ├── example1.cpp │ ├── mat2npz │ ├── npy2mat │ └── npz2mat │ ├── generatePlots.py │ ├── kfusion.h │ ├── lodepng.cpp │ ├── lodepng.h │ ├── nvprof2log.py │ ├── oni2raw.cpp │ ├── processPowerRpt │ ├── scene2raw.cpp │ ├── tiny_obj_loader.cc │ └── tiny_obj_loader.h ├── lib ├── algorithms │ ├── edge_tables.h │ ├── filter.hpp │ ├── mapping.hpp │ ├── meshing.hpp │ ├── raycasting.cpp │ └── unique.hpp ├── data.hpp ├── functors │ ├── axis_aligned_functor.hpp │ ├── data_handler.hpp │ └── projective_functor.hpp ├── geometry │ ├── aabb_collision.hpp │ └── octree_collision.hpp ├── interpolation │ └── interp_gather.hpp ├── node.hpp ├── octant_ops.hpp ├── octree.hpp ├── octree_defines.h ├── test │ ├── CMakeLists.txt │ ├── Makefile │ ├── algorithms │ │ ├── CMakeLists.txt │ │ └── unique_unittest.cpp │ ├── allocation │ │ ├── CMakeLists.txt │ │ └── alloc_unittest.cpp │ ├── functor │ │ ├── CMakeLists.txt │ │ └── axisaligned_unittest.cpp │ ├── geometry │ │ ├── CMakeLists.txt │ │ ├── aabb_collision_unittest.cpp │ │ └── octree_collision_unittest.cpp │ ├── interp │ │ ├── CMakeLists.txt │ │ ├── gather_unittest.cpp │ │ └── interpolation_unittest.cpp │ ├── multiscale │ │ ├── CMakeLists.txt │ │ └── multiscale_unittest.cpp │ ├── octree │ │ ├── CMakeLists.txt │ │ └── octree_unittest.cpp │ └── utils │ │ ├── CMakeLists.txt │ │ └── morton_unittest.cpp ├── utils │ ├── memory_pool.hpp │ └── morton_utils.hpp └── voxel_traits.hpp ├── teaser └── midfusion.gif └── utils ├── cutil_math.h ├── download_data.py ├── math_utils.h ├── perfstats.h ├── timings.h └── vector_types.h /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "midfusion Dev Container", 3 | "dockerComposeFile": "docker-compose.yml", 4 | "service": "midfusion", 5 | "workspaceFolder": "/volume", 6 | "shutdownAction": "stopCompose", 7 | "extensions": [ 8 | "ms-vscode.cpptools", 9 | "ms-vscode.cmake-tools", 10 | "mutantdino.resourcemonitor", 11 | "ms-python.python" 12 | ] 13 | } 14 | -------------------------------------------------------------------------------- /.devcontainer/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: "3.8" 2 | 3 | services: 4 | midfusion: 5 | image: midfusion:latest 6 | userns_mode: host 7 | build: 8 | context: .. 9 | dockerfile: ./Dockerfile 10 | stdin_open: true 11 | tty: true 12 | network_mode: host 13 | environment: 14 | NVIDIA_DRIVER_CAPABILITIES: compute,utility,graphics 15 | DISPLAY: $DISPLAY 16 | volumes: 17 | - ../:/volume 18 | - /tmp/.X11-unix:/tmp/.X11-unix 19 | - /home/bxu/data/datasets:/data 20 | working_dir: /volume 21 | command: /bin/bash 22 | 23 | 24 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | # Controls when the action will run. Triggers the workflow on push or pull request events 4 | on: [push, pull_request] 5 | 6 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 7 | jobs: 8 | # This workflow contains a single job called "build" 9 | build: 10 | # The type of runner that the job will run on 11 | runs-on: ubuntu-18.04 12 | 13 | # Steps represent a sequence of tasks that will be executed as part of the job 14 | steps: 15 | # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it 16 | - uses: actions/checkout@v2 17 | - name: install dependency 18 | run: | 19 | sudo apt-get install libeigen3-dev libopencv-dev qtbase5-dev 20 | 21 | - name: build 22 | run: | 23 | cd apps/kfusion 24 | make 25 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | apps/kfusion/build/ 2 | apps/kfusion/Release/ 3 | apps/kfusion/cmake-build-debug/ 4 | apps/kfusion/TooN/ 5 | apps/kfusion/scripts/results/ 6 | apps/kfusion/scripts/*.pyc 7 | apps/kfusion/test/tracking/CMakeFiles/ 8 | apps/kfusion/test/preprocessing/CMakeFiles/ 9 | apps/kfusion/test/tracking/Makefile 10 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:18.04 2 | 3 | ARG DEBIAN_FRONTEND=noninteractive 4 | 5 | # system 6 | RUN echo "Installing apt packages..." \ 7 | && export DEBIAN_FRONTEND=noninteractive \ 8 | && apt -y update --no-install-recommends \ 9 | && apt -y install --no-install-recommends \ 10 | git \ 11 | wget \ 12 | libeigen3-dev \ 13 | freeglut3-dev \ 14 | sudo \ 15 | && apt autoremove -y \ 16 | && apt clean -y \ 17 | && export DEBIAN_FRONTEND=dialog 18 | 19 | RUN DEBIAN_FRONTEND=noninteractive TZ=Etc/UTC apt-get -y install tzdata 20 | 21 | # install OKVIS dependencies 22 | RUN echo "Installing apt packages..." \ 23 | && export DEBIAN_FRONTEND=noninteractive \ 24 | && apt -y update --no-install-recommends \ 25 | && apt -y install --no-install-recommends \ 26 | cmake \ 27 | # libatlas-base-dev \ 28 | libeigen3-dev \ 29 | # libsuitesparse-dev \ 30 | libeigen3-dev libopencv-dev qtbase5-dev \ 31 | gcc-6 g++-6 \ 32 | build-essential \ 33 | libboost-all-dev \ 34 | libopencv-dev \ 35 | # libceres-dev \ 36 | libomp-dev \ 37 | ca-certificates \ 38 | && apt autoremove -y \ 39 | && apt clean -y \ 40 | && export DEBIAN_FRONTEND=dialog 41 | 42 | # # install librealsense 43 | # RUN echo "Installing librealsense..." \ 44 | # && export DEBIAN_FRONTEND=noninteractive \ 45 | # && apt -y update --no-install-recommends \ 46 | # && apt -y install --no-install-recommends \ 47 | # libusb-1.0-0-dev \ 48 | # libglfw3-dev \ 49 | # pkg-config \ 50 | # libgtk-3-dev \ 51 | # gcc-6 g++-6 \ 52 | # build-essential \ 53 | # && apt autoremove -y \ 54 | # && apt clean -y \ 55 | # && export DEBIAN_FRONTEND=dialog 56 | 57 | # # install legacy realsense 1 58 | # COPY ./thirdparty/librealsense /opt/librealsense 59 | # RUN cd /opt/librealsense \ 60 | # && mkdir build \ 61 | # && cd build \ 62 | # && cmake -DCMAKE_BUILD_TYPE=Release .. \ 63 | # && make -j8 \ 64 | # && make install 65 | 66 | # # install geographiclib 67 | # RUN echo "Installing geographiclib..." 68 | # RUN git clone git://git.code.sourceforge.net/p/geographiclib/code /opt/geographiclib \ 69 | # && cd /opt/geographiclib \ 70 | # && mkdir build \ 71 | # && cd build \ 72 | # && cmake -DCMAKE_BUILD_TYPE=Release .. \ 73 | # && make -j8 \ 74 | # && make install 75 | 76 | 77 | # # RUN git clone git@bitbucket.org:sleutenegger/okvis.git /opt/okvis \ 78 | # # RUN cd /opt/okvis \ 79 | # # && git checkout ofusion++ \ 80 | # # TODO: hack, because ofusion++ branch is not public -> replace it with public branch later 81 | # COPY ./thirdparty/okvis /opt/okvis 82 | # RUN cd /opt/okvis \ 83 | # && mkdir build \ 84 | # && cd build \ 85 | # && cmake -D CMAKE_C_COMPILER=gcc-6 -D CMAKE_CXX_COMPILER=g++-6 -DCMAKE_BUILD_TYPE=Release .. \ 86 | # && make -j8 \ 87 | # && make install 88 | 89 | # install librealsense 90 | RUN echo "Installing GUI stuff..." \ 91 | && export DEBIAN_FRONTEND=noninteractive \ 92 | && apt -y update --no-install-recommends \ 93 | && apt -y install --no-install-recommends \ 94 | language-pack-en-base \ 95 | libcanberra-gtk-module libcanberra-gtk3-module \ 96 | && apt autoremove -y \ 97 | && apt clean -y \ 98 | && export DEBIAN_FRONTEND=dialog 99 | RUN dpkg-reconfigure locales && \ 100 | locale-gen en_US.UTF-8 && \ 101 | /usr/sbin/update-locale LANG=en_US.UTF-8 102 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright © 2017-2019 Smart Robotics Lab, Imperial College London 4 | Copyright © 2017-2019 Binbin Xu 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /apps/kfusion/Makefile: -------------------------------------------------------------------------------- 1 | BLOCK_SIZE = 8 2 | SIZE = 512 3 | C=2 4 | #### ICL-NUIM TRAJECTORIES PARAMETERS #### 5 | 0 = -s 5.0 -p 0.34,0.5,0.24 -z 1 -c $C -r 1 -k 481.2,-480,320,240 -B $(BLOCK_SIZE) -v $(SIZE) -m 0.1 -F 6 | 1 = -s 5.0 -p 0.485,0.5,0.55 -z 1 -c $C -r 1 -k 481.2,-480,320,240 -B $(BLOCK_SIZE) -v $(SIZE) -m 0.1 -F 7 | 2 = -s 5.0 -p 0.34,0.5,0.24 -z 1 -c $C -r 1 -k 481.2,-480,320,240 -B $(BLOCK_SIZE) -v $(SIZE) -m 0.1 -F 8 | 3 = -s 5.0 -p 0.2685,0.5,0.4 -z 1 -c $C -r 1 -k 481.2,-480,320,240 -B $(BLOCK_SIZE) -v $(SIZE) -m 0.1 -F 9 | live = -s 5.0 -p 0.5,0.5,0.5 -z 1 -c 1 -r 1 -k 580.8,581.8,308.8,253.0 -B $(BLOCK_SIZE) -v $(SIZE) -m 0.1 10 | fr2desk = -s 5.0 -p 0.5,0.5,0.2 -z 1 -c 2 -r 1 -k 580.8,581.8,308.8,253.0 -B $(BLOCK_SIZE) -v $(SIZE) -m 0.1 -F 11 | # fr2xyz = -s 8.0 -p 0.5,0.5,0.5 -z 1 -c 2 -r 1 -k 580.8,581.8,308.8,253.0 -B $(BLOCK_SIZE) -v $(SIZE),$(SIZE),$(SIZE) -m 0.1 -F 12 | 13 | ROOT_DIR=$(shell pwd) 14 | TOON_DIR=${ROOT_DIR}/TooN/install_dir 15 | TOON_INCLUDE_DIR=${TOON_DIR}/include/ 16 | 17 | 18 | all : TooN 19 | mkdir -p build/ 20 | cd build/ && cmake -DCMAKE_BUILD_TYPE=Release -DTOON_INCLUDE_PATH=${TOON_INCLUDE_DIR} $(CMAKE_ARGUMENTS) .. 21 | $(MAKE) -C build $(MFLAGS) $(SPECIFIC_TARGET) 22 | 23 | 24 | debug: TooN 25 | mkdir -p build/ 26 | mkdir -p build/logs/ 27 | cd build/ && cmake -DCMAKE_BUILD_TYPE=Debug -DTOON_INCLUDE_PATH=${TOON_INCLUDE_DIR} $(CMAKE_ARGUMENTS) .. 28 | $(MAKE) -C build $(MFLAGS) 29 | 30 | 31 | TooN: 32 | git clone https://github.com/edrosten/TooN.git 33 | cd TooN && git checkout 92241416d2a4874fd2334e08a5d417dfea6a1a3f 34 | mkdir -p ${TOON_DIR} 35 | cd TooN && ./configure --prefix=${TOON_DIR} && make install 36 | 37 | #### Usage #### 38 | demo_cup_bottle: 39 | @demo/cup_and_bottle.sh 40 | 41 | demo_carrot: 42 | @demo/carrot.sh 43 | 44 | demo_multi_obj: 45 | @demo/multi_moving.sh 46 | 47 | demo_rotate_book: 48 | @demo/rotate_book.sh 49 | 50 | demo_syn_2objs: 51 | @demo/syn_2objs.sh 52 | 53 | #### GENERAL GENERATION #### 54 | clean : 55 | rm -rf build 56 | rm -fr TooN 57 | 58 | .PHONY : clean debug all -------------------------------------------------------------------------------- /apps/kfusion/cmake/FindOpenNI2.cmake: -------------------------------------------------------------------------------- 1 | 2 | find_library(OPENNI2_LIBRARY 3 | NAMES OpenNI2 4 | PATHS ~/usr/lib ~/usr/local/lib ~/.local/lib64 ~/.local/lib /usr/lib 5 | /usr/local/lib /data/Repositories/OpenNI2-2.2-beta2/Bin/x64-Release 6 | /scratch/cad/OpenNI/Redist 7 | /data/sw/OpenNI/OpenNI2-2.2-beta2/Bin/x64-Release 8 | /opt/OpenNI-Linux-x64-2.2/Redist 9 | PATH_SUFFIXES openni2 ni2 10 | ) 11 | 12 | find_path(OPENNI2_INCLUDE_PATH 13 | NAMES OpenNI.h 14 | PATHS ~/usr/include ~/.local/include ~/usr/local/include /usr/include 15 | /usr/local/include /scratch/cad/OpenNI/Include 16 | /data/sw/OpenNI/OpenNI2-2.2-beta2/Include 17 | /opt/OpenNI-Linux-x64-2.2/Include 18 | PATH_SUFFIXES openni2 ni2 19 | ) 20 | 21 | 22 | if(OPENNI2_LIBRARY) 23 | else() 24 | message("-- NOT Found OpenNI2 Library: ${OPENNI2_LIBRARY}") 25 | endif() 26 | if(OPENNI2_INCLUDE_PATH) 27 | else() 28 | message("-- NOT Found OpenNI2 includes: ${OPENNI2_INCLUDE_PATH}") 29 | endif() 30 | if(OPENNI2_LIBRARY AND OPENNI2_INCLUDE_PATH) 31 | message("-- Found OpenNI2: ${OPENNI2_LIBRARY}") 32 | set(OPENNI2_FOUND TRUE) 33 | set(OPENNI2_INCLUDE_PATHS ${OPENNI2_INCLUDE_PATH} CACHE STRING "The include paths needed to use OpenNI2") 34 | set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} CACHE STRING "The libraries needed to use OpenNI2") 35 | else() 36 | message("-- NOT FOUND: OpenNI2") 37 | endif() 38 | 39 | mark_as_advanced( 40 | OPENNI2_INCLUDE_PATHS 41 | OPENNI2_LIBRARIES 42 | ) 43 | -------------------------------------------------------------------------------- /apps/kfusion/cmake/FindPAPI.cmake: -------------------------------------------------------------------------------- 1 | # Try to find PAPI headers and libraries. 2 | # 3 | # Usage of this module as follows: 4 | # 5 | # find_package(PAPI) 6 | # 7 | # Variables used by this module, they can change the default behaviour and need 8 | # to be set before calling find_package: 9 | # 10 | # PAPI_PREFIX Set this variable to the root installation of 11 | # libpapi if the module has problems finding the 12 | # proper installation path. 13 | # 14 | # Variables defined by this module: 15 | # 16 | # PAPI_FOUND System has PAPI libraries and headers 17 | # PAPI_LIBRARIES The PAPI library 18 | # PAPI_INCLUDE_DIRS The location of PAPI headers 19 | 20 | find_path(PAPI_PREFIX 21 | NAMES include/papi.h 22 | ) 23 | 24 | find_library(PAPI_LIBRARIES 25 | # Pick the static library first for easier run-time linking. 26 | NAMES libpapi.a papi 27 | HINTS ${PAPI_PREFIX}/lib ${HILTIDEPS}/lib 28 | ) 29 | 30 | find_path(PAPI_INCLUDE_DIRS 31 | NAMES papi.h 32 | HINTS ${PAPI_PREFIX}/include ${HILTIDEPS}/include 33 | ) 34 | 35 | include(FindPackageHandleStandardArgs) 36 | find_package_handle_standard_args(PAPI DEFAULT_MSG 37 | PAPI_LIBRARIES 38 | PAPI_INCLUDE_DIRS 39 | ) 40 | 41 | mark_as_advanced( 42 | PAPI_PREFIX_DIRS 43 | PAPI_LIBRARIES 44 | PAPI_INCLUDE_DIRS 45 | ) -------------------------------------------------------------------------------- /apps/kfusion/cmake/FindTooN.cmake: -------------------------------------------------------------------------------- 1 | 2 | find_path(TOON_INCLUDE_PATH TooN/TooN.h 3 | ~/usr/include 4 | ~/usr/.local/include 5 | ~/.local/include 6 | ~/usr/local/include 7 | /usr/include 8 | /usr/local/include 9 | thirdparty 10 | ) 11 | 12 | if(TOON_INCLUDE_PATH) 13 | set(TooN_FOUND TRUE) 14 | set(TOON_INCLUDE_PATHS ${TOON_INCLUDE_PATH} CACHE STRING "The include paths needed to use TooN") 15 | endif() 16 | 17 | mark_as_advanced( 18 | TOON_INCLUDE_PATHS 19 | ) 20 | 21 | 22 | # Generate appropriate messages 23 | if(TooN_FOUND) 24 | if(NOT TooN_FIND_QUIETLY) 25 | message("-- Found Toon: ${TOON_INCLUDE_PATH}") 26 | endif(NOT TooN_FIND_QUIETLY) 27 | else(TooN_FOUND) 28 | if(TooN_FIND_REQUIRED) 29 | message(FATAL_ERROR "-- Could NOT find TooN (missing: TOON_INCLUDE_PATH)") 30 | endif(TooN_FIND_REQUIRED) 31 | endif(TooN_FOUND) 32 | -------------------------------------------------------------------------------- /apps/kfusion/demo/carrot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 3 | ROOT=$HERE/.. 4 | 5 | cd $ROOT/build 6 | ./kfusion-main-openmp --compute-size-ratio 2 --fps 0 --block-read False --input-file /data/midfusion/carrot/2018-08-22.08.raw --mask-rcnn-folder /data/midfusion/carrot/mask_RCNN --icp-threshold 1e-05 --mu 0.075 --init-pose 0.5,0.5,0.4 --integration-rate 1 --volume-size 2.5 -B 8 --tracking-rate 1 --volume-resolution 1024 --pyramid-levels 10,5,4 --rendering-rate 1 -k 525,525,319.5,239.5 -a 0.6574,0.6126,-0.2949,-0.3248 -F --min-obj-ratio 0.000000001 7 | 8 | -------------------------------------------------------------------------------- /apps/kfusion/demo/cup_and_bottle.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 3 | ROOT=$HERE/.. 4 | 5 | cd $ROOT/build 6 | ./kfusion-main-openmp --compute-size-ratio 2 --fps 0 --block-read False --input-file /data/midfusion/cup_bottle/2018-08-23.00.raw --mask-rcnn-folder /data/midfusion/cup_bottle/mask_RCNN --icp-threshold 1e-05 --mu 0.075 --init-pose 0.5,0.5,0.4 --integration-rate 1 --volume-size 2.5 -B 8 --tracking-rate 1 --volume-resolution 512 --pyramid-levels 10,5,4 --rendering-rate 1 -k 525,525,319.5,239.5 -a 0.6574,0.6126,-0.2949,-0.3248 -F --min-obj-ratio 0.001 -------------------------------------------------------------------------------- /apps/kfusion/demo/multi_moving.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 4 | ROOT=$HERE/.. 5 | 6 | cd $ROOT/build 7 | ./kfusion-main-openmp --compute-size-ratio 1 --fps 0 --block-read False --input-file /data/midfusion/multi-moving_obj/2018-09-10.02.raw --mask-rcnn-folder /data/midfusion/multi-moving_obj/mask_RCNN --icp-threshold 1e-05 --mu 0.075 --init-pose 0.5,0.5,0.4 --integration-rate 1 --volume-size 2.5 -B 8 --tracking-rate 1 --volume-resolution 512 --pyramid-levels 10,5,4 --rendering-rate 1 -k 525,525,319.5,239.5 -a 0.6574,0.6126,-0.2949,-0.3248 -F --min-obj-ratio 0.001 8 | 9 | -------------------------------------------------------------------------------- /apps/kfusion/demo/rotate_book.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 4 | ROOT=$HERE/.. 5 | 6 | cd $ROOT/build 7 | ./kfusion-main-openmp --compute-size-ratio 2 --fps 0 --block-read False --input-file /data/midfusion/rotated_book/2018-08-30.01.raw --mask-rcnn-folder /data/midfusion/rotated_book/mask_RCNN --icp-threshold 1e-05 --mu 0.075 --init-pose 0.5,0.5,0.4 --integration-rate 1 --volume-size 5 -B 8 --tracking-rate 1 --volume-resolution 1024 --pyramid-levels 10,5,4 --rendering-rate 1 -k 525,525,319.5,239.5 -a 0.6574,0.6126,-0.2949,-0.3248 -F --absorb-outlier-bg --init 1 -absorb-outlier-bg -------------------------------------------------------------------------------- /apps/kfusion/demo/syn_2objs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 4 | ROOT=$HERE/.. 5 | 6 | cd $ROOT/build 7 | ./kfusion-main-openmp --compute-size-ratio 2 --fps 0 --block-read False --input-file /data/midfusion/synthetic/cam0_gt.raw --mask-rcnn-folder /data/midfusion/synthetic/mask_RCNN --icp-threshold 1e-05 --mu 0.075 --init-pose 0.5,0.5,0.4 --integration-rate 1 --volume-size 10 -B 8 --tracking-rate 1 --volume-resolution 512 --pyramid-levels 10,10,10 --rendering-rate 1 -k 600,600,319.5,239.5 -a 0.6574,0.6126,-0.2949,-0.3248 -F --min-obj-ratio 0.01 --absorb-outlier-bg --ground-truth-segment /data/midfusion/synthetic/label0/data/ 8 | 9 | -------------------------------------------------------------------------------- /apps/kfusion/include/PowerMonitor.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 4 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 5 | 6 | This code is licensed under the MIT License. 7 | 8 | */ 9 | 10 | #ifndef __PowerMonitor__ 11 | #define __PowerMonitor__ 12 | class PowerMonitor { 13 | 14 | public: 15 | enum Sensor { 16 | SENSOR_A7 = 45, SENSOR_A15 = 40, SENSOR_GPU = 44, SENSOR_DRAM = 41 17 | }; 18 | enum PowerMethod { 19 | ODROID, ARM_PROBE, PERFORMANCE_COUNTERS, NONE 20 | }; 21 | PowerMonitor(); 22 | ~PowerMonitor(); 23 | float getPower(Sensor); 24 | bool isActive(); 25 | PerfStats powerStats; 26 | double sample(); 27 | double start(); 28 | private: 29 | 30 | FILE *powerA7; 31 | FILE *powerA15; 32 | FILE *powerGPU; 33 | FILE *powerDRAM; 34 | int enableSensor(Sensor); 35 | PowerMethod sensingMethod; 36 | double startTime; 37 | }; 38 | #endif 39 | -------------------------------------------------------------------------------- /apps/kfusion/include/config.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | #ifndef CONFIG_H 9 | #define CONFIG_H 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | struct Configuration { 16 | 17 | // 18 | // KFusion configuration parameters 19 | // Command line arguments are parsed in default_parameters.h 20 | // 21 | 22 | int compute_size_ratio; 23 | int integration_rate; 24 | int rendering_rate; 25 | int tracking_rate; 26 | uint3 volume_resolution; 27 | float3 volume_size; 28 | int voxel_block_size; 29 | float3 initial_pos_factor; 30 | std::vector pyramid; 31 | std::string dump_volume_file; 32 | std::string input_file; 33 | std::string log_file; 34 | std::string groundtruth_file; 35 | 36 | float4 camera; 37 | bool camera_overrided; 38 | 39 | float mu; 40 | int fps; 41 | bool blocking_read; 42 | float icp_threshold; 43 | bool no_gui; 44 | bool render_volume_fullsize; 45 | bool bilateralFilter; 46 | bool colouredVoxels; 47 | bool multiResolution; 48 | bool bayesian; 49 | 50 | //for inputing mask-rcnn segmentation 51 | std::string maskrcnn_folder; 52 | std::string gt_mask_folder; 53 | 54 | bool pause; 55 | bool in_debug; 56 | std::string output_images; 57 | float min_obj_ratio; 58 | bool disable_rgb; 59 | bool absorb_outlier_bg; 60 | bool geom_refine_human; 61 | float obj_moved; 62 | int init_frame; 63 | }; 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /apps/kfusion/include/constant_parameters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | #ifndef CONSTANT_PARAMETERS_H_ 9 | #define CONSTANT_PARAMETERS_H_ 10 | 11 | ////////////////////////// COMPILATION PARAMETERS ////////////////////// 12 | 13 | const float e_delta = 0.1f; 14 | const int radius = 2; 15 | const float dist_threshold = 0.1f; 16 | const float normal_threshold = 0.8f; 17 | const float track_threshold = 0.15f; 18 | const float maxweight = 100.0f; 19 | const float nearPlane = 0.1f; 20 | const float farPlane = 4.0f; 21 | const float delta = 4.0f; 22 | 23 | const float3 light = make_float3(1, 1, -1.0); 24 | const float3 ambient = make_float3(0.1, 0.1, 0.1); 25 | 26 | #endif /* CONSTANT_PARAMETERS_H_ */ 27 | -------------------------------------------------------------------------------- /apps/kfusion/include/draw.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2011-2013 Gerhard Reitmayr, TU Graz 4 | 5 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 6 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 7 | 8 | This code is licensed under the MIT License. 9 | 10 | */ 11 | #ifdef __APPLE__ 12 | #include 13 | #else 14 | #include 15 | #endif 16 | 17 | #define WIDTH 1024 18 | #define HEIGHT 768 19 | 20 | template struct gl; 21 | 22 | template<> struct gl { 23 | static const int format = GL_LUMINANCE; 24 | static const int type = GL_UNSIGNED_BYTE; 25 | }; 26 | 27 | template<> struct gl { 28 | static const int format = GL_RGB; 29 | static const int type = GL_UNSIGNED_BYTE; 30 | }; 31 | 32 | template<> struct gl { 33 | static const int format = GL_RGBA; 34 | static const int type = GL_UNSIGNED_BYTE; 35 | }; 36 | 37 | template<> struct gl { 38 | static const int format = GL_LUMINANCE; 39 | static const int type = GL_FLOAT; 40 | }; 41 | template<> struct gl { 42 | static const int format = GL_LUMINANCE; 43 | static const int type = GL_UNSIGNED_SHORT; 44 | }; 45 | template<> struct gl { 46 | static const int format = GL_RGB; 47 | static const int type = GL_FLOAT; 48 | }; 49 | 50 | template 51 | void drawit(T* scene, uint2 size) { 52 | static uint2 lastsize = { 0, 0 }; 53 | char * t = (char*) "toto"; 54 | int g = 1; 55 | if (lastsize.x != size.x || lastsize.y != size.y) { 56 | lastsize = size; 57 | glutInit(&g, &t); 58 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB); 59 | 60 | glutInitWindowPosition(100, 100); 61 | glutInitWindowSize(size.x, size.y); 62 | glutCreateWindow(" "); 63 | } 64 | 65 | glClear(GL_COLOR_BUFFER_BIT); 66 | glRasterPos2i(-1, 1); 67 | glPixelZoom(1, -1); 68 | glPixelStorei(GL_UNPACK_ALIGNMENT, 1); 69 | glPixelStorei(GL_UNPACK_ROW_LENGTH, size.x); 70 | glDrawPixels(size.x, size.y, gl::format, gl::type, scene); 71 | glutSwapBuffers(); 72 | 73 | } 74 | template 75 | void drawthem(A* scene1, B* scene2, C* scene3, D* scene4, E* scene5, F* scene6, 76 | uint2 size_s1, uint2 size_s2, uint2 size_s3, uint2 size_s4, uint2 77 | size_s5, uint2 size_s6) { 78 | static uint2 lastsize = { 0, 0 }; 79 | char * t = (char*) "toto"; 80 | int g = 1; 81 | if (lastsize.x != size_s2.x || lastsize.y != size_s2.y) { 82 | lastsize = size_s2; 83 | glutInit(&g, &t); 84 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB); 85 | glutInitWindowSize(320 * 2, 240 * 3); 86 | // glutInitWindowPosition(100, 100); 87 | 88 | glutCreateWindow("MID-Fusion Display"); 89 | glPixelStorei(GL_UNPACK_ALIGNMENT, 1); 90 | glMatrixMode(GL_PROJECTION); 91 | 92 | gluOrtho2D(0.0, (GLfloat) 640, 0.0, (GLfloat) 720); 93 | glMatrixMode(GL_MODELVIEW); 94 | 95 | } 96 | glClear(GL_COLOR_BUFFER_BIT); 97 | 98 | glRasterPos2i(0, 240); 99 | glPixelZoom(320.0 / size_s1.x, -240.0 / size_s1.y); 100 | glDrawPixels(size_s1.x, size_s1.y, gl::format, gl::type, scene1); 101 | 102 | glRasterPos2i(320, 240); 103 | glPixelZoom(320.0 / size_s2.x, -240.0 / size_s2.y); 104 | glDrawPixels(size_s2.x, size_s2.y, gl::format, gl::type, scene2); 105 | 106 | glRasterPos2i(0, 480); 107 | glPixelZoom(320.0 / size_s3.x, -240.0 / size_s3.y); 108 | glDrawPixels(size_s3.x, size_s3.y, gl::format, gl::type, scene3); 109 | 110 | glRasterPos2i(320, 480); 111 | glPixelZoom(320.0 / size_s4.x, -240.0 / size_s4.y); 112 | glDrawPixels(size_s4.x, size_s4.y, gl::format, gl::type, scene4); 113 | 114 | glRasterPos2i(0, 720); 115 | glPixelZoom(320.0 / size_s5.x, -240.0 / size_s5.y); 116 | glDrawPixels(size_s5.x, size_s5.y, gl::format, gl::type, scene5); 117 | 118 | glRasterPos2i(320, 720); 119 | glPixelZoom(320.0 / size_s6.x, -240.0 / size_s6.y); 120 | glDrawPixels(size_s6.x, size_s6.y, gl::format, gl::type, scene6); 121 | glutSwapBuffers(); 122 | 123 | } 124 | 125 | -------------------------------------------------------------------------------- /apps/kfusion/include/marching_cubes.h: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2011-2013 Gerhard Reitmayr, TU Graz 2 | * Copyright (c) 2013 Jan Jachnik 3 | * 4 | * Permission is hereby granted, free of charge, to any person 5 | * obtaining a copy of this software and associated documentation 6 | * files (the "Software"), to deal in the Software without 7 | * restriction, including without limitation the rights to use, 8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the 10 | * Software is furnished to do so, subject to the following 11 | * conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be 14 | * included in all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 23 | * OTHER DEALINGS IN THE SOFTWARE. 24 | */ 25 | // 26 | // This file is added to the project by hh1013 27 | // 28 | #ifndef MARCHING_CUBES_H 29 | #define MARCHING_CUBES_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | struct triangle 37 | { 38 | float3 vertex[3]; 39 | }; 40 | 41 | struct coloured_vertex{ 42 | float3 pt; 43 | uchar3 col; 44 | 45 | coloured_vertex(float3 _pt, uchar3 _col) 46 | { 47 | pt = _pt; 48 | col = _col; 49 | } 50 | 51 | coloured_vertex(){} 52 | }; 53 | 54 | struct coloured_triangle 55 | { 56 | coloured_vertex vertex[3]; 57 | }; 58 | 59 | 60 | 61 | uint8_t getCubeIndex(int x, int y, int z, const Volume & vol); 62 | float3 LinearInterpolate(uint3 a, uint3 b, const Volume & vol); 63 | coloured_vertex LinearInterpolateColor(uint3 a, uint3 b, const Volume & vol, const RGBVolume & pho); 64 | float3 calcPt(int edge, int x, int y, int z, const Volume & vol); 65 | 66 | void marchingCubes(const Volume vol, std::vector& triangles, 67 | std::string filename); 68 | void marchingCubesColor(const Volume vol, const RGBVolume pho, std::string filename); 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /apps/kfusion/include/tick.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2011-2013 Gerhard Reitmayr, TU Graz 4 | 5 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 6 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 7 | 8 | This code is licensed under the MIT License. 9 | 10 | */ 11 | 12 | #include /* clock_t, clock, CLOCKS_PER_SEC */ 13 | 14 | inline float tick() { 15 | 16 | static struct timeval t; 17 | 18 | struct timeval diff = t; 19 | gettimeofday(&t, NULL); 20 | 21 | return ((t.tv_sec - diff.tv_sec) * 1000000u + t.tv_usec - diff.tv_usec) 22 | / 1.e6; 23 | /* 24 | static clock_t t = clock(); 25 | clock_t diff = clock() - t ; 26 | t = clock(); 27 | return ((float)diff) / (float) CLOCKS_PER_SEC; 28 | */ 29 | } 30 | 31 | -------------------------------------------------------------------------------- /apps/kfusion/moc_OpenGLWidget.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** Meta object code from reading C++ file 'OpenGLWidget.h' 3 | ** 4 | ** Created by: The Qt Meta Object Compiler version 67 (Qt 5.2.1) 5 | ** 6 | ** WARNING! All changes made in this file will be lost! 7 | *****************************************************************************/ 8 | 9 | #include "qt/OpenGLWidget.h" 10 | #include 11 | #include 12 | #if !defined(Q_MOC_OUTPUT_REVISION) 13 | #error "The header file 'OpenGLWidget.h' doesn't include ." 14 | #elif Q_MOC_OUTPUT_REVISION != 67 15 | #error "This file was generated using the moc from 5.2.1. It" 16 | #error "cannot be used with the include files from this version of Qt." 17 | #error "(The moc has changed too much.)" 18 | #endif 19 | 20 | QT_BEGIN_MOC_NAMESPACE 21 | struct qt_meta_stringdata_OpenGLWidget_t { 22 | QByteArrayData data[1]; 23 | char stringdata[14]; 24 | }; 25 | #define QT_MOC_LITERAL(idx, ofs, len) \ 26 | Q_STATIC_BYTE_ARRAY_DATA_HEADER_INITIALIZER_WITH_OFFSET(len, \ 27 | offsetof(qt_meta_stringdata_OpenGLWidget_t, stringdata) + ofs \ 28 | - idx * sizeof(QByteArrayData) \ 29 | ) 30 | static const qt_meta_stringdata_OpenGLWidget_t qt_meta_stringdata_OpenGLWidget = { 31 | { 32 | QT_MOC_LITERAL(0, 0, 12) 33 | }, 34 | "OpenGLWidget\0" 35 | }; 36 | #undef QT_MOC_LITERAL 37 | 38 | static const uint qt_meta_data_OpenGLWidget[] = { 39 | 40 | // content: 41 | 7, // revision 42 | 0, // classname 43 | 0, 0, // classinfo 44 | 0, 0, // methods 45 | 0, 0, // properties 46 | 0, 0, // enums/sets 47 | 0, 0, // constructors 48 | 0, // flags 49 | 0, // signalCount 50 | 51 | 0 // eod 52 | }; 53 | 54 | void OpenGLWidget::qt_static_metacall(QObject *_o, QMetaObject::Call _c, int _id, void **_a) 55 | { 56 | Q_UNUSED(_o); 57 | Q_UNUSED(_id); 58 | Q_UNUSED(_c); 59 | Q_UNUSED(_a); 60 | } 61 | 62 | const QMetaObject OpenGLWidget::staticMetaObject = { 63 | { &QGLWidget::staticMetaObject, qt_meta_stringdata_OpenGLWidget.data, 64 | qt_meta_data_OpenGLWidget, qt_static_metacall, 0, 0} 65 | }; 66 | 67 | 68 | const QMetaObject *OpenGLWidget::metaObject() const 69 | { 70 | return QObject::d_ptr->metaObject ? QObject::d_ptr->dynamicMetaObject() : &staticMetaObject; 71 | } 72 | 73 | void *OpenGLWidget::qt_metacast(const char *_clname) 74 | { 75 | if (!_clname) return 0; 76 | if (!strcmp(_clname, qt_meta_stringdata_OpenGLWidget.stringdata)) 77 | return static_cast(const_cast< OpenGLWidget*>(this)); 78 | return QGLWidget::qt_metacast(_clname); 79 | } 80 | 81 | int OpenGLWidget::qt_metacall(QMetaObject::Call _c, int _id, void **_a) 82 | { 83 | _id = QGLWidget::qt_metacall(_c, _id, _a); 84 | if (_id < 0) 85 | return _id; 86 | return _id; 87 | } 88 | QT_END_MOC_NAMESPACE 89 | -------------------------------------------------------------------------------- /apps/kfusion/qt/MainWindow.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 4 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 5 | 6 | This code is licensed under the MIT License. 7 | 8 | */ 9 | 10 | #ifndef MAINWINDOW_H 11 | #define MAINWINDOW_H 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "OpenGLWidget.h" 22 | 23 | #include 24 | #include 25 | namespace Ui { 26 | class Viewers; 27 | } 28 | class Viewers; 29 | class QCustomPlot; 30 | struct Stream { 31 | FImage image; 32 | PerfStats *statistics; 33 | char *title; 34 | char *help; 35 | bool *active; 36 | int id; 37 | bool allowScaling; 38 | }; 39 | typedef struct Stream Stream; 40 | enum CameraState { 41 | CAMERA_RUNNING, CAMERA_CLOSED, CAMERA_PAUSED, CAMERA_LIVE, UNINITIALISED 42 | }; 43 | 44 | class Viewer: public QWidget { 45 | Q_OBJECT 46 | public: 47 | 48 | Viewer(Stream *stream, int id, QWidget *parent); 49 | ~Viewer(); 50 | void setStream(Stream *stream); 51 | Stream *getStream(); 52 | int getStreamID(); 53 | void setActive(bool value); 54 | bool getActive(); 55 | int getID(); 56 | void pause(); 57 | void resume(); 58 | void update(); 59 | void setZoom(float zoomX, float zoomY); 60 | void setBufferSize(int width, int height); 61 | void setEnableStates(QStringList elements); 62 | QString getOptions(); 63 | QCustomPlot *plot; 64 | QSize sizeHint() const; 65 | void setHoldVariable(bool *variable); 66 | 67 | signals: 68 | // define mouseClick signal 69 | void mouseClickEvent();protected slots: 70 | void mousePressEvent(QMouseEvent * event); 71 | void contextMenuEvent(QContextMenuEvent *event); 72 | void checkBoxChanged(); 73 | void plotWaveSelectionChanged(); 74 | private: 75 | enum MixType { 76 | ALLOW, IGNORE, WARNING 77 | }; 78 | FImage image; 79 | OpenGLWidget *opengl; 80 | MixType allowMixedPlot; 81 | bool mixedPlot; 82 | QCheckBox *checkbox; 83 | QLabel *label; 84 | QPushButton *optionButton; 85 | QWidget *parent; 86 | QGridLayout *layout; 87 | QFrame *frame; 88 | int _id; 89 | bool *holdProcessing; 90 | Viewers *_container; 91 | Stream *_stream; 92 | void setStreamImage(Stream *stream); 93 | void setStreamStats(Stream *stream); 94 | void setOptionMenu(); 95 | double lastTime; 96 | QStringList cfgDisable; 97 | void updateEnableStates(); 98 | int lastFrame; 99 | }; 100 | 101 | class Viewers: public QWidget { //QMainWindow{ 102 | Q_OBJECT 103 | friend class Viewer; 104 | public: 105 | Viewers(QWidget *parent = 0); 106 | ~Viewers(); 107 | void updateImages(); 108 | void setIdleFunction(void (*_callback)()); 109 | void setZoomViewer(int id, float zoomX, float zoomY); 110 | void setBufferSize(int width, int height); 111 | void addViewer(FImage image, const char *title, const char *help, 112 | bool *active); 113 | void addViewer(FImage image, const char *title, const char *help, 114 | bool *active, bool allowScale); 115 | void addViewer(PerfStats *stats, const char *title, const char *help, 116 | bool *active); 117 | void setCurrentViewer(Viewer *viewer); 118 | void pauseStream(int id); 119 | void resumeStream(int id); 120 | void setZoom(float zoomX, float zoomY); 121 | void setZoom(float zoom); 122 | void addDefConfig(int viewer, QString name, bool enabled, 123 | QStringList visible); 124 | void setStatEntry(QString title, std::vector entries, 125 | bool enabled); 126 | QString getConfig(int viewerID); 127 | int getViewerCount(); 128 | PerfStats *getStats(char *); 129 | QMenu *streamMenu; 130 | void reLayout(); 131 | bool holdProcessing();protected slots: 132 | void menuAction( QAction *action); 133 | 134 | private: 135 | QActionGroup *streamMenuActions; 136 | QList viewers; 137 | QList streams; 138 | void addViewer(Stream *); 139 | QGridLayout *layout; 140 | Viewer *_currentViewer; 141 | bool isStreamActive(int id); 142 | bool _holdProcessing; 143 | 144 | }; 145 | 146 | #endif // MAINWINDOW_H 147 | -------------------------------------------------------------------------------- /apps/kfusion/qt/OpenGLWidget.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 4 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 5 | 6 | This code is licensed under the MIT License. 7 | 8 | */ 9 | 10 | #include 11 | #include 12 | #include "OpenGLWidget.h" 13 | #include "MainWindow.h" 14 | #include 15 | OpenGLWidget::OpenGLWidget(FImage image, QWidget *parent) : 16 | QGLWidget(QGLFormat(QGL::SampleBuffers), parent) { 17 | callback = NULL; 18 | video = false; 19 | xRot = 0; 20 | yRot = 0; 21 | zRot = 0; 22 | zoomX = 1; 23 | zoomY = 1; 24 | m_nWidth = image.x; 25 | m_nHeight = image.y; 26 | _3d = false; 27 | m_nCurrWidth = m_nWidth; 28 | m_nCurrHeight = m_nHeight; 29 | setImage(image); 30 | } 31 | void OpenGLWidget::setBufferSize(int width, int height) { 32 | m_nWidth = width; 33 | m_nHeight = height; 34 | //glOrtho(0, m_nWidth, 0, m_nHeight, 0, 1); 35 | //glViewport(0, 0, m_nWidth, m_nHeight); 36 | } 37 | OpenGLWidget::OpenGLWidget(QWidget *parent, int width, int height) : 38 | QGLWidget(QGLFormat(QGL::SampleBuffers), parent) { 39 | callback = NULL; 40 | video = false; 41 | xRot = 0; 42 | yRot = 0; 43 | zRot = 0; 44 | zoomX = 1.0; 45 | zoomY = 1.0; 46 | m_nWidth = width; 47 | m_nHeight = height; 48 | _3d = false; 49 | m_nCurrWidth = m_nWidth; 50 | m_nCurrHeight = m_nHeight; 51 | _parent = parent; 52 | } 53 | void OpenGLWidget::setCallback(void (*_callback)()) { 54 | callback = _callback; 55 | } 56 | void OpenGLWidget::initializeGL() { 57 | glClearColor(0.0f, 0.0f, 0.0f, 0.0f); 58 | glMatrixMode (GL_PROJECTION); 59 | glLoadIdentity(); 60 | glOrtho(0, m_nWidth, 0, m_nHeight, 0, 1); 61 | glMatrixMode (GL_MODELVIEW); 62 | glShadeModel (GL_FLAT); 63 | glPixelStorei(GL_UNPACK_ALIGNMENT, 1); 64 | 65 | } 66 | 67 | void OpenGLWidget::paintGL() { 68 | 69 | glClear (GL_COLOR_BUFFER_BIT); 70 | glPixelZoom(m_nCurrWidth / (double) m_nWidth, 71 | -(m_nCurrHeight / ((double) m_nHeight))); 72 | glRasterPos2i(0, m_nHeight); //m_nHeight *( m_nCurrHeight / ((double) m_nHeight) )); 73 | glDrawPixels(m_nWidth, m_nHeight, pixelFormat, pixelType, pixelData); 74 | } 75 | 76 | void OpenGLWidget::resizeGL(int width, int height) { 77 | glViewport(0, 0, width, height); 78 | m_nCurrWidth = width; 79 | m_nCurrHeight = height; 80 | glPixelZoom(m_nCurrWidth / (double) m_nWidth, 81 | -m_nCurrHeight / (double) m_nHeight); 82 | 83 | //std::cerr << width << "x" << height << " ----- " << m_nWidth << "x"< 14 | struct FImage { 15 | int x; 16 | int y; 17 | int format; 18 | int type; 19 | void *data; 20 | }; 21 | typedef FImage FImage; 22 | 23 | class OpenGLWidget: public QGLWidget { 24 | Q_OBJECT 25 | public: 26 | explicit OpenGLWidget(QWidget *parent = 0, int width = 640, 27 | int height = 480); 28 | OpenGLWidget(FImage image, QWidget *parent = 0); 29 | void setCallback(void (*_callback)()); 30 | void setZoom(float zoomX, float zoomY); 31 | void setImage(FImage image); 32 | void setBufferSize(int width, int height); 33 | void updateImage();signals: 34 | protected: 35 | void initializeGL(); 36 | void paintGL(); 37 | void resizeGL(int width, int height); 38 | //void mousePressEvent(QMouseEvent *event); 39 | //void mouseMoveEvent(QMouseEvent *event); 40 | 41 | private: 42 | QWidget *_parent; 43 | void makeImage(); 44 | void draw(); 45 | QPoint lastPos; 46 | void *pixelData; 47 | int pixelFormat; 48 | int pixelType; 49 | float zoomX; 50 | float zoomY; 51 | int xRot; 52 | int yRot; 53 | int zRot; 54 | bool _3d; 55 | GLubyte* chkImage; 56 | void (*callback)(); 57 | bool video; 58 | int m_nWidth, m_nHeight; 59 | int m_nCurrWidth, m_nCurrHeight;public slots: 60 | 61 | }; 62 | 63 | #endif // OPENGLWIDGET_H 64 | -------------------------------------------------------------------------------- /apps/kfusion/qt/images.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | images/clear24.png 4 | images/rotccw24.png 5 | images/rotcw24.png 6 | images/rotu24.png 7 | images/rotd24.png 8 | images/pause.png 9 | images/play.png 10 | images/step.png 11 | images/restart.png 12 | images/camera.png 13 | images/tripod.png 14 | images/live.png 15 | images/film.png 16 | 17 | 18 | -------------------------------------------------------------------------------- /apps/kfusion/qt/images.qrc.depends: -------------------------------------------------------------------------------- 1 | 2 | 3 | images/clear24.png 4 | images/rotccw24.png 5 | images/rotcw24.png 6 | images/rotu24.png 7 | images/rotd24.png 8 | images/pause.png 9 | images/play.png 10 | images/step.png 11 | images/restart.png 12 | images/camera.png 13 | images/tripod.png 14 | images/live.png 15 | images/film.png 16 | 17 | 18 | -------------------------------------------------------------------------------- /apps/kfusion/qt/images/camera.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/camera.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/clear24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/clear24.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/film.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/film.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/live.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/live.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/pause.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/pause.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/play.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/play.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/restart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/restart.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/rotccw24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/rotccw24.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/rotcw24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/rotcw24.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/rotd24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/rotd24.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/rotu24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/rotu24.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/step.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/step.png -------------------------------------------------------------------------------- /apps/kfusion/qt/images/tripod.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/qt/images/tripod.png -------------------------------------------------------------------------------- /apps/kfusion/qt/licenses/products.txt: -------------------------------------------------------------------------------- 1 | About the Qt Interface 2 | ---------------------- 3 | 4 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 5 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 6 | 7 | The Qt interface is a non critical part of the kfusion application 8 | so the user is at liberty to build it or use the GLUT based version. 9 | 10 | The Qt interface uses third part tools licensed under the following licenses : 11 | - Qt5 Opensource version avaliable from https://qt-project.org/ under LGPL V3 12 | - QCustomPlot avaliable from http://www.qcustomplot.com/ under GPL v3 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /apps/kfusion/scripts/README.md: -------------------------------------------------------------------------------- 1 | # Scripts # 2 | The sripts in this directory are to help with the automation of running tests. 3 | 4 | They provide a python'ic OO interface. 5 | 6 | These scripts assume the datasets are contained in a single directory 7 | E.g. 8 | 9 | * ~/datasets/icl-nuim_living_room_1/... 10 | * ~/datasets/icl-nuim_living_room_2/... 11 | 12 | and so on 13 | 14 | 15 | # Initialisation # 16 | On the first use there is some setup to be performed. 17 | 18 | ## systemsettings.py ## 19 | Setup the correct paths 20 | 21 | * BIN_PATH : Complete path to binaries (probabily $clone_dir$/build 22 | * RESULTS_PATH : Path to store intermediate results 23 | * DATASETS_PATH : Path to folder containing datasets (in the example above this would be ~/datasets) 24 | 25 | 26 | ## datasets.py ## 27 | Most of the settings in here are fixed for a particular dataset. You may need to change the paths to the datasets. The paths are relative to DATASETS_PATH. 28 | 29 | 30 | 31 | # Examples in this directory # 32 | 33 | ## run_lsd_* ## 34 | LSD-SLAM examples: 35 | 36 | * run_lsd_def.py - Default LSD-SLAM settings (as in my report) 37 | * run_lsd_var_kfusage_kfdist.py - LSD-SLAM varying the thresholds for determining when to create a new key-frame 38 | 39 | 40 | 41 | ## run_kf_* ## 42 | KFusion examples: 43 | 44 | * run_kf_def.py - Default KFusion settings (as in my report) 45 | * run_kf_vary_icp.py - KFusion varying the ICP threshold 46 | 47 | 48 | # Using the evaluate stuff # 49 | 50 | For the algorithm (either KFusion or LSD-SLAM) you need to specify how it is going to calculate the ATE. This does depend a little on the dataset. 51 | 52 | ## Pose Optimise using Horn method ## 53 | ``` 54 | #! 55 | lsdslam.pose_optimise = True 56 | ``` 57 | 58 | ## Associate on the identity (True for ICL-NUIM, false for everything else) ## 59 | ``` 60 | #! 61 | lsdslam.ate_associate_identitiy = False 62 | ``` 63 | 64 | ## Do what the original checkPos.py did (True iff using ICL-NUIM) ## 65 | ``` 66 | #! 67 | lsdslam.ate_remove_offset = False 68 | ``` 69 | 70 | ## If using an ICL-NUIM dataset (invert the y axis) ## 71 | ``` 72 | #! 73 | lsdslam.ate_is_icl_nuim = False 74 | ``` -------------------------------------------------------------------------------- /apps/kfusion/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/scripts/__init__.py -------------------------------------------------------------------------------- /apps/kfusion/scripts/_dataset.py: -------------------------------------------------------------------------------- 1 | class Dataset: 2 | 3 | def __init__(self): 4 | self.dataset = None 5 | self.groundtruth = None 6 | self.camera_file = None 7 | self.camera = None 8 | self.quat = None 9 | self.init_pose = None 10 | self.rgb_image = None 11 | self.pre_assoc_file_path = None 12 | self.descr = None 13 | self.maskrcnn_folder = None 14 | -------------------------------------------------------------------------------- /apps/kfusion/scripts/_util.py: -------------------------------------------------------------------------------- 1 | from math import sqrt 2 | 3 | 4 | def golden_section_search(lower, upper, epsilon, func): 5 | """ Do a golden section search for minimum between lower and upper to epsilon precision using func to do evaluation 6 | :param lower: 7 | :param upper: 8 | :param epsilon: 9 | :param func: 10 | :return: 11 | """ 12 | phi = (-1.0 + sqrt(5)) / 2.0 13 | 14 | f_lower = func(lower) 15 | f_upper = func(upper) 16 | 17 | while abs(upper - lower) > epsilon: 18 | a = upper - phi * (upper - lower) 19 | b = lower + phi * (upper - lower) 20 | 21 | f_a = func(a) 22 | f_b = func(b) 23 | 24 | if f_a < f_b: 25 | upper, f_upper = b, f_b 26 | else: 27 | lower, f_lower = a, f_a 28 | 29 | center = (upper + lower) / 2 30 | return center, func(center) 31 | -------------------------------------------------------------------------------- /apps/kfusion/scripts/associate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2013, Juergen Sturm, TUM 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of TUM nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Requirements: 35 | # sudo apt-get install python-argparse 36 | 37 | """ 38 | The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images. 39 | 40 | For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches. 41 | """ 42 | 43 | import argparse 44 | import sys 45 | import os 46 | import numpy 47 | 48 | 49 | def read_file_list(filename): 50 | """ 51 | Reads a trajectory from a text file. 52 | 53 | File format: 54 | The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) 55 | and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 56 | 57 | Input: 58 | filename -- File name 59 | 60 | Output: 61 | dict -- dictionary of (stamp,data) tuples 62 | 63 | """ 64 | file = open(filename) 65 | data = file.read() 66 | lines = data.replace(","," ").replace("\t"," ").split("\n") 67 | list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] 68 | list = [(float(l[0]),l[1:]) for l in list if len(l)>1] 69 | return dict(list) 70 | 71 | def associate(first_list, second_list,offset,max_difference): 72 | """ 73 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 74 | to find the closest match for every input tuple. 75 | 76 | Input: 77 | first_list -- first dictionary of (stamp,data) tuples 78 | second_list -- second dictionary of (stamp,data) tuples 79 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) 80 | max_difference -- search radius for candidate generation 81 | 82 | Output: 83 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) 84 | 85 | """ 86 | first_keys = first_list.keys() 87 | second_keys = second_list.keys() 88 | potential_matches = [(abs(a - (b + offset)), a, b) 89 | for a in first_keys 90 | for b in second_keys 91 | if abs(a - (b + offset)) < max_difference ] 92 | potential_matches.sort() 93 | matches = [] 94 | for diff, a, b in potential_matches: 95 | if a in first_keys and b in second_keys: 96 | first_keys.remove(a) 97 | second_keys.remove(b) 98 | matches.append((a, b)) 99 | 100 | matches.sort() 101 | return matches 102 | 103 | if __name__ == '__main__': 104 | 105 | # parse command line 106 | parser = argparse.ArgumentParser(description=''' 107 | This script takes two data files with timestamps and associates them 108 | ''') 109 | parser.add_argument('first_file', help='first text file (format: timestamp data)') 110 | parser.add_argument('second_file', help='second text file (format: timestamp data)') 111 | parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true') 112 | parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) 113 | parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) 114 | args = parser.parse_args() 115 | 116 | first_list = read_file_list(args.first_file) 117 | second_list = read_file_list(args.second_file) 118 | 119 | matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) 120 | 121 | if args.first_only: 122 | for a,b in matches: 123 | print("%f %s"%(a," ".join(first_list[a]))) 124 | else: 125 | for a,b in matches: 126 | print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b]))) 127 | 128 | 129 | -------------------------------------------------------------------------------- /apps/kfusion/scripts/run_kf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | from _run import * 3 | from systemsettings import * 4 | from datasets import * 5 | import numpy as np 6 | 7 | # TUM_RGB_FR1 = [TUM_RGB_FR1_XYZ, TUM_RGB_FR1_360, TUM_RGB_FR1_FLOOR, TUM_RGB_FR1_PLANT, TUM_RGB_FR1_DESK] 8 | # TUM_RGB_FR2 = [TUM_RGB_FR2_DESK, TUM_RGB_FR2_COKE] 9 | # TUM_RGB_FR3 = [TUM_RGB_FR3_DESK] 10 | # TUM_RGB_FR3_dynamic = [TUM_RGB_FR3_SIT_static, TUM_RGB_FR3_SIT_xyz, TUM_RGB_FR3_SIT_halfsphere, TUM_RGB_FR3_WALK_static, TUM_RGB_FR3_WALK_xyz, TUM_RGB_FR3_WALK_halfsphere] 11 | # ICL = [ICL_NUIM_LIV_0, ICL_NUIM_LIV_1, ICL_NUIM_LIV_2, ICL_NUIM_LIV_3, ICL_NUIM_LIV_1_SHORT] 12 | 13 | TUM_RGB_FR1 = [] 14 | TUM_RGB_FR2 = [] 15 | TUM_RGB_FR3 = [] 16 | # TUM_RGB_FR3_dynamic = [TUM_RGB_FR3_WALK_halfsphere] 17 | # TUM_RGB_FR3_dynamic = [TUM_RGB_FR3_SIT_halfsphere, TUM_RGB_FR3_WALK_xyz, TUM_RGB_FR3_WALK_halfsphere] 18 | TUM_RGB_FR3_dynamic = [TUM_RGB_FR3_SIT_static, TUM_RGB_FR3_SIT_xyz, TUM_RGB_FR3_SIT_halfsphere, TUM_RGB_FR3_WALK_static, TUM_RGB_FR3_WALK_xyz, TUM_RGB_FR3_WALK_halfsphere] 19 | ICL = [] 20 | 21 | if __name__ == "__main__": 22 | results_dir = gen_results_dir(RESULTS_PATH) 23 | algorithm = KinectFusion(BIN_PATH) 24 | # -q --fps 10 --block-read 1 25 | 26 | # Find the best alignment between the gt and the computed trajectory. 27 | # It influences results a lot, we should really discuss this. 28 | algorithm.ate_align = True 29 | 30 | # All must be true for ICL-NUIM 31 | algorithm.ate_associate_identity = False # 1to1 association gt-tra 32 | #algorithm.ate_associate_identity = True # 1to1 association gt-tra 33 | # When true the trajectory is offset by the first position. 34 | algorithm.ate_remove_offset = False 35 | #algorithm.ate_remove_offset = True 36 | algorithm.voxel_block = '8' 37 | algorithm.rendering_rate = '1' 38 | algorithm.bilateralFilter = True 39 | min_ate = 100.0 40 | run_results = {} 41 | # for mu in [0.1, 0.05]: 42 | run_counter = 0 43 | for sequence in ICL + TUM_RGB_FR1 + TUM_RGB_FR2 + TUM_RGB_FR3 + TUM_RGB_FR3_dynamic: 44 | # for sequence in [ICL_NUIM_LIV_2]: 45 | for resol in [512]: 46 | for version in ['openmp']: 47 | kernel_data = [] 48 | mu = 0.075 49 | algorithm.impl = version 50 | algorithm.volume_resolution = str(resol) 51 | algorithm.volume_size = '5' 52 | algorithm.compute_size_ratio = 2 53 | algorithm.integration_rate = 1 54 | algorithm.mu = mu 55 | algorithm.init_pose = sequence.init_pose 56 | algorithm.dump_volume = ".vtk" 57 | res = algorithm.run(sequence) 58 | res['sequence'] = sequence.descr 59 | res['noise_factor'] = mu 60 | run_results[run_counter] = res 61 | run_counter += 1 62 | kernel_data.append(res) 63 | 64 | with open(results_dir + '/resume.log', 'w') as f: 65 | f.write('{:>10}\t{:>10}\t{:>10}\t{:>10}\t{:>10}\t{:>10}\t{:>10}\n'\ 66 | .format('dataset', 'noise_factor', 'ATE', 67 | 'preprocessing', 68 | 'integration', 69 | 'raycasting', 70 | 'computation')) 71 | for k, d in run_results.iteritems(): 72 | data = d['data'] 73 | f.write('{:>10}\t{:>10.4f}\t{:>10.4f}\t{:>10.4f}\t{:>10.4f}\t{:>10.4f}\t{:>10.4f}\n'.format(d['sequence'], 74 | float(d['noise_factor']), 75 | float(d['ate_mean']), 76 | float(data['preprocessing']['mean']), 77 | float(data['integration']['mean']), 78 | float(data['raycasting']['mean']), 79 | float(data['computation']['mean']))) 80 | -------------------------------------------------------------------------------- /apps/kfusion/scripts/systemsettings.py: -------------------------------------------------------------------------------- 1 | BIN_PATH = '../build/' 2 | # RESULTS_PATH = './results/' 3 | RESULTS_PATH = '/media/binbin/data/result/' 4 | DATASETS_PATH = '/media/binbin/data/dataset' 5 | -------------------------------------------------------------------------------- /apps/kfusion/src/PowerMonitor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 4 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 5 | 6 | This code is licensed under the MIT License. 7 | 8 | */ 9 | 10 | #include 11 | #include 12 | #include "PowerMonitor.h" 13 | 14 | PowerMonitor::PowerMonitor() { 15 | powerA7 = NULL; 16 | powerA15 = NULL; 17 | powerGPU = NULL; 18 | powerDRAM = NULL; 19 | sensingMethod = NONE; 20 | if (enableSensor(SENSOR_A7) == 0) { 21 | enableSensor(SENSOR_A15); 22 | enableSensor(SENSOR_GPU); 23 | enableSensor(SENSOR_DRAM); 24 | sensingMethod = ODROID; 25 | std::cerr << "Power sensors: ODROID" << std::endl; 26 | } 27 | } 28 | bool PowerMonitor::isActive() { 29 | if (sensingMethod == NONE) 30 | return (false); 31 | else 32 | return (true); 33 | } 34 | double PowerMonitor::start() { 35 | startTime = powerStats.get_time(); 36 | return (startTime); 37 | } 38 | double PowerMonitor::sample() { 39 | double time = 0; 40 | if (sensingMethod == ODROID) { 41 | double a15 = getPower(SENSOR_A15); 42 | time = powerStats.sample("Sample_time", 43 | powerStats.get_time() - startTime, PerfStats::TIME); 44 | if (powerA7) 45 | powerStats.sample("Power_A7", getPower(SENSOR_A7), 46 | PerfStats::POWER); 47 | if (powerA15) 48 | powerStats.sample("Power_A15", a15, PerfStats::POWER); 49 | if (powerGPU) 50 | powerStats.sample("Power_GPU", getPower(SENSOR_GPU), 51 | PerfStats::POWER); 52 | if (powerDRAM) 53 | powerStats.sample("Power_DRAM", getPower(SENSOR_DRAM), 54 | PerfStats::POWER); 55 | } 56 | return (time); 57 | } 58 | PowerMonitor::~PowerMonitor() { 59 | if (powerA7) 60 | fclose(powerA7); 61 | if (powerA15) 62 | fclose(powerA15); 63 | if (powerGPU) 64 | fclose(powerGPU); 65 | if (powerDRAM) 66 | fclose(powerDRAM); 67 | } 68 | 69 | float PowerMonitor::getPower(Sensor sensor) { 70 | FILE *tmp; 71 | float power; 72 | if (sensingMethod == ODROID) { 73 | switch (sensor) { 74 | case SENSOR_A7: 75 | tmp = powerA7; 76 | break; 77 | case SENSOR_A15: 78 | tmp = powerA15; 79 | break; 80 | case SENSOR_GPU: 81 | tmp = powerGPU; 82 | break; 83 | case SENSOR_DRAM: 84 | tmp = powerDRAM; 85 | break; 86 | } 87 | if (tmp) { 88 | rewind(tmp); 89 | int res = fscanf(tmp, "%f\n", &power); 90 | return (power); 91 | } 92 | } 93 | 94 | } 95 | 96 | int PowerMonitor::enableSensor(Sensor sensor) { 97 | char enableFile[256]; 98 | FILE *tmp; 99 | bool done = false; 100 | for (int dn = 1; dn < 5; dn++) { 101 | sprintf(enableFile, "/sys/bus/i2c/drivers/INA231/%d-00%d/enable", dn, 102 | sensor); 103 | 104 | if (tmp = fopen(enableFile, "a")) { 105 | fprintf(tmp, "1\n"); 106 | fclose(tmp); 107 | 108 | sprintf(enableFile, "/sys/bus/i2c/drivers/INA231/%d-00%d/sensor_W", 109 | dn, sensor); 110 | if (tmp = fopen(enableFile, "r")) { 111 | switch (sensor) { 112 | case SENSOR_A7: 113 | powerA7 = tmp; 114 | break; 115 | case SENSOR_A15: 116 | powerA15 = tmp; 117 | break; 118 | case SENSOR_GPU: 119 | powerGPU = tmp; 120 | break; 121 | case SENSOR_DRAM: 122 | powerDRAM = tmp; 123 | break; 124 | } 125 | return (0); 126 | } 127 | } 128 | } 129 | return (1); 130 | } 131 | 132 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/bfusion/rendering_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | #ifndef BFUSION_RENDERING_IMPL_H 9 | #define BFUSION_RENDERING_IMPL_H 10 | 11 | #include 12 | #include 13 | inline float4 raycast(const Volume& volume, const uint2 pos, 14 | const Matrix4 view, const float nearPlane, 15 | const float farPlane, const float, const float step, 16 | const float) 17 | { 18 | const float3 origin = get_translation(view); 19 | const float3 direction = normalize(rotate(view, make_float3(pos.x, pos.y, 1.f))); 20 | 21 | // intersect ray with a box 22 | // http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm 23 | // compute intersection of ray with all six bbox planes 24 | const float3 invR = make_float3(1.0f) / direction; 25 | const float3 tbot = -1 * invR * origin; 26 | const float3 ttop = invR * (volume._size - origin); 27 | 28 | // re-order intersections to find smallest and largest on each axis 29 | const float3 tmin = fminf(ttop, tbot); 30 | const float3 tmax = fmaxf(ttop, tbot); 31 | 32 | // find the largest tmin and the smallest tmax 33 | const float largest_tmin = fmaxf(fmaxf(tmin.x, tmin.y), 34 | fmaxf(tmin.x, tmin.z)); 35 | const float smallest_tmax = fminf(fminf(tmax.x, tmax.y), 36 | fminf(tmax.x, tmax.z)); 37 | 38 | // check against near and far plane 39 | const float tnear = fmaxf(largest_tmin, nearPlane); 40 | const float tfar = fminf(smallest_tmax, farPlane); 41 | auto select_occupancy = [](const auto& val){ return val.x; }; 42 | 43 | if (tnear < tfar) { 44 | float t = tnear; 45 | float stepsize = step; 46 | float f_t = volume.interp(origin + direction * t, select_occupancy); 47 | float f_tt = 0; 48 | 49 | // if we are not already in it 50 | if (f_t <= SURF_BOUNDARY) { 51 | for (; t < tfar; t += stepsize) { 52 | Volume::compute_type data = volume.get(origin + direction * t); 53 | if(data.x > -100.f && data.y > 0.f){ 54 | f_tt = volume.interp(origin + direction * t, select_occupancy); 55 | } 56 | if (f_tt > SURF_BOUNDARY) break; 57 | f_t = f_tt; 58 | } 59 | if (f_tt > SURF_BOUNDARY) { 60 | // got it, calculate accurate intersection 61 | t = t - stepsize * (f_tt - SURF_BOUNDARY) / (f_tt - f_t); 62 | return make_float4(origin + direction * t, t); 63 | } 64 | } 65 | } 66 | return make_float4(0); 67 | } 68 | #endif //BFUSION_RENDERING_IMPL_H 69 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/continuous/volume_instance.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * 3 | * Copyright 2016 Emanuele Vespa, Imperial College London 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * */ 31 | 32 | #ifndef VOLUME_INSTANCE_HPP 33 | #define VOLUME_INSTANCE_HPP 34 | #include "volume_template.hpp" 35 | #include "volume_traits.hpp" 36 | #include "octree.hpp" 37 | 38 | /****************************************************************************** 39 | * 40 | * Volume Class Instance 41 | * 42 | ******************************************************************************/ 43 | 44 | 45 | #ifndef STORAGE_LAYOUT 46 | #define STORAGE_LAYOUT DynamicStorage 47 | #endif 48 | 49 | #ifndef INDEX_STRUCTURE 50 | #define INDEX_STRUCTURE Octree 51 | #endif 52 | 53 | //typedef BFusion FieldType; 54 | typedef SDF FieldType; 55 | 56 | template 57 | using Volume = VolumeTemplate; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/continuous/volume_template.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * 3 | * Copyright 2016 Emanuele Vespa, Imperial College London 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * */ 31 | 32 | #ifndef VOLUME_TEMPLATE_H 33 | #define VOLUME_TEMPLATE_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | template 42 | class Void {}; 43 | 44 | template class Indexer> 45 | class VolumeTemplate {}; 46 | 47 | /** 48 | * Continuous volume abstraction 49 | * Sparse, dynamically allocated storage accessed through the 50 | * appropriate indexer (octree/hash table). 51 | * */ 52 | template class Indexer> 53 | class VolumeTemplate { 54 | 55 | public: 56 | typedef voxel_traits traits_type; 57 | typedef typename traits_type::ComputeType compute_type; 58 | typedef typename traits_type::StoredType stored_type; 59 | typedef FieldType field_type; 60 | 61 | void init(uint resol, float size) { 62 | _resol = resol; 63 | _size = size; 64 | _map_index.init(_resol, _size); 65 | } 66 | 67 | void release(){}; 68 | 69 | inline float3 pos(const uint3 & p) const { 70 | static const float voxelSize = _size/_resol; 71 | return make_float3(p.x * voxelSize, p.y * voxelSize, p.z * voxelSize); 72 | } 73 | 74 | void set(const uint3 & , const compute_type& ) {} 75 | 76 | compute_type operator[](const float3 & p) const { 77 | const float inverseVoxelSize = _resol/_size; 78 | const int3 scaled_pos = make_int3(make_float3((p.x * inverseVoxelSize), 79 | (p.y * inverseVoxelSize), (p.z * inverseVoxelSize))); 80 | return _map_index.get(scaled_pos.x, scaled_pos.y, scaled_pos.z); 81 | } 82 | 83 | compute_type get(const float3 & p) const { 84 | const float inverseVoxelSize = _resol/_size; 85 | const int3 scaled_pos = make_int3(make_float3((p.x * inverseVoxelSize), 86 | (p.y * inverseVoxelSize), (p.z * inverseVoxelSize))); 87 | return _map_index.get_fine(scaled_pos.x, scaled_pos.y, scaled_pos.z); 88 | } 89 | 90 | compute_type operator[](const uint3 p) const { 91 | return _map_index.get(p.x, p.y, p.z); 92 | } 93 | 94 | template 95 | float interp(const float3 & pos, FieldSelector select) const { 96 | const float inverseVoxelSize = _resol / _size; 97 | const float3 scaled_pos = make_float3((pos.x * inverseVoxelSize), 98 | (pos.y * inverseVoxelSize), 99 | (pos.z * inverseVoxelSize)); 100 | return _map_index.interp(scaled_pos, select); 101 | } 102 | 103 | template 104 | float3 grad(const float3 & pos, FieldSelector select) const { 105 | 106 | const float inverseVoxelSize = _resol / _size; 107 | const float3 scaled_pos = make_float3((pos.x * inverseVoxelSize), 108 | (pos.y * inverseVoxelSize), 109 | (pos.z * inverseVoxelSize)); 110 | return _map_index.grad(scaled_pos, select); 111 | } 112 | 113 | unsigned int _resol; 114 | float _size; 115 | std::vector _allocationList; 116 | Indexer _map_index; 117 | 118 | private: 119 | 120 | inline uint3 pos(const float3 & p) const { 121 | static const float inverseVoxelSize = _resol/_size; 122 | return make_uint3(p.x * inverseVoxelSize, p.y * inverseVoxelSize, 123 | p.z * inverseVoxelSize); 124 | } 125 | }; 126 | #endif 127 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/continuous/volume_traits.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | 9 | #ifndef VOLUME_H 10 | #define VOLUME_H 11 | 12 | // Data types definitions 13 | #include "voxel_traits.hpp" 14 | 15 | /****************************************************************************** 16 | * 17 | * KFusion Truncated Signed Distance Function voxel traits 18 | * 19 | ****************************************************************************/ 20 | 21 | class SDF {}; 22 | template<> 23 | struct voxel_traits { 24 | // typedef float2 ComputeType; 25 | // typedef float2 StoredType; 26 | typedef struct ComputeType { 27 | float x; //tsdf 28 | float y; //weight 29 | float r; //color information: R 30 | float g; //color information: G 31 | float b; //color information: B 32 | int c; //color votes 33 | double fg; //foreground probability 34 | int w; //foreground witness 35 | } ComputeType; 36 | typedef ComputeType StoredType; 37 | // static inline ComputeType empty(){ return make_float2(1.f, -1.f); } 38 | static inline ComputeType empty(){ return {1.f, -1.f, 0.f, 0.f, 0.f, 0, 0.f, 0}; } 39 | // static inline StoredType initValue(){ return make_float2(1.f, 0.f); } 40 | static inline StoredType initValue(){ return {1.f, 0.f, 0.f, 0.f, 0.f, 0, 0.f, 0}; } 41 | static inline ComputeType translate(const StoredType value){ 42 | // return make_float2(value.x, value.y); 43 | return value; 44 | } 45 | }; 46 | 47 | /****************************************************************************** 48 | * 49 | * Bayesian Fusion voxel traits and algorithm specificic defines 50 | * 51 | ****************************************************************************/ 52 | 53 | class BFusion {}; 54 | template<> 55 | struct voxel_traits { 56 | // typedef struct ComputeType { 57 | // float x; 58 | // double y; 59 | // } ComputeType; 60 | typedef struct ComputeType { 61 | float x; 62 | double y; 63 | float r; //color information: R 64 | float g; //color information: G 65 | float b; //color information: B 66 | int c; //color votes 67 | double fg; //foreground probability 68 | int w; //foreground witness 69 | } ComputeType; 70 | typedef ComputeType StoredType; 71 | // static inline ComputeType empty(){ return {0.f, 0.f}; } 72 | // static inline StoredType initValue(){ return {0.f, 0.f}; } 73 | static inline ComputeType empty(){ return {0.f, 0.f, 0.f, 0.f, 0.f, 0, 0.f, 0}; } 74 | static inline StoredType initValue(){ return {0.f, 0.f, 0.f, 0.f, 0.f, 0, 0.f, 0}; } 75 | static inline StoredType translate(const ComputeType value) { 76 | return value; 77 | } 78 | }; 79 | 80 | // Windowing parameters 81 | #define DELTA_T 1.f 82 | #define CAPITAL_T 4.f 83 | 84 | #define INTERP_THRESH 0.05f 85 | #define SURF_BOUNDARY 0.f 86 | #define TOP_CLAMP 1000.f 87 | #define BOTTOM_CLAMP (-TOP_CLAMP) 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/kfusion/mapping_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | 9 | #ifndef KFUSION_MAPPING_HPP 10 | #define KFUSION_MAPPING_HPP 11 | #include 12 | #include "../continuous/volume_traits.hpp" 13 | #include 14 | 15 | struct sdf_update { 16 | 17 | template 18 | void operator()(DataHandlerT& handler, const int3&, const float3& pos, 19 | const float2& pixel) { 20 | 21 | const uint2 px = make_uint2(pixel.x, pixel.y); 22 | const float depthSample = depth[px.x + depthSize.x*px.y]; 23 | if (depthSample <= 0 || mask_.at(px.y, px.x) < 0) return; 24 | const float diff = (depthSample - pos.z) 25 | * std::sqrt( 1 + sq(pos.x / pos.z) + sq(pos.y / pos.z)); 26 | if (diff > -mu) { 27 | const float sdf = fminf(1.f, diff / mu); 28 | auto data = handler.get(); 29 | // tsdf 30 | data.x = clamp((data.y * data.x + sdf) / (data.y + 1), -1.f, 31 | 1.f); 32 | // weight 33 | data.y = fminf(data.y + 1, maxweight); 34 | 35 | //color information 36 | const float3 rgb_measured = rgb_[px.x + depthSize.x*px.y]; 37 | data.r = (rgb_measured.x + data.r * data.c)/(data.c + 1); 38 | data.g = (rgb_measured.y + data.g * data.c)/(data.c + 1); 39 | data.b = (rgb_measured.z + data.b * data.c)/(data.c + 1); 40 | data.c += 1; 41 | 42 | //foreground probability 43 | const float fg_measured = mask_.at(px.y, px.x); 44 | data.fg = (fg_measured + data.fg * data.w)/(data.w + 1); 45 | data.w ++; 46 | 47 | handler.set(data); 48 | } 49 | } 50 | 51 | //grey version 52 | sdf_update(const float * d, const uint2 framesize, float m, int mw) : 53 | depth(d), rgb_(nullptr), depthSize(framesize), mu(m), maxweight(mw){}; 54 | 55 | //color version 56 | sdf_update(const float *d, const float3 *rgb, const uint2 framesize, float m, 57 | int mw) : 58 | depth(d), rgb_(rgb), depthSize(framesize), mu(m), maxweight(mw){}; 59 | 60 | //mask color version 61 | sdf_update(const float *d, const float3 *rgb, const cv::Mat& mask, 62 | const uint2 framesize, float m, int mw) : 63 | depth(d), rgb_(rgb), mask_(mask), depthSize(framesize), mu(m), 64 | maxweight(mw){}; 65 | 66 | const float * depth; 67 | const float3 * rgb_; 68 | const cv::Mat mask_; 69 | uint2 depthSize; 70 | float mu; 71 | int maxweight; 72 | }; 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/kfusion/rendering_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | 9 | #ifndef KFUSION_RENDERING_IMPL_H 10 | #define kFUSION_RENDERING_IMPL_H 11 | 12 | #include 13 | #include 14 | 15 | inline float4 raycast(const Volume& volume, const uint2 pos, 16 | const Matrix4 view, const float nearPlane, 17 | const float farPlane, const float mu, 18 | const float step, const float largestep) { 19 | 20 | const float3 origin = get_translation(view); 21 | const float3 direction = normalize(rotate(view, make_float3(pos.x, pos.y, 1.f))); 22 | 23 | // intersect ray with a box 24 | // http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm 25 | // compute intersection of ray with all six bbox planes 26 | const float3 invR = make_float3(1.0f) / direction; 27 | const float3 tbot = -1 * invR * origin; 28 | const float3 ttop = invR * (volume._size - origin); 29 | 30 | // re-order intersections to find smallest and largest on each axis 31 | const float3 tmin = fminf(ttop, tbot); 32 | const float3 tmax = fmaxf(ttop, tbot); 33 | 34 | // find the largest tmin and the smallest tmax 35 | const float largest_tmin = fmaxf(fmaxf(tmin.x, tmin.y), 36 | fmaxf(tmin.x, tmin.z)); 37 | const float smallest_tmax = fminf(fminf(tmax.x, tmax.y), 38 | fminf(tmax.x, tmax.z)); 39 | 40 | // check against near and far plane 41 | const float tnear = fmaxf(largest_tmin, nearPlane); 42 | const float tfar = fminf(smallest_tmax, farPlane); 43 | auto select_depth = [](const auto& val){ return val.x; }; 44 | 45 | if (tnear < tfar) { 46 | // first walk with largesteps until we found a hit 47 | float t = tnear; 48 | float stepsize = largestep; 49 | float3 position = origin + direction * t; 50 | float f_t = volume.interp(position, select_depth); 51 | float f_tt = 0; 52 | if (f_t > 0) { // ups, if we were already in it, then don't render anything here 53 | for (; t < tfar; t += stepsize) { 54 | Volume::compute_type data = volume.get(position); 55 | if(data.y < 0){ 56 | stepsize = largestep; 57 | position += stepsize*direction; 58 | continue; 59 | } 60 | f_tt = data.x; 61 | if(f_tt <= 0.1 && f_tt >= -0.5f){ 62 | f_tt = volume.interp(position, select_depth); 63 | } 64 | if (f_tt < 0) // got it, jump out of inner loop 65 | break; 66 | stepsize = fmaxf(f_tt * mu, step); 67 | position += stepsize*direction; 68 | //stepsize = step; 69 | f_t = f_tt; 70 | } 71 | if (f_tt < 0) { // got it, calculate accurate intersection 72 | t = t + stepsize * f_tt / (f_t - f_tt); 73 | return make_float4(origin + direction * t, t); 74 | } 75 | } 76 | } 77 | return make_float4(0); 78 | } 79 | 80 | inline float4 raycast(const Volume& volume, const float3 origin, 81 | const float3 direction, const float nearPlane, 82 | const float farPlane, const float mu, 83 | const float step, const float largestep) { 84 | 85 | // intersect ray with a box 86 | // http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm 87 | // compute intersection of ray with all six bbox planes 88 | const float3 invR = make_float3(1.0f) / direction; 89 | const float3 tbot = -1 * invR * origin; 90 | const float3 ttop = invR * (volume._size - origin); 91 | 92 | // re-order intersections to find smallest and largest on each axis 93 | const float3 tmin = fminf(ttop, tbot); 94 | const float3 tmax = fmaxf(ttop, tbot); 95 | 96 | // find the largest tmin and the smallest tmax 97 | const float largest_tmin = fmaxf(fmaxf(tmin.x, tmin.y), 98 | fmaxf(tmin.x, tmin.z)); 99 | const float smallest_tmax = fminf(fminf(tmax.x, tmax.y), 100 | fminf(tmax.x, tmax.z)); 101 | 102 | // check against near and far plane 103 | const float tnear = fmaxf(largest_tmin, nearPlane); 104 | const float tfar = fminf(smallest_tmax, farPlane); 105 | auto select_depth = [](const auto& val){ return val.x; }; 106 | 107 | if (tnear < tfar) { 108 | // first walk with largesteps until we found a hit 109 | float t = tnear; 110 | float stepsize = largestep; 111 | float3 position = origin + direction * t; 112 | float f_t = volume.interp(position, select_depth); 113 | float f_tt = 0; 114 | if (f_t > 0) { // ups, if we were already in it, then don't render anything here 115 | for (; t < tfar; t += stepsize) { 116 | Volume::compute_type data = volume.get(position); 117 | if(data.y < 0){ 118 | stepsize = largestep; 119 | position += stepsize*direction; 120 | continue; 121 | } 122 | f_tt = data.x; 123 | if(f_tt <= 0.1 && f_tt >= -0.5f){ 124 | f_tt = volume.interp(position, select_depth); 125 | } 126 | if (f_tt < 0) // got it, jump out of inner loop 127 | break; 128 | stepsize = fmaxf(f_tt * mu, step); 129 | position += stepsize*direction; 130 | //stepsize = step; 131 | f_t = f_tt; 132 | } 133 | if (f_tt < 0) { // got it, calculate accurate intersection 134 | t = t + stepsize * f_tt / (f_t - f_tt); 135 | return make_float4(origin + direction * t, t); 136 | } 137 | } 138 | } 139 | return make_float4(0); 140 | } 141 | 142 | 143 | #endif //KFUSION_RENDERING_IMPL_H 144 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/mapping.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef MAPPING_HPP 33 | #define MAPPING_HPP 34 | #include 35 | #include 36 | 37 | #include "bfusion/mapping_impl.hpp" 38 | #include "kfusion/mapping_impl.hpp" 39 | 40 | template 41 | void integratePass(VoxelBlock ** blockList, unsigned int list_size, 42 | const float * depth, uint2 depthSize, const float voxelSize, 43 | const Matrix4 invTrack, const Matrix4 K, const float mu, 44 | const float , const double timestamp) { 45 | 46 | #pragma omp parallel for 47 | for(unsigned int i = 0; i < list_size; ++i){ 48 | integrate(blockList[i], depth, depthSize, voxelSize, 49 | invTrack, K, mu, timestamp); 50 | // blockList[i]->timestamp(current_frame); 51 | } 52 | } 53 | 54 | /* 55 | * MemoryBufferType is an instance of the memory allocator class 56 | */ 57 | template 58 | void integratePass(MemoryBufferType& nodes_list, unsigned int list_size, 59 | UpdateFunctor f) { 60 | 61 | #pragma omp parallel for 62 | for(unsigned int i = 0; i < list_size; ++i){ 63 | f(nodes_list[i]); 64 | } 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/object/object.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 3 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 4 | * SPDX-License-Identifier: BSD-3-Clause 5 | */ 6 | 7 | 8 | #ifndef OFUSION_OBJECT_H 9 | #define OFUSION_OBJECT_H 10 | 11 | #include "vector_types.h" 12 | #include 13 | #include "math_utils.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "../continuous/volume_instance.hpp" 19 | 20 | #include "../bfusion/mapping_impl.hpp" 21 | #include "../kfusion/mapping_impl.hpp" 22 | #include "../bfusion/alloc_impl.hpp" 23 | #include "../kfusion/alloc_impl.hpp" 24 | 25 | #include "opencv2/opencv.hpp" 26 | 27 | class Object; 28 | typedef std::shared_ptr ObjectPointer; 29 | typedef std::vector ObjectList; 30 | typedef ObjectList::iterator ObjectListIterator; 31 | 32 | 33 | class Object{ 34 | private: 35 | // volume 36 | int voxel_block_size_ = 8; 37 | float3 volume_size_ = make_float3(10);//dimensions=size 38 | uint3 volume_resol_ = make_uint3(1024);//resolutions 39 | float volume_step = min(volume_size_) / max(volume_resol_); 40 | bool isStatic_; 41 | 42 | // SegmentationResult; 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | //initilisation 46 | // Object(const Matrix4& pose, const int id, const uint2 imgSize); 47 | 48 | Object(const int voxel_block_size, const float3& volume_size, const uint3& 49 | volume_resol, const Matrix4& pose, const Matrix4& virtual_T_w_c, 50 | const int class_id, const All_Prob_Vect& all_prob, 51 | const uint2 imgSize); 52 | 53 | void set_volume_size(const float3& volume_size){ 54 | this->volume_size_ = volume_size; 55 | }; 56 | 57 | float3 get_volume_size(){ 58 | return this->volume_size_; 59 | }; 60 | 61 | uint3 get_volume_resol(){ 62 | return this->volume_resol_; 63 | }; 64 | 65 | float get_voxel_size(){ 66 | return volume_size_.x/volume_resol_.x; 67 | } 68 | 69 | float get_volume_step(){ 70 | return volume_step; 71 | } 72 | 73 | /** 74 | * @brief integrate a depth image into volume, based on camera pose T_w_c 75 | * @param depthImage [in] input depth image 76 | * @param rgbImage [in] input rgb image 77 | * @param imgSize [in] image size 78 | * @param T_w_c [in] camera pose 79 | * @param mu [in] TSDF mu 80 | * @param k [in] intrinsic matrix 81 | * @param frame [in] frame number for bfusion integration 82 | */ 83 | void integration_static_kernel(const float * depthImage, const float3*rgbImage, 84 | const uint2& imgSize, const Matrix4& T_w_c, 85 | const float mu, const float4& k, const uint frame); 86 | 87 | /** 88 | * @brief integrate a depth image into volume, based on camera pose T_w_c, and 89 | * the estimated volume pose T_w_o 90 | * @param depthImage [in] input depth image 91 | * @param rgbImage [in] input rgb image 92 | * @param mask [in] segmentation mask corresponding this volume instance 93 | * @param imgSize [in] image size 94 | * @param T_w_c [in] camera pose 95 | * @param mu [in] TSDF mu 96 | * @param k [in] intrinsic matrix 97 | * @param frame [in] frame number for bfusion integration 98 | */ 99 | void integrate_volume_kernel(const float * depthImage, const float3*rgbImage, 100 | const cv::Mat& mask, const uint2& imgSize, 101 | const Matrix4& T_w_c, const float mu, 102 | const float4& k, const uint frame); 103 | 104 | void fuse_semantic_kernel(const SegmentationResult& segmentationResult, 105 | const int instance_id); 106 | 107 | void fuseSemanticLabel(const instance_seg& input_seg); 108 | 109 | void refine_mask_use_motion(cv::Mat& mask, const bool use_icp, 110 | const bool use_rgb); 111 | 112 | void set_static_state(const bool state); 113 | 114 | bool is_static() const; 115 | 116 | virtual ~Object(); 117 | 118 | public: 119 | //octree-based volume representation 120 | Volume volume_; 121 | octlib::key_t* allocationList_ = nullptr; 122 | size_t reserved_ = 0; 123 | 124 | //vertex and normal belonging to this volumne 125 | float3 * m_vertex; 126 | float3 * m_normal; 127 | 128 | //vertex and normal before intergration, belonging to this volumne 129 | float3 * m_vertex_bef_integ; 130 | float3 * m_normal_bef_integ; 131 | 132 | //pose 133 | Matrix4 volume_pose_; 134 | 135 | //virtual camera pose 136 | Matrix4 virtual_camera_pose_; 137 | 138 | //labels 139 | int class_id_; 140 | int instance_label_; 141 | 142 | //semanticfusion 143 | All_Prob_Vect semanticfusion_ = All_Prob_Vect::Zero(); 144 | int fused_time_; 145 | static const uint label_size_ = 80; 146 | 147 | //Tracking result for this frame 148 | TrackData* trackresult_; 149 | 150 | static const std::set static_object; 151 | bool pose_saved_; 152 | 153 | static bool absorb_outlier_bg; 154 | }; 155 | 156 | 157 | 158 | 159 | #endif //OFUSION_OBJECT_H 160 | -------------------------------------------------------------------------------- /apps/kfusion/src/cpp/preprocessing/preprocessing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 3 | * SPDX-FileCopyrightText: 2017-2019 Smart Robotics Lab, Imperial College London 4 | * SPDX-FileCopyrightText: 2017-2019 Binbin Xu 5 | * SPDX-License-Identifier: BSD-3-Clause 6 | */ 7 | 8 | 9 | #ifndef PREPROCESSING_H 10 | #define PREPROCESSING_H 11 | 12 | #include "timings.h" 13 | #include 14 | #include 15 | #include "commons.h" 16 | #include "../continuous/volume_instance.hpp" 17 | 18 | 19 | void bilateralFilterKernel(float* out, const float* in, uint2 size, 20 | const float * gaussian, float e_d, int r); 21 | 22 | void depth2vertexKernel(float3* vertex, const float * depth, uint2 imageSize, 23 | const Matrix4 invK); 24 | 25 | void rgb2intensity(float * grey_out, float3 *rgb_out, uint2 outSize, const uchar3 * in, 26 | uint2 inSize); 27 | 28 | void rgb2intensity(float * out, uint2 outSize, const uchar3 * in, 29 | uint2 inSize); 30 | 31 | void mm2metersKernel(float * out, uint2 outSize, const ushort * in, 32 | uint2 inSize); 33 | 34 | void halfSampleRobustImageKernel(float* out, const float* in, uint2 inSize, 35 | const float e_d, const int r); 36 | 37 | void gradientsKernel(float * gradx, float * grady, const float * in, uint2 size); 38 | 39 | template 40 | void vertex2normalKernel(float3 * out, const float3 * in, uint2 imageSize) { 41 | TICK(); 42 | unsigned int x, y; 43 | #pragma omp parallel for \ 44 | shared(out), private(x,y) 45 | for (y = 0; y < imageSize.y; y++) { 46 | for (x = 0; x < imageSize.x; x++) { 47 | const uint2 pleft = make_uint2(max(int(x) - 1, 0), y); 48 | const uint2 pright = make_uint2(min(x + 1, (int) imageSize.x - 1), 49 | y); 50 | 51 | // Swapped to match the left-handed coordinate system of ICL-NUIM 52 | uint2 pup, pdown; 53 | if(NegY) { 54 | pup = make_uint2(x, max(int(y) - 1, 0)); 55 | pdown = make_uint2(x, min(y + 1, ((int) imageSize.y) - 1)); 56 | } else { 57 | pdown = make_uint2(x, max(int(y) - 1, 0)); 58 | pup = make_uint2(x, min(y + 1, ((int) imageSize.y) - 1)); 59 | } 60 | 61 | const float3 left = in[pleft.x + imageSize.x * pleft.y]; 62 | const float3 right = in[pright.x + imageSize.x * pright.y]; 63 | const float3 up = in[pup.x + imageSize.x * pup.y]; 64 | const float3 down = in[pdown.x + imageSize.x * pdown.y]; 65 | 66 | if (left.z == 0 || right.z == 0 || up.z == 0 || down.z == 0) { 67 | out[x + y * imageSize.x].x = INVALID; 68 | continue; 69 | } 70 | const float3 dxv = right - left; 71 | const float3 dyv = up - down; 72 | if(std::is_same::value) { 73 | out[x + y * imageSize.x] = normalize(cross(dyv, dxv)); // switched dx and dy to get factor -1 74 | } 75 | else if(std::is_same::value) { 76 | out[x + y * imageSize.x] = normalize(cross(dxv, dyv)); // switched dx and dy to get factor -1 77 | } 78 | } 79 | } 80 | TOCK("vertex2normalKernel", imageSize.x * imageSize.y); 81 | } 82 | 83 | void vertex2depth (float *rendered_depth, const float3 *vertex, const uint2 inputSize, const Matrix4& pose); 84 | 85 | Eigen::Quaternionf getQuaternion(const Matrix4& Trans); 86 | 87 | #endif //PREPROCESSING_H 88 | -------------------------------------------------------------------------------- /apps/kfusion/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kfusion_unit_testing) 3 | 4 | # GTest Root - Change to reflect your install dir 5 | #set(GTEST_ROOT ~/software/googletest/googletest) 6 | find_package(GTest REQUIRED) 7 | find_package( OpenCV REQUIRED ) 8 | 9 | set(TOON_INCLUDE_PATH ../TooN/install_dir/include) 10 | 11 | set(OCTREELIB_ROOT ../../../) 12 | set(KFUSION_ROOT ../) 13 | 14 | enable_testing() 15 | add_compile_options(-std=c++1y ) 16 | include_directories(${KFUSION_ROOT}/include ${KFUSION_ROOT}/kfusion/src 17 | ${KFUSION_ROOT}/thirdparty ${OCTREELIB_ROOT}/utils ${OCTREELIB_ROOT}/lib 18 | ${TOON_INCLUDE_PATH} ${OpenCV_INCLUDE_DIRS}) 19 | 20 | add_subdirectory(preprocessing) 21 | add_subdirectory(tracking) 22 | add_subdirectory(segmentation) 23 | -------------------------------------------------------------------------------- /apps/kfusion/test/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | mkdir -p build/ 3 | cd build/ && cmake .. -DCMAKE_BUILD_TYPE=Release 4 | $(MAKE) -C build 5 | 6 | debug: 7 | mkdir -p build/ 8 | cd build/ && cmake .. -DCMAKE_BUILD_TYPE=Debug 9 | $(MAKE) -C build 10 | 11 | test: 12 | $(MAKE) -C build test 13 | 14 | clean: 15 | rm -rf build/ ./CMakeFiles 16 | 17 | .PHONY: all clean test 18 | 19 | -------------------------------------------------------------------------------- /apps/kfusion/test/callSegmentationUnitTest: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #cd ../../ 4 | #make 5 | 6 | cd build/segmentation/ 7 | make 8 | ./segmentation-unittest 9 | -------------------------------------------------------------------------------- /apps/kfusion/test/callTrackingUnitTest: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #cd ../../ 4 | #make 5 | 6 | cd build/tracking/ 7 | make 8 | ./tracking-jacobian-unittest 9 | -------------------------------------------------------------------------------- /apps/kfusion/test/preprocessing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(octree_lib) 3 | 4 | set(PROJECT_TEST_NAME preprocessing) 5 | set(UNIT_TEST_NAME ${PROJECT_TEST_NAME}-interp-unittest) 6 | add_executable(${UNIT_TEST_NAME} interp_unittest.cpp) 7 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 8 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 9 | target_compile_definitions(${UNIT_TEST_NAME} PRIVATE FILEPATH="${CMAKE_CURRENT_LIST_DIR}") 10 | 11 | gtest_add_tests(TARGET ${UNIT_TEST_NAME}) 12 | -------------------------------------------------------------------------------- /apps/kfusion/test/preprocessing/interp_unittest.cpp: -------------------------------------------------------------------------------- 1 | #include "math_utils.h" 2 | #include "gtest/gtest.h" 3 | 4 | template 5 | class InterpTest: public ::testing::Test { 6 | protected: 7 | virtual void SetUp() { 8 | imgsize = make_uint2(64, 64); 9 | img = (T *) calloc (sizeof(T) * imgsize.x * imgsize.y, 1); 10 | for(unsigned y = 0; y < imgsize.y - 1; y++) 11 | for (unsigned x = 0; x < imgsize.x - 1; x++) { 12 | // Checkboard 13 | img[x + y*imgsize.x] = (T)((x & 1) ^ (y & 1)); 14 | } 15 | } 16 | 17 | uint2 imgsize; 18 | T * img; 19 | }; 20 | 21 | typedef ::testing::Types testtypes; 22 | TYPED_TEST_CASE(InterpTest, testtypes); 23 | 24 | TYPED_TEST(InterpTest, OnOddPoint) { 25 | const float2 pix = make_float2(31.f, 32.f); 26 | const TypeParam interpolated = bilinear_interp(this->img, this->imgsize, pix); 27 | ASSERT_FLOAT_EQ(1.f, interpolated); 28 | } 29 | 30 | TYPED_TEST(InterpTest, OnEvenPoint) { 31 | const float2 pix = make_float2(24.f, 52.f); 32 | const TypeParam interpolated = bilinear_interp(this->img, this->imgsize, pix); 33 | ASSERT_FLOAT_EQ(0.f, interpolated); 34 | } 35 | 36 | TYPED_TEST(InterpTest, OnCellCentre) { 37 | const float2 pix = make_float2(24.5f, 52.5f); 38 | const TypeParam interpolated = bilinear_interp(this->img, this->imgsize, pix); 39 | ASSERT_FLOAT_EQ(0.5f, interpolated); 40 | } 41 | 42 | TYPED_TEST(InterpTest, OnCellQuarter) { 43 | const float2 pix = make_float2(24.25f, 52.25f); 44 | const TypeParam interpolated = bilinear_interp(this->img, this->imgsize, pix); 45 | ASSERT_FLOAT_EQ(0.375f, interpolated); 46 | } 47 | 48 | TYPED_TEST(InterpTest, OnCellArbitrary) { 49 | const float2 pix = make_float2(0.82f, 0.74f); 50 | const TypeParam interpolated = bilinear_interp(this->img, this->imgsize, pix); 51 | ASSERT_FLOAT_EQ(0.3464f, interpolated); 52 | } 53 | -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/1341846436.889894.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/1341846436.889894.png -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/1341846436.889927.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/1341846436.889927.png -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/1341846436.922139.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/1341846436.922139.png -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/1341846436.922153.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/1341846436.922153.png -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(ofusion++) 3 | 4 | set(PROJECT_TEST_NAME segmentation) 5 | set(UNIT_TEST_NAME ${PROJECT_TEST_NAME}-unittest) 6 | 7 | file(GLOB PREPROCESSING "../../src/cpp/preprocessing/*.cpp") 8 | file(GLOB SEGMENTATION "../../src/cpp/segmentation/*.cpp") 9 | 10 | add_executable(${UNIT_TEST_NAME} segmentation_unittest.cpp ${PREPROCESSING} ${SEGMENTATION}) 11 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 12 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread 13 | ${OpenCV_LIBS} cnpy) 14 | target_compile_definitions(${UNIT_TEST_NAME} PRIVATE FILEPATH="${CMAKE_CURRENT_LIST_DIR}") 15 | 16 | gtest_add_tests(TARGET ${UNIT_TEST_NAME}) 17 | -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/classid/0064.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/classid/0064.npy -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/classid/0065.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/classid/0065.npy -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/mask/0064.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/mask/0064.npy -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/mask/0065.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/segmentation/mask/0065.npy -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/pose: -------------------------------------------------------------------------------- 1 | tum fr2w_hs: 2 | 1341846436.889894 rgb/1341846436.889894.png 1341846436.889927 depth/1341846436.889927.png 1341846436.886900 0.0068 -2.7651 1.6077 -0.7091 -0.2017 0.2146 0.6407 3 | 1341846436.922139 rgb/1341846436.922139.png 1341846436.922153 depth/1341846436.922153.png 1341846436.927000 0.0167 -2.7562 1.6066 -0.7062 -0.2011 0.2219 0.6416 4 | -------------------------------------------------------------------------------- /apps/kfusion/test/segmentation/segmentation_unittest.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by binbin on 19/06/18. 3 | // 4 | #include "math_utils.h" 5 | #include "commons.h" 6 | #include "gtest/gtest.h" 7 | 8 | #include "opencv2/imgproc/imgproc.hpp" 9 | #include "opencv2/highgui/highgui.hpp" 10 | #include 11 | #include 12 | 13 | #include "../../src/cpp/segmentation/segmentation.h" 14 | 15 | class SegmentationTest : public ::testing::Test{ 16 | protected: 17 | std::shared_ptr segmenter; 18 | virtual void SetUp (){ 19 | 20 | I_r = cv::imread(FILEPATH"/1341846436.889894.png"); 21 | D_r = cv::imread(FILEPATH"/1341846436.889927.png"); 22 | I_l = cv::imread(FILEPATH"/1341846436.922139.png"); 23 | D_l = cv::imread(FILEPATH"/1341846436.922153.png"); 24 | 25 | //TUM fr3walking-halfsphere data 26 | T_w_r = toMatrix4({0.0068, -2.7651, 1.6077, -0.7091}, {-0.2017, 0.2146, 0.6407}); 27 | T_w_l = toMatrix4({0.0167, -2.7562, 1.6066, -0.7062}, {-0.2011, 0.2219, 0.6416}); 28 | k = {525.0, 525.0, 319.5, 239.5};//tum-ros 29 | 30 | //read maskfusion result 31 | mask_r = FILEPATH"/mask/0064.npy"; 32 | mask_l = FILEPATH"/mask/0065.npy"; 33 | classid_r = FILEPATH"/classid/0064.npy"; 34 | classid_l = FILEPATH"/classid/0065.npy"; 35 | 36 | SegmentationResult maskrcnn_r= segmenter->load_mask_rcnn(classid_r, mask_r, 37 | framesize.y, framesize.x); 38 | SegmentationResult maskrcnn_l= segmenter->load_mask_rcnn(classid_l, mask_l, 39 | framesize.y, framesize.x); 40 | 41 | } 42 | Matrix4 T_w_r; 43 | Matrix4 T_w_l; 44 | float4 k; 45 | 46 | cv::Mat I_r; 47 | cv::Mat I_l; 48 | cv::Mat D_r; 49 | cv::Mat D_l; 50 | 51 | uint2 framesize = {640, 480}; 52 | 53 | std::string mask_r; 54 | std::string mask_l; 55 | std::string classid_r; 56 | std::string classid_l; 57 | 58 | 59 | }; 60 | 61 | TEST_F(SegmentationTest, realtimeMaskRCNN){ 62 | bool run = true; 63 | ASSERT_EQ(run, 1); 64 | } 65 | 66 | 67 | -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/1341846436.889894.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/1341846436.889894.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/1341846436.889927.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/1341846436.889927.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/1341846436.922139.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/1341846436.922139.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/1341846436.922153.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/1341846436.922153.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/612.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/612.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/613.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/613.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/613_depth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/613_depth.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/720.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/720.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/720_depth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/720_depth.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/721.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/721.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/721_depth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/721_depth.png -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(octree_lib) 3 | file(GLOB PREPROCESSING "../../src/cpp/preprocessing/*.cpp") 4 | file(GLOB TRACKING "../../src/cpp/tracking/*.cpp") 5 | 6 | #file(GLOB TRACKING "src/cpp/tracking/*.cpp") 7 | add_library(lodepng SHARED ../../thirdparty/lodepng.cpp) 8 | 9 | set(PROJECT_TEST_NAME tracking) 10 | set(UNIT_TEST_NAME ${PROJECT_TEST_NAME}-jacobian-unittest) 11 | add_executable(${UNIT_TEST_NAME} jacobians_unittest.cpp ${PREPROCESSING} ${TRACKING}) 12 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS} 13 | ${OpenCV_LIBS} ) 14 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} ${OpenCV_LIBS} pthread lodepng) 15 | target_compile_definitions(${UNIT_TEST_NAME} PRIVATE FILEPATH="${CMAKE_CURRENT_LIST_DIR}") 16 | 17 | gtest_add_tests(TARGET ${UNIT_TEST_NAME}) 18 | -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/tum pose: -------------------------------------------------------------------------------- 1 | tum fr2w_hs: 2 | 1341846436.889894 rgb/1341846436.889894.png 1341846436.889927 depth/1341846436.889927.png 1341846436.886900 0.0068 -2.7651 1.6077 -0.7091 -0.2017 0.2146 0.6407 3 | 1341846436.922139 rgb/1341846436.922139.png 1341846436.922153 depth/1341846436.922153.png 1341846436.927000 0.0167 -2.7562 1.6066 -0.7062 -0.2011 0.2219 0.6416 4 | -------------------------------------------------------------------------------- /apps/kfusion/test/tracking/tum unit test.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/apps/kfusion/test/tracking/tum unit test.zip -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | if (OPENNI2_FOUND) 4 | 5 | include_directories(${OPENNI2_INCLUDE_PATHS}) 6 | add_executable(oni2raw oni2raw.cpp) 7 | target_link_libraries(oni2raw ${OPENNI2_LIBRARIES}) 8 | 9 | endif(OPENNI2_FOUND) 10 | 11 | if (OPENCL_FOUND) 12 | 13 | include_directories(${OPENCL_INCLUDE_DIRS}) 14 | add_library(oclwrapper SHARED OCLWrapper.cpp) 15 | target_link_libraries(oclwrapper ${OPENCL_LIBRARIES}) 16 | 17 | endif(OPENCL_FOUND) 18 | 19 | include_directories(.) 20 | 21 | add_library(lodepng SHARED lodepng.cpp) 22 | add_executable(scene2raw scene2raw.cpp) 23 | target_link_libraries(scene2raw lodepng) 24 | 25 | # add_subdirectory(./SurfReg/src) 26 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/checkKernels.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 3 | # Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 4 | # 5 | # This code is licensed under the MIT License. 6 | 7 | 8 | 9 | 10 | import sys 11 | import re 12 | import math 13 | import numpy 14 | 15 | 16 | kernel_consistency = [ 17 | ["mm2meters", "mm2metersKernel"], 18 | ["bilateral_filter","bilateralFilterKernel"], 19 | ["halfSampleRobust","halfSampleRobustImageKernel"], 20 | ["depth2vertex","depth2vertexKernel"], 21 | ["vertex2normal","vertex2normalKernel"], 22 | ["track","trackKernel"], 23 | ["reduce","reduceKernel"], 24 | ["integrate","integrateKernel"], 25 | ["raycast","raycastKernel"], 26 | ["renderDepth","renderDepthKernel"], 27 | ["renderLight","renderLightKernel"], 28 | ["renderTrack","renderTrackKernel"], 29 | ["renderVolume", "renderVolumeKernel"], 30 | ["ResetVolume","initVolumeKernel"], 31 | ["updatePose","updatePoseKernel"] 32 | ] 33 | 34 | def translateName(n) : 35 | for variations in kernel_consistency : 36 | if n in variations : 37 | return variations[0] 38 | return n 39 | 40 | 41 | kernel_log_regex = "([^ ]+)\s([0-9.]+)" 42 | 43 | 44 | # open files 45 | 46 | if len(sys.argv) != 2 : 47 | print "I need only one parameter, the kernel log file." 48 | exit (1) 49 | 50 | # open log file first 51 | print 52 | print "Kernel-level statistics. Times are in nanoseconds." 53 | fileref = open(sys.argv[1],'r') 54 | data = fileref.read() 55 | fileref.close() 56 | lines = data.split("\n") # remove head + first line 57 | 58 | data = {} 59 | 60 | for line in lines : 61 | matching = re.match(kernel_log_regex,line) 62 | if matching : 63 | name = translateName(matching.group(1)) 64 | if not name in data : 65 | data[name] = [] 66 | data[name].append(float(matching.group(2))) 67 | # else : 68 | # print "Skip SlamBench line : " + line 69 | 70 | 71 | for variable in sorted(data.keys()) : 72 | print "%20.20s"% str(variable), 73 | print "\tCount : %d" % len(data[variable]), 74 | print "\tMin : %d" % min(data[variable]), 75 | print "\tMax : %d" % max(data[variable]), 76 | print "\tMean : %f" % numpy.mean(data[variable]), 77 | print "\tTotal : %d" % sum(data[variable]) 78 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6 FATAL_ERROR) 2 | if(COMMAND cmake_policy) 3 | cmake_policy(SET CMP0003 NEW) 4 | endif(COMMAND cmake_policy) 5 | 6 | project(CNPY) 7 | 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 9 | 10 | option(ENABLE_STATIC "Build static (.a) library" ON) 11 | 12 | find_package(ZLIB REQUIRED) 13 | 14 | include_directories(${ZLIB_INCLUDE_DIRS}) 15 | 16 | add_library(cnpy SHARED "cnpy.cpp") 17 | target_link_libraries(cnpy ${ZLIB_LIBRARIES}) 18 | install(TARGETS "cnpy" LIBRARY DESTINATION lib PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE) 19 | 20 | if(ENABLE_STATIC) 21 | add_library(cnpy-static STATIC "cnpy.cpp") 22 | set_target_properties(cnpy-static PROPERTIES OUTPUT_NAME "cnpy") 23 | install(TARGETS "cnpy-static" ARCHIVE DESTINATION lib) 24 | endif(ENABLE_STATIC) 25 | 26 | install(FILES "cnpy.h" DESTINATION include) 27 | install(FILES "mat2npz" "npy2mat" "npz2mat" DESTINATION bin PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE) 28 | 29 | add_executable(example1 example1.cpp) 30 | target_link_libraries(example1 cnpy) 31 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License 2 | 3 | Copyright (c) Carl Rogers, 2011 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 21 | THE SOFTWARE. 22 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/README.md: -------------------------------------------------------------------------------- 1 | # Purpose: 2 | 3 | NumPy offers the `save` method for easy saving of arrays into .npy and `savez` for zipping multiple .npy arrays together into a .npz file. 4 | 5 | `cnpy` lets you read and write to these formats in C++. 6 | 7 | The motivation comes from scientific programming where large amounts of data are generated in C++ and analyzed in Python. 8 | 9 | Writing to .npy has the advantage of using low-level C++ I/O (fread and fwrite) for speed and binary format for size. 10 | The .npy file header takes care of specifying the size, shape, and data type of the array, so specifying the format of the data is unnecessary. 11 | 12 | Loading data written in numpy formats into C++ is equally simple, but requires you to type-cast the loaded data to the type of your choice. 13 | 14 | # Installation: 15 | 16 | Default installation directory is /usr/local. 17 | To specify a different directory, add `-DCMAKE_INSTALL_PREFIX=/path/to/install/dir` to the cmake invocation in step 4. 18 | 19 | 1. get [cmake](www.cmake.org) 20 | 2. create a build directory, say $HOME/build 21 | 3. cd $HOME/build 22 | 4. cmake /path/to/cnpy 23 | 5. make 24 | 6. make install 25 | 26 | # Using: 27 | 28 | To use, `#include"cnpy.h"` in your source code. Compile the source code mycode.cpp as 29 | 30 | ```bash 31 | g++ -o mycode mycode.cpp -L/path/to/install/dir -lcnpy -lz --std=c++11 32 | ``` 33 | 34 | # Description: 35 | 36 | There are two functions for writing data: `npy_save` and `npz_save`. 37 | 38 | There are 3 functions for reading: 39 | - `npy_load` will load a .npy file. 40 | - `npz_load(fname)` will load a .npz and return a dictionary of NpyArray structues. 41 | - `npz_load(fname,varname)` will load and return the NpyArray for data varname from the specified .npz file. 42 | 43 | The data structure for loaded data is below. 44 | Data is accessed via the `data()`-method, which returns a pointer of the specified type (which must match the underlying datatype of the data). 45 | The array shape and word size are read from the npy header. 46 | 47 | ```c++ 48 | struct NpyArray { 49 | std::vector shape; 50 | size_t word_size; 51 | template T* data(); 52 | }; 53 | ``` 54 | 55 | See [example1.cpp](example1.cpp) for examples of how to use the library. example1 will also be build during cmake installation. 56 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/example1.cpp: -------------------------------------------------------------------------------- 1 | #include"cnpy.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | const int Nx = 128; 9 | const int Ny = 64; 10 | const int Nz = 32; 11 | 12 | int main() 13 | { 14 | //set random seed so that result is reproducible (for testing) 15 | srand(0); 16 | //create random data 17 | std::vector> data(Nx*Ny*Nz); 18 | for(int i = 0;i < Nx*Ny*Nz;i++) data[i] = std::complex(rand(),rand()); 19 | 20 | //save it to file 21 | cnpy::npy_save("arr1.npy",&data[0],{Nz,Ny,Nx},"w"); 22 | 23 | //load it into a new array 24 | cnpy::NpyArray arr = cnpy::npy_load("arr1.npy"); 25 | std::complex* loaded_data = arr.data>(); 26 | 27 | //make sure the loaded data matches the saved data 28 | assert(arr.word_size == sizeof(std::complex)); 29 | assert(arr.shape.size() == 3 && arr.shape[0] == Nz && arr.shape[1] == Ny && arr.shape[2] == Nx); 30 | for(int i = 0; i < Nx*Ny*Nz;i++) assert(data[i] == loaded_data[i]); 31 | 32 | //append the same data to file 33 | //npy array on file now has shape (Nz+Nz,Ny,Nx) 34 | cnpy::npy_save("arr1.npy",&data[0],{Nz,Ny,Nx},"a"); 35 | 36 | //now write to an npz file 37 | //non-array variables are treated as 1D arrays with 1 element 38 | double myVar1 = 1.2; 39 | char myVar2 = 'a'; 40 | cnpy::npz_save("out.npz","myVar1",&myVar1,{1},"w"); //"w" overwrites any existing file 41 | cnpy::npz_save("out.npz","myVar2",&myVar2,{1},"a"); //"a" appends to the file we created above 42 | cnpy::npz_save("out.npz","arr1",&data[0],{Nz,Ny,Nx},"a"); //"a" appends to the file we created above 43 | 44 | //load a single var from the npz file 45 | cnpy::NpyArray arr2 = cnpy::npz_load("out.npz","arr1"); 46 | 47 | //load the entire npz file 48 | cnpy::npz_t my_npz = cnpy::npz_load("out.npz"); 49 | 50 | //check that the loaded myVar1 matches myVar1 51 | cnpy::NpyArray arr_mv1 = my_npz["myVar1"]; 52 | double* mv1 = arr_mv1.data(); 53 | assert(arr_mv1.shape.size() == 1 && arr_mv1.shape[0] == 1); 54 | assert(mv1[0] == myVar1); 55 | } 56 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/mat2npz: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | from numpy import savez 5 | from scipy.io import loadmat 6 | 7 | assert len(sys.argv) > 1 8 | 9 | files = sys.argv[1:] 10 | 11 | for f in files: 12 | mat_vars = loadmat(f) 13 | mat_vars.pop('__version__') 14 | mat_vars.pop('__header__') 15 | mat_vars.pop('__globals__') 16 | 17 | fn = f.replace('.mat','.npz') 18 | savez(fn,**mat_vars) 19 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/npy2mat: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | from numpy import load 5 | from scipy.io import savemat 6 | 7 | assert len(sys.argv) > 1 8 | 9 | files = sys.argv[1:] 10 | 11 | for f in files: 12 | data = load(f) 13 | fn = f.replace('.npy','') 14 | fn = fn.replace('.','_') 15 | savemat(fn,{fn : data}) 16 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/cnpy/npz2mat: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | from numpy import load 5 | from scipy.io import savemat 6 | 7 | assert len(sys.argv) > 1 8 | 9 | files = sys.argv[1:] 10 | 11 | for f in files: 12 | data = load(f) 13 | fn = f.replace('.npz','') 14 | fn = fn.replace('.','_') #matlab cant handle dots 15 | savemat(fn,data) 16 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/generatePlots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 3 | # Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 4 | # 5 | # This code is licensed under the MIT License. 6 | 7 | import sys 8 | import re 9 | import math 10 | import numpy 11 | import matplotlib.pyplot as plt 12 | from matplotlib.backends.backend_pdf import PdfPages 13 | 14 | kfusion_log_regex = "([0-9]+[\s]*)\\t" 15 | kfusion_log_regex += 8 * "([0-9.]+)\\t" 16 | kfusion_log_regex += 3 * "([-0-9.]+)\\t" 17 | kfusion_log_regex += "([01])\s+([01])" 18 | 19 | nuim_log_regex = "([0-9]+)" 20 | nuim_log_regex += 7 * "\\s+([-0-9e.]+)\\s*" 21 | 22 | 23 | # open files 24 | 25 | if len(sys.argv) != 3 : 26 | print "I need two parameters, the benchmark log file and the original scene camera position file." 27 | exit (1) 28 | 29 | # open benchmark log file first 30 | print "Get KFusion output data." 31 | framesDropped = 0 32 | validFrames = 0 33 | lastFrame = -1 34 | untracked = -4; 35 | kfusion_traj = [] 36 | fileref = open(sys.argv[1],'r') 37 | data = fileref.read() 38 | fileref.close() 39 | lines = data.split("\n") # remove head + first line 40 | headers = lines[0].split("\t") 41 | fulldata = {} 42 | if len(headers) == 15 : 43 | if headers[14] == "" : 44 | del headers[14] 45 | if len(headers) != 14 : 46 | print "Wrong KFusion log file. Expected 14 columns but found " + str(len(headers)) 47 | exit(1) 48 | for variable in headers[2:7] : 49 | fulldata[variable] = [] 50 | 51 | for line in lines[1:] : 52 | matching = re.match(kfusion_log_regex,line) 53 | if matching : 54 | dropped = int( matching.group(1)) - lastFrame - 1 55 | if dropped>0: 56 | framesDropped = framesDropped + dropped 57 | for pad in range(0,dropped) : 58 | kfusion_traj.append( lastValid ) 59 | validFrames = validFrames +1 60 | for elem_idx in range(len(headers)) : 61 | if headers[elem_idx] in fulldata: 62 | fulldata[headers[elem_idx]].append(float(matching.group(elem_idx+1))) 63 | 64 | lastFrame = int(matching.group(1)) 65 | else : 66 | #print "Skip KFusion line : " + line 67 | break 68 | 69 | 70 | #print "The following are designed to enable easy macchine readability of key data" 71 | #print "MRkey:,logfile,ATE,computaion,dropped,untracked" 72 | #print ("MRdata:,%s,%6.6f,%6.6f,%d,%d") % ( sys.argv[1], numpy.mean(fulldata["ATE"]), numpy.mean(fulldata["computation"]), framesDropped, untracked) 73 | 74 | print "\nGenerating plots.." 75 | 76 | for variable in sorted(fulldata.keys()) : 77 | if "X" in variable or "Z" in variable or "Y" in variable or "frame" in variable or "tracked" in variable or "integrated" in variable : 78 | del fulldata[variable] 79 | continue 80 | 81 | if (framesDropped == 0) and (str(variable) == "ATE_wrt_kfusion"): 82 | del fulldata[variable] 83 | continue 84 | if variable == "ATE": 85 | del fulldata[variable] 86 | continue 87 | 88 | print "%20.20s" % str(variable), 89 | print "\tMin : %6.6f" % min(fulldata[variable]), 90 | print "\tMax : %0.6f" % max(fulldata[variable]), 91 | print "\tMean : %0.6f" % numpy.mean(fulldata[variable]), 92 | print "\tStdev : %0.6f" % numpy.std(fulldata[variable]), 93 | print "\tTotal : %0.8f" % sum(fulldata[variable]) 94 | 95 | # Configuring the plot 96 | 97 | index = numpy.arange(len(fulldata.values())) 98 | bar_width = 0.30 99 | error_config = { 'ecolor': '0.3'} 100 | means = [numpy.mean(fulldata[variable]) for variable in fulldata.keys()] 101 | stdv = [numpy.std(fulldata[variable]) for variable in fulldata.keys() ] 102 | print "Dictionary length: ", index 103 | plot = plt.bar(index + bar_width, means, bar_width, color='#43656a' ) 104 | 105 | plt.xticks(index + (1.5 * bar_width), fulldata.keys(), rotation='vertical') 106 | plt.grid(True) 107 | plt.tight_layout() 108 | 109 | pdf = PdfPages('plot.pdf') 110 | plt.savefig(pdf, format='pdf') 111 | pdf.close() 112 | 113 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/nvprof2log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 3 | # Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 4 | # 5 | # This code is licensed under the MIT License. 6 | 7 | import re 8 | import sys 9 | 10 | for line in sys.stdin: 11 | matcher = re.match("([^\s]+)\s+([^\s]+)\s.*\s([^<]+)=]*\(.*",line) 12 | if matcher == None : 13 | continue 14 | name = matcher.group(3) 15 | duration = 0 16 | if matcher.group(2)[-2:] == "ms" : 17 | duration = int(float(matcher.group(2)[:-3]) * 1000000) 18 | else : 19 | if matcher.group(2)[-2:] == "us": 20 | duration = int(float(matcher.group(2)[:-3]) * 1000) 21 | else : 22 | duration = -1 23 | print "%s %d" % (name, duration) 24 | sys.stdout.flush() 25 | 26 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/processPowerRpt: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 3 | # Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 4 | # 5 | # This code is licensed under the MIT License. 6 | 7 | # the next line restarts using tclsh \ 8 | exec tclsh "$0" ${1+"$@"} 9 | 10 | if { $argc < 1 } { 11 | puts "[lindex argv 0] " 12 | exit 13 | } 14 | 15 | #puts $heading 16 | #puts $files 17 | set file [lindex $argv 0] 18 | foreach file $argv { 19 | set fp [ open $file] 20 | set pos -1 21 | puts "$file" 22 | if { [gets $fp line] >=0 } { 23 | set keys [split [regexp -inline -all -- {\S+} $line] " " ] 24 | set time [ lsearch $keys "Sample_time" ] 25 | set i 0 26 | 27 | foreach key $keys { 28 | set total($i) 0 29 | incr i 30 | } 31 | } 32 | 33 | 34 | 35 | while { [gets $fp line] >=0 } { 36 | if { [string range $line 0 0] != "#" } { 37 | set words [split [regexp -inline -all -- {\S+} $line] " " ] 38 | set i 0 39 | set delay [lindex $words $time] 40 | foreach col $words { 41 | if { $i == $time } { 42 | set total($i) [expr $total($i) + $col] 43 | } else { 44 | set total($i) [expr $total($i) + [expr $col * $delay] ] 45 | } 46 | incr i 47 | } 48 | } 49 | } 50 | 51 | close $fp 52 | set i 0; 53 | set totalEnergy 0; 54 | 55 | foreach key $keys { 56 | if { [string compare $key "Sample_time"] != 0 } { 57 | puts "${file}: $key\tTotal energy: $total($i) Average Power: [expr $total($i) / $total($time)]" 58 | set totalEnergy [expr $totalEnergy + $total($i)] 59 | } 60 | incr i 61 | } 62 | puts "${file}: System Total energy: $totalEnergy Average Power: [expr ${totalEnergy}/$total($time)]" 63 | } 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/scene2raw.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester. 4 | Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1 5 | 6 | This code is licensed under the MIT License. 7 | 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "lodepng.h" 24 | 25 | #include 26 | #include 27 | 28 | typedef unsigned short int ushort; 29 | 30 | static const float SceneK[3][3] = { 481.20, 0.00, 319.50, 0.00, -480.00, 239.50, 31 | 0.00, 0.00, 1.00 }; 32 | 33 | static const int _scenewidth = 640; 34 | static const int _sceneheight = 480; 35 | static const float _u0 = SceneK[0][2]; 36 | static const float _v0 = SceneK[1][2]; 37 | static const float _focal_x = SceneK[0][0]; 38 | static const float _focal_y = SceneK[1][1]; 39 | 40 | struct uchar3 { 41 | unsigned char x, y, z; 42 | }; 43 | struct uchar4 { 44 | unsigned char x, y, z, w; 45 | }; 46 | 47 | struct uint2 { 48 | unsigned int x, y; 49 | }; 50 | 51 | inline uchar3 make_uchar3(unsigned char x, unsigned char y, unsigned char z) { 52 | uchar3 val; 53 | val.x = x; 54 | val.y = y; 55 | val.z = z; 56 | return val; 57 | } 58 | 59 | inline uchar4 make_uchar4(unsigned char x, unsigned char y, unsigned char z, 60 | unsigned char w) { 61 | uchar4 val; 62 | val.x = x; 63 | val.y = y; 64 | val.z = z; 65 | val.w = w; 66 | return val; 67 | } 68 | 69 | inline uint2 make_uint2(unsigned int x, unsigned int y) { 70 | uint2 val; 71 | val.x = x; 72 | val.y = y; 73 | return val; 74 | } 75 | 76 | int readDepthFile(ushort * depthMap, const char * filename) { 77 | 78 | std::ifstream source; 79 | source.open(filename, std::ios_base::in); 80 | double * scene_depth_float_buffer = (double*) malloc( 81 | sizeof(double) * _scenewidth * _sceneheight); 82 | 83 | if (!source) { 84 | std::cerr << "Can't open Data from " << filename << " !\n"; 85 | return -1; 86 | } 87 | int index = 0; 88 | while (source.good()) { 89 | double d; 90 | source >> d; 91 | if (640 * 480 <= index) 92 | continue; // FIXME : not sure why it read one more float 93 | scene_depth_float_buffer[index] = d * 1000; 94 | index++; 95 | } 96 | 97 | for (int v = 0; v < _sceneheight; v++) { 98 | for (int u = 0; u < _scenewidth; u++) { 99 | double u_u0_by_fx = (u - _u0) / _focal_x; 100 | double v_v0_by_fy = (v - _v0) / _focal_y; 101 | 102 | depthMap[u + v * _scenewidth] = scene_depth_float_buffer[u 103 | + v * _scenewidth] 104 | / std::sqrt( 105 | u_u0_by_fx * u_u0_by_fx + v_v0_by_fy * v_v0_by_fy 106 | + 1); 107 | 108 | } 109 | } 110 | free(scene_depth_float_buffer); 111 | //std::cout << "End of file reading (" << index << ")." << std::endl; 112 | return index; 113 | } 114 | 115 | int main(int argc, char ** argv) { 116 | 117 | uint2 inputSize = make_uint2(640, 480); 118 | static const uint2 imageSize = { 320, 240 }; 119 | 120 | if (argc != 3) { 121 | std::cout 122 | << "Bad entries... I just need the scene directory and the output file" 123 | << std::endl; 124 | exit(1); 125 | } 126 | 127 | // Input 128 | // wait for something like this : scene/ ( scene_00_0000.depth ) 129 | std::string dir = argv[1]; 130 | FILE* pFile = fopen(argv[2], "wb"); 131 | if (!pFile) { 132 | std::cout << "File opening failed : " << argv[2] << std::endl; 133 | exit(1); 134 | } 135 | 136 | uchar3 * rgbImage = (uchar3*) malloc( 137 | sizeof(uchar3) * inputSize.x * inputSize.y); 138 | 139 | ushort * inputFile = (ushort*) malloc( 140 | sizeof(ushort) * inputSize.x * inputSize.y); 141 | 142 | for (int i = 0; true; i++) { 143 | 144 | unsigned error; 145 | uchar4* image; 146 | unsigned width, height; 147 | 148 | std::ostringstream filename; 149 | std::ostringstream rgbfilename; 150 | 151 | filename << dir << "/scene_00_" << std::setfill('0') << std::setw(4) 152 | << i << ".depth"; 153 | rgbfilename << dir << "/scene_00_" << std::setfill('0') << std::setw(4) 154 | << i << ".png"; 155 | 156 | if (readDepthFile(inputFile, filename.str().c_str()) == -1) { 157 | break; 158 | } 159 | 160 | error = lodepng_decode32_file((unsigned char**) &image, &width, &height, 161 | rgbfilename.str().c_str()); 162 | if (error) { 163 | printf("error %u: %s\n", error, lodepng_error_text(error)); 164 | } else { 165 | for (unsigned int i = 0; i < inputSize.y * inputSize.x; i++) { 166 | rgbImage[i] = make_uchar3(image[i].x, image[i].y, image[i].z); 167 | } 168 | } 169 | 170 | int total = 0; 171 | total += fwrite(&(inputSize), sizeof(imageSize), 1, pFile); 172 | total += fwrite(inputFile, sizeof(uint16_t), inputSize.x * inputSize.y, 173 | pFile); 174 | total += fwrite(&(inputSize), sizeof(imageSize), 1, pFile); 175 | total += fwrite(rgbImage, sizeof(uchar3), inputSize.x * inputSize.y, 176 | pFile); 177 | 178 | std::cout << "\rRead frame " << std::setw(10) << i << " "; 179 | 180 | if (i % 2) { 181 | fflush(stdout); 182 | } 183 | free(image); 184 | } 185 | std::cout << std::endl; 186 | fclose(pFile); 187 | } 188 | -------------------------------------------------------------------------------- /apps/kfusion/thirdparty/tiny_obj_loader.cc: -------------------------------------------------------------------------------- 1 | #define TINYOBJLOADER_IMPLEMENTATION 2 | #include "tiny_obj_loader.h" 3 | -------------------------------------------------------------------------------- /lib/algorithms/filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef ACTIVE_LIST_HPP 33 | #define ACTIVE_LIST_HPP 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace algorithms { 41 | 42 | template 43 | bool in_frustum(const VoxelBlockType* v, float voxelSize, 44 | const Matrix4& camera, const int2& frameSize) { 45 | const float3 block_coord = make_float3(v->coordinates()) * voxelSize; 46 | const float3 v_camera = camera*block_coord; 47 | const int2 px = make_int2(v_camera.x/v_camera.z, v_camera.y/v_camera.z); 48 | if( px.x >= 0 && px.x < frameSize.x && px.y >= 0 && px.y < frameSize.y) 49 | return true; 50 | return false; 51 | } 52 | 53 | template 54 | bool satisfies(const ValueType& el, P predicate) { 55 | return predicate(el); 56 | } 57 | 58 | template 59 | bool satisfies(const ValueType& el, P predicate, Ps... others) { 60 | return predicate(el) || satisfies(el, others...); 61 | } 62 | 63 | #ifdef _OPENMP 64 | template 65 | void filter(std::vector& out, 66 | const MemoryPool& block_array, Predicates... ps) { 67 | 68 | std::vector temp; 69 | int num_elem = block_array.size(); 70 | temp.resize(num_elem); 71 | 72 | int * thread_start = new int[omp_get_max_threads()]; 73 | int * thread_end = new int[omp_get_max_threads()]; 74 | int spawn_threads; 75 | #pragma omp parallel 76 | { 77 | int threadid = omp_get_thread_num(); 78 | int num_threads = omp_get_num_threads(); 79 | int my_start = thread_start[threadid] = (threadid) * num_elem / num_threads; 80 | int my_end = (threadid+1) * num_elem / num_threads; 81 | int count = 0; 82 | #pragma omp simd 83 | for(int i = my_start; i < my_end; ++i) { 84 | if(satisfies(block_array[i], ps...)){ 85 | temp[my_start + count] = block_array[i]; 86 | count++; 87 | } 88 | } 89 | /* Store the actual end */ 90 | thread_end[threadid] = count; 91 | if(threadid == 0) spawn_threads = num_threads; 92 | } 93 | 94 | int total = 0; 95 | for(int i = 0; i < spawn_threads; ++i) { 96 | total += thread_end[i]; 97 | } 98 | out.resize(total); 99 | /* Copy the first */ 100 | std::memcpy(out.data(), temp.data(), sizeof(BlockType *) * thread_end[0]); 101 | int copied = thread_end[0]; 102 | /* Copy the rest */ 103 | for(int i = 1; i < spawn_threads; ++i) { 104 | std::memcpy(out.data() + copied, 105 | temp.data() + thread_start[i], sizeof(BlockType *) * thread_end[i]); 106 | copied += thread_end[i]; 107 | } 108 | } 109 | 110 | #else 111 | template 112 | void filter(std::vector& out, 113 | const MemoryPool& block_array, Predicates... ps) { 114 | for(unsigned int i = 0; i < block_array.size(); ++i) { 115 | if(satisfies(block_array[i], ps...)){ 116 | out.push_back(block_array[i]); 117 | } 118 | } 119 | } 120 | #endif 121 | } 122 | #endif 123 | -------------------------------------------------------------------------------- /lib/algorithms/mapping.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef ALGO_MAPPING_HPP 33 | #define ALGO_MAPPING_HPP 34 | #include 35 | 36 | inline float3 voxelToPos(const int3 p, const float voxelSize){ 37 | return make_float3(p.x * voxelSize, p.y * voxelSize, p.z * voxelSize); 38 | } 39 | 40 | namespace algorithms { 41 | 42 | template 43 | void integratePass(VoxelBlock ** blockList, unsigned int list_size, 44 | const float * depth, uint2 depthSize, const float voxelSize, 45 | const Matrix4 invTrack, const Matrix4 K, const float mu, 46 | const float maxweight, const int current_frame) { 47 | 48 | #pragma omp parallel for 49 | for(unsigned int i = 0; i < list_size; ++i){ 50 | integrate(blockList[i], depth, depthSize, voxelSize, 51 | invTrack, K, mu, maxweight); 52 | blockList[i]->timestamp(current_frame); 53 | } 54 | } 55 | 56 | /* 57 | * MemoryBufferType is an instance of the memory allocator class 58 | */ 59 | template 60 | void integratePass(MemoryBufferType& nodes_list, unsigned int list_size, 61 | UpdateFunctor f) { 62 | 63 | #pragma omp parallel for 64 | for(unsigned int i = 0; i < list_size; ++i){ 65 | f(nodes_list[i]); 66 | } 67 | } 68 | } 69 | #endif 70 | -------------------------------------------------------------------------------- /lib/algorithms/unique.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef UNIQUE_HPP 33 | #define UNIQUE_HPP 34 | 35 | namespace algorithms { 36 | template 37 | inline int unique(T* keys, int num_keys){ 38 | int end = 1; 39 | if(num_keys < 2) return end; 40 | for (int i = 1; i < num_keys; ++i){ 41 | if(keys[i] != keys[i-1]){ 42 | keys[end] = keys[i]; 43 | ++end; 44 | } 45 | } 46 | return end; 47 | } 48 | 49 | template 50 | inline int unique_multiscale(KeyT* keys, int num_keys, 51 | const KeyT level_mask){ 52 | int end = 1; 53 | const KeyT key_mask = ~level_mask; 54 | if(num_keys < 2) return end; 55 | KeyT prev_key = keys[0] & key_mask; 56 | KeyT prev_level = keys[0] & level_mask; 57 | for (int i = 1; i < num_keys; ++i){ 58 | const KeyT key = keys[i] & key_mask; 59 | const KeyT level = keys[i] & level_mask; 60 | if(key != prev_key){ 61 | keys[end] = keys[i]; 62 | ++end; 63 | } else if(level > prev_level) { 64 | /* end does not advance but previous entry is overwritten */ 65 | keys[end-1] = keys[i]; 66 | } 67 | prev_key = key; 68 | prev_level = level; 69 | } 70 | return end; 71 | } 72 | 73 | template 74 | inline int unique_multiscale(KeyT* keys, int num_keys, 75 | const KeyT level_mask, const unsigned current_level){ 76 | int end = 1; 77 | const KeyT key_mask = ~level_mask; 78 | if(num_keys < 2) return end; 79 | KeyT prev_key = keys[0] & key_mask; 80 | KeyT prev_level = keys[0] & level_mask; 81 | for (int i = 1; i < num_keys; ++i){ 82 | const KeyT key = keys[i] & key_mask; 83 | const KeyT level = keys[i] & level_mask; 84 | // std::cout << "Key = " << key << " level = " << level << " prev_key = " 85 | // << prev_key << " prev_level = " << prev_level << std::endl; 86 | if(level >= current_level) { 87 | if(key != prev_key){ 88 | keys[end++] = keys[i]; 89 | } else if(level > prev_level) { 90 | /* end does not advance but previous entry is overwritten */ 91 | keys[end-1] = keys[i]; 92 | } 93 | prev_key = key; 94 | prev_level = level; 95 | } 96 | } 97 | return end; 98 | } 99 | } 100 | #endif 101 | -------------------------------------------------------------------------------- /lib/data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef DATA_H 33 | #define DATA_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | class StaticStorage; 43 | class DynamicStorage; 44 | 45 | template 46 | class Data {}; 47 | 48 | template 49 | class Data { 50 | 51 | public: 52 | typedef voxel_traits traits_type; 53 | typedef typename traits_type::StoredType stored_type; 54 | 55 | stored_type& operator()(const int i) { 56 | return _data[i]; 57 | }; 58 | 59 | const stored_type& operator()(const int i) const { 60 | return _data[i]; 61 | }; 62 | 63 | void init(std::size_t size) { 64 | _data = (stored_type *) malloc(size * sizeof(stored_type)); 65 | }; 66 | void release() { 67 | free(_data); 68 | }; 69 | 70 | stored_type * raw_data() { return _data; }; 71 | 72 | private: 73 | stored_type * _data; 74 | 75 | }; 76 | 77 | template 78 | class Data { 79 | 80 | public: 81 | typedef voxel_traits traits_type; 82 | typedef typename traits_type::StoredType stored_type; 83 | stored_type& operator()(const int pos, const int i); 84 | const stored_type& operator()(const int pos, const int i) const; 85 | 86 | private: 87 | stored_type * _data; 88 | }; 89 | #endif 90 | -------------------------------------------------------------------------------- /lib/functors/axis_aligned_functor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef AA_FUNCTOR_HPP 33 | #define AA_FUNCTOR_HPP 34 | #include 35 | #include 36 | 37 | #include "math_utils.h" 38 | #include "algorithms/filter.hpp" 39 | #include "node.hpp" 40 | #include "functors/data_handler.hpp" 41 | #include "geometry/aabb_collision.hpp" 42 | 43 | namespace iterators { 44 | namespace functor { 45 | 46 | template class MapT, 47 | typename UpdateF> 48 | 49 | class axis_aligned { 50 | public: 51 | axis_aligned(MapT& map, UpdateF f) : _map(map), _function(f), 52 | _min(make_int3(0)), _max(make_int3(map.size())){ } 53 | 54 | axis_aligned(MapT& map, UpdateF f, const int3 min, 55 | const int3 max) : _map(map), _function(f), 56 | _min(min), _max(max){ } 57 | 58 | void update_block(VoxelBlock * block) { 59 | int3 blockCoord = block->coordinates(); 60 | unsigned int y, z, x; 61 | int3 blockSide = make_int3(VoxelBlock::side); 62 | int3 start = max(blockCoord, _min); 63 | int3 last = min(blockCoord + blockSide, _max); 64 | 65 | for(z = start.z; z < last.z; ++z) { 66 | for (y = start.y; y < last.y; ++y) { 67 | for (x = start.x; x < last.x; ++x) { 68 | int3 vox = make_int3(x, y, z); 69 | VoxelBlockHandler handler = {block, vox}; 70 | _function(handler, vox); 71 | } 72 | } 73 | } 74 | } 75 | 76 | void update_node(Node * node) { 77 | int3 voxel = make_int3(unpack_morton(node->code)); 78 | #pragma omp simd 79 | for(int i = 0; i < 8; ++i) { 80 | const int3 dir = make_int3((i & 1) > 0, (i & 2) > 0, (i & 4) > 0); 81 | voxel = voxel + (dir * (node->side/2)); 82 | if(!(in(voxel.x, _min.x, _max.x) && in(voxel.y, _min.y, _max.y) 83 | && in(voxel.z, _min.z, _max.z))) continue; 84 | NodeHandler handler = {node, i}; 85 | _function(handler, voxel); 86 | } 87 | } 88 | 89 | void apply() { 90 | 91 | auto& block_list = _map.getBlockBuffer(); 92 | size_t list_size = block_list.size(); 93 | #pragma omp parallel for 94 | for(unsigned int i = 0; i < list_size; ++i){ 95 | update_block(block_list[i]); 96 | } 97 | 98 | auto& nodes_list = _map.getNodesBuffer(); 99 | list_size = nodes_list.size(); 100 | #pragma omp parallel for 101 | for(unsigned int i = 0; i < list_size; ++i){ 102 | update_node(nodes_list[i]); 103 | } 104 | } 105 | 106 | private: 107 | MapT& _map; 108 | UpdateF _function; 109 | int3 _min; 110 | int3 _max; 111 | }; 112 | } 113 | } 114 | #endif 115 | -------------------------------------------------------------------------------- /lib/functors/data_handler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | 33 | #ifndef DATA_HANDLER_HPP 34 | #define DATA_HANDLER_HPP 35 | #include "math_utils.h" 36 | #include "node.hpp" 37 | 38 | template 39 | class DataHandlerBase { 40 | typename NodeT::compute_type get() { 41 | return static_cast(this)->get(); 42 | } 43 | void set(const typename NodeT::compute_type& val) { 44 | static_cast(this)->set(val); 45 | } 46 | }; 47 | 48 | template 49 | class VoxelBlockHandler : 50 | DataHandlerBase, VoxelBlock > { 51 | 52 | public: 53 | VoxelBlockHandler(VoxelBlock* ptr, int3 v) : 54 | _block(ptr), _voxel(v) {} 55 | 56 | typename VoxelBlock::compute_type get() { 57 | return _block->data(_voxel); 58 | } 59 | 60 | void set(const typename VoxelBlock::compute_type& val) { 61 | _block->data(_voxel, val); 62 | } 63 | 64 | private: 65 | VoxelBlock * _block; 66 | int3 _voxel; 67 | }; 68 | 69 | template 70 | class NodeHandler: DataHandlerBase, Node > { 71 | public: 72 | NodeHandler(Node* ptr, int i) : _node(ptr), _idx(i) {} 73 | 74 | typename Node::compute_type get() { 75 | return _node->value_[_idx]; 76 | } 77 | 78 | void set(const typename Node::compute_type& val) { 79 | _node->value_[_idx] = val; 80 | } 81 | 82 | private: 83 | Node * _node; 84 | int _idx; 85 | }; 86 | 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /lib/geometry/aabb_collision.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef AABB_COLLISION_HPP 33 | #define AABB_COLLISION_HPP 34 | #include 35 | #include 36 | 37 | namespace geometry { 38 | inline int axis_overlap(int a, const int a_edge, 39 | int b, const int b_edge) { 40 | /* Half plane intersection test */ 41 | a = a + (a_edge/2); 42 | b = b + (b_edge/2); 43 | return (std::abs(b - a) > (a_edge + b_edge)/2) ? 0 : 1; 44 | } 45 | 46 | inline int axis_overlap(float a, const float a_edge, 47 | float b, const float b_edge) { 48 | /* Half plane intersection test */ 49 | a = a + (a_edge/2); 50 | b = b + (b_edge/2); 51 | return (std::fabs(b - a) > (a_edge + b_edge)/2) ? 0 : 1; 52 | } 53 | 54 | inline int axis_contained(float a, const float a_edge, 55 | float b, const float b_edge) { 56 | /* Segment a includes segment b */ 57 | return a < b && a + a_edge > b + b_edge; 58 | } 59 | 60 | inline int aabb_aabb_collision(const int3 a, const int3 a_edge, 61 | const int3 b, const int3 b_edge){ 62 | return axis_overlap(a.x, a_edge.x, b.x, b_edge.x) && 63 | axis_overlap(a.y, a_edge.y, b.y, b_edge.y) && 64 | axis_overlap(a.z, a_edge.z, b.z, b_edge.z); 65 | } 66 | 67 | inline int aabb_aabb_inclusion(const int3 a, const int3 a_edge, 68 | const int3 b, const int3 b_edge){ 69 | /* Box a contains box b */ 70 | return axis_contained(a.x, a_edge.x, b.x, b_edge.x) && 71 | axis_contained(a.y, a_edge.y, b.y, b_edge.y) && 72 | axis_contained(a.z, a_edge.z, b.z, b_edge.z); 73 | } 74 | } 75 | #endif 76 | -------------------------------------------------------------------------------- /lib/node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef NODE_H 33 | #define NODE_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | template 42 | class Node { 43 | 44 | public: 45 | typedef voxel_traits traits_type; 46 | typedef typename traits_type::ComputeType compute_type; 47 | typedef typename traits_type::StoredType stored_type; 48 | compute_type empty() const { return traits_type::empty(); } 49 | compute_type init_val() const { return traits_type::initValue(); } 50 | 51 | compute_type value_[8]; 52 | octlib::key_t code; 53 | unsigned int side; 54 | unsigned char children_mask_; 55 | 56 | Node(){ 57 | code = 0; 58 | side = 0; 59 | children_mask_ = 0; 60 | for (unsigned int i = 0; i < 8; i++){ 61 | value_[i] = init_val(); 62 | child_ptr_[i] = NULL; 63 | } 64 | } 65 | 66 | virtual ~Node(){}; 67 | 68 | Node *& child(const int x, const int y, 69 | const int z) { 70 | return child_ptr_[x + y*2 + z*4]; 71 | }; 72 | 73 | Node *& child(const int offset ){ 74 | return child_ptr_[offset]; 75 | } 76 | 77 | virtual bool isLeaf(){ return false; } 78 | 79 | protected: 80 | Node *child_ptr_[8]; 81 | }; 82 | 83 | template 84 | class VoxelBlock: public Node { 85 | 86 | public: 87 | 88 | typedef voxel_traits traits_type; 89 | typedef typename traits_type::ComputeType compute_type; 90 | typedef typename traits_type::StoredType stored_type; 91 | static constexpr unsigned int side = BLOCK_SIDE; 92 | static constexpr unsigned int sideSq = side*side; 93 | 94 | static constexpr compute_type empty() { 95 | return traits_type::empty(); 96 | } 97 | static constexpr stored_type initValue() { 98 | return traits_type::initValue(); 99 | } 100 | stored_type translate(const compute_type value) { 101 | return traits_type::translate(value); 102 | } 103 | compute_type translate(const stored_type value) const { 104 | return traits_type::translate(value); 105 | } 106 | 107 | VoxelBlock(){ 108 | timestamp_ = 0; 109 | coordinates_ = make_int3(0); 110 | for (unsigned int i = 0; i < side*sideSq; i++) 111 | voxel_block_[i] = initValue(); 112 | } 113 | 114 | bool isLeaf(){ return true; } 115 | 116 | int3 coordinates() const { return coordinates_; } 117 | void coordinates(const int3 c){ coordinates_ = c; } 118 | 119 | compute_type data(const int3 pos) const; 120 | void data(const int3 pos, const compute_type& value); 121 | 122 | void timestamp(const time_t frame){ 123 | timestamp_ = frame; 124 | } 125 | 126 | int timestamp() const { 127 | return timestamp_; 128 | } 129 | 130 | void active(const bool a){ active_ = a; } 131 | bool active() const { return active_; } 132 | 133 | stored_type * getBlockRawPtr(){ return voxel_block_; } 134 | static constexpr int size(){ return sizeof(VoxelBlock); } 135 | 136 | private: 137 | VoxelBlock(const VoxelBlock&) = delete; 138 | int3 coordinates_; 139 | stored_type voxel_block_[side*sideSq]; // Brick of data. 140 | time_t timestamp_; 141 | bool active_; 142 | }; 143 | 144 | template 145 | inline typename VoxelBlock::compute_type VoxelBlock::data(const int3 pos) const { 146 | int3 offset = pos - coordinates_; 147 | const stored_type& data = voxel_block_[offset.x + offset.y*side + 148 | offset.z*sideSq]; 149 | return translate(data); 150 | } 151 | 152 | template 153 | inline void VoxelBlock::data(const int3 pos, 154 | const compute_type &value){ 155 | int3 offset = pos - coordinates_; 156 | voxel_block_[offset.x + offset.y*side + offset.z*sideSq] = translate(value); 157 | } 158 | #endif 159 | -------------------------------------------------------------------------------- /lib/octree_defines.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef OCTREE_CONFIG_H 33 | #define OCTREE_CONFIG_H 34 | 35 | #include 36 | #include "math_utils.h" 37 | 38 | #define BLOCK_SIDE 8 39 | #define MAX_BITS 21 40 | #define CAST_STACK_DEPTH 23 41 | #define SCALE_MASK ((octlib::key_t)0x1FF) 42 | 43 | namespace octlib { 44 | typedef uint64_t key_t; 45 | // typedef long long int morton_type; 46 | } 47 | 48 | /* 49 | * Mask generated with: 50 | MASK[0] = 0x7000000000000000, 51 | for(int i = 1; i < 21; ++i) { 52 | MASK[i] = MASK[i-1] | (MASK[0] >> (i*3)); 53 | std::bitset<64> b(MASK[i]); 54 | std::cout << std::hex << b.to_ullong() << std::endl; 55 | } 56 | * 57 | */ 58 | constexpr uint64_t MASK[] = { 59 | 0x7000000000000000, 60 | 0x7e00000000000000, 61 | 0x7fc0000000000000, 62 | 0x7ff8000000000000, 63 | 0x7fff000000000000, 64 | 0x7fffe00000000000, 65 | 0x7ffffc0000000000, 66 | 0x7fffff8000000000, 67 | 0x7ffffff000000000, 68 | 0x7ffffffe00000000, 69 | 0x7fffffffc0000000, 70 | 0x7ffffffff8000000, 71 | 0x7fffffffff000000, 72 | 0x7fffffffffe00000, 73 | 0x7ffffffffffc0000, 74 | 0x7fffffffffff8000, 75 | 0x7ffffffffffff000, 76 | 0x7ffffffffffffe00, 77 | 0x7fffffffffffffc0, 78 | 0x7ffffffffffffff8, 79 | 0x7fffffffffffffff 80 | }; 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /lib/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(octree_lib_unit_testing) 3 | 4 | # GTest Root - Change to reflect your install dir 5 | set(GTEST_ROOT ~/software/googletest/googletest) 6 | find_package(GTest REQUIRED) 7 | 8 | enable_testing() 9 | add_compile_options(-std=c++1y ) 10 | include_directories(../../lib ../../utils ../../lib/geometry) 11 | 12 | add_subdirectory(octree) 13 | add_subdirectory(allocation) 14 | add_subdirectory(functor) 15 | add_subdirectory(interp) 16 | add_subdirectory(multiscale) 17 | add_subdirectory(algorithms) 18 | add_subdirectory(geometry) 19 | add_subdirectory(utils) 20 | -------------------------------------------------------------------------------- /lib/test/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | mkdir -p build/ 3 | cd build/ && cmake .. 4 | $(MAKE) -C build 5 | 6 | debug: 7 | mkdir -p build/ 8 | cd build/ && cmake .. -DCMAKE_BUILD_TYPE=Debug 9 | $(MAKE) -C build 10 | 11 | test: 12 | $(MAKE) -C build test 13 | 14 | clean: 15 | rm -rf build/ ./CMakeFiles 16 | 17 | .PHONY: all clean test 18 | -------------------------------------------------------------------------------- /lib/test/algorithms/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(octree_lib) 3 | 4 | 5 | set(PROJECT_TEST_NAME algorithms_unittest) 6 | add_executable(${PROJECT_TEST_NAME} unique_unittest.cpp) 7 | target_include_directories(${PROJECT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 8 | target_link_libraries(${PROJECT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 9 | 10 | GTEST_ADD_TESTS(${PROJECT_TEST_NAME} "" AUTO) 11 | -------------------------------------------------------------------------------- /lib/test/algorithms/unique_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include "octree.hpp" 33 | #include "math_utils.h" 34 | #include "utils/morton_utils.hpp" 35 | #include "algorithms/unique.hpp" 36 | #include "gtest/gtest.h" 37 | #include 38 | 39 | typedef float testT; 40 | typedef unsigned int MortonType; 41 | 42 | template <> 43 | struct voxel_traits { 44 | typedef float ComputeType; 45 | typedef float StoredType; 46 | static inline ComputeType empty(){ return 0.f; } 47 | static inline ComputeType initValue(){ return 0.f; } 48 | static inline StoredType translate(const ComputeType value) { 49 | return value; 50 | } 51 | }; 52 | 53 | class UniqueTest : public ::testing::Test { 54 | protected: 55 | virtual void SetUp() { 56 | const int3 blocks[10] = { 57 | {56, 12, 12}, {56, 12, 15}, 58 | {128, 128, 128}, 59 | {128, 128, 125}, {128, 128, 127}, 60 | {128, 136, 129}, 61 | {128, 136, 127}, 62 | {136, 128, 136}, 63 | {128, 240, 136}, {128, 241, 136}}; 64 | for(int i = 0; i < 10; ++i) { 65 | keys[i] = oct.hash(blocks[i].x, blocks[i].y, blocks[i].z); 66 | } 67 | } 68 | 69 | MortonType keys[10]; 70 | typedef Octree OctreeF; 71 | OctreeF oct; 72 | }; 73 | 74 | class UniqueMultiscaleTest : public ::testing::Test { 75 | protected: 76 | virtual void SetUp() { 77 | 78 | oct.init(256, 10); 79 | 80 | const int3 blocks[10] = { 81 | {56, 12, 12}, 82 | {56, 12, 15}, 83 | {128, 128, 128}, 84 | {128, 128, 125}, 85 | {128, 128, 127}, 86 | {128, 136, 129}, 87 | {128, 136, 127}, 88 | {136, 128, 136}, 89 | {128, 240, 136}, {128, 241, 136}}; 90 | for(int i = 0; i < 10; ++i) { 91 | keys[i] = oct.hash(blocks[i].x, blocks[i].y, blocks[i].z, 7); 92 | } 93 | 94 | keys[2] = oct.hash(blocks[2].x, blocks[2].y, blocks[2].z, 3); 95 | keys[3] = oct.hash(blocks[3].x, blocks[3].y, blocks[3].z, 5); 96 | keys[4] = oct.hash(blocks[4].x, blocks[4].y, blocks[4].z, 6); 97 | std::sort(keys, keys + 10); 98 | } 99 | 100 | MortonType keys[10]; 101 | typedef Octree OctreeF; 102 | OctreeF oct; 103 | }; 104 | 105 | TEST_F(UniqueTest, FilterDuplicates) { 106 | std::sort(keys, keys + 10); 107 | const int last = algorithms::unique(keys, 10); 108 | for(int i = 1; i < last; ++i) { 109 | ASSERT_TRUE(keys[i] != keys[i-1]); 110 | } 111 | } 112 | 113 | TEST_F(UniqueMultiscaleTest, FilterDuplicates) { 114 | std::sort(keys, keys + 10); 115 | /* 116 | * 0x1FFu extracts the last 9 bits of a morton number, 117 | * corresponding to the edge of a voxel block: 3*log2(VoxelBlock::side) 118 | */ 119 | const int last = algorithms::unique_multiscale(keys, 10, 0x1FFu); 120 | ASSERT_EQ(last, 7); 121 | for(int i = 1; i < last; ++i) { 122 | // std::cout << "(Key: " << (keys[i-1] & (~0x1FFu)) << ", Scale: " 123 | // << (keys[i-1] & 0x1FFu) << "), "; 124 | ASSERT_TRUE(keys[i] != keys[i-1]); 125 | } 126 | std::cout << std::endl; 127 | } 128 | 129 | TEST_F(UniqueMultiscaleTest, FilterDuplicatesTillLevel) { 130 | std::sort(keys, keys + 10); 131 | /* 132 | * 0x1FFu extracts the last 9 bits of a morton number, 133 | * corresponding to the edge of a voxel block: 3*log2(VoxelBlock::side) 134 | */ 135 | const int last = algorithms::unique_multiscale(keys, 10, 0x1FFu, 6); 136 | ASSERT_EQ(last, 6); 137 | for(int i = 1; i <= last; ++i) { 138 | // std::cout << "(Key: " << (keys[i-1] & (~0x1FFu)) << ", Scale: " 139 | // << (keys[i-1] & 0x1FFu) << "), "; 140 | ASSERT_TRUE(keys[i] != keys[i-1]); 141 | } 142 | std::cout << std::endl; 143 | } 144 | -------------------------------------------------------------------------------- /lib/test/allocation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(octree_lib) 3 | 4 | set(PROJECT_TEST_NAME allocation) 5 | set(UNIT_TEST_NAME ${PROJECT_TEST_NAME}-unittest) 6 | add_executable(${UNIT_TEST_NAME} alloc_unittest.cpp) 7 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 8 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 9 | 10 | gtest_add_tests(TARGET ${UNIT_TEST_NAME}) 11 | -------------------------------------------------------------------------------- /lib/test/allocation/alloc_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include 33 | #include "octree.hpp" 34 | #include "math_utils.h" 35 | #include "utils/morton_utils.hpp" 36 | #include "gtest/gtest.h" 37 | 38 | template <> 39 | struct voxel_traits { 40 | typedef float ComputeType; 41 | typedef float StoredType; 42 | static inline ComputeType empty(){ return 0.f; } 43 | static inline ComputeType initValue(){ return 0.f; } 44 | static inline StoredType translate(const ComputeType value) { 45 | return value; 46 | } 47 | }; 48 | 49 | TEST(AllocationTest, EmptySingleVoxel) { 50 | typedef Octree OctreeF; 51 | OctreeF oct; 52 | oct.init(256, 5); 53 | const int3 vox = {25, 65, 127}; 54 | const octlib::key_t code = oct.hash(vox.x, vox.y, vox.z); 55 | octlib::key_t allocList[1] = {code}; 56 | const float val = oct.get(vox.x, vox.y, vox.z); 57 | EXPECT_EQ(val, voxel_traits::empty()); 58 | } 59 | 60 | TEST(AllocationTest, SetSingleVoxel) { 61 | typedef Octree OctreeF; 62 | OctreeF oct; 63 | oct.init(256, 5); 64 | const int3 vox = {25, 65, 127}; 65 | const octlib::key_t code = oct.hash(vox.x, vox.y, vox.z); 66 | octlib::key_t allocList[1] = {code}; 67 | oct.allocate(allocList, 1); 68 | 69 | VoxelBlock * block = oct.fetch(vox.x, vox.y, vox.z); 70 | float written_val = 2.f; 71 | block->data(vox, written_val); 72 | 73 | const float read_val = oct.get(vox.x, vox.y, vox.z); 74 | EXPECT_EQ(written_val, read_val); 75 | } 76 | 77 | TEST(AllocationTest, FetchOctant) { 78 | typedef Octree OctreeF; 79 | OctreeF oct; 80 | oct.init(256, 5); 81 | const int3 vox = {25, 65, 127}; 82 | const uint code = oct.hash(vox.x, vox.y, vox.z); 83 | octlib::key_t allocList[1] = {code}; 84 | oct.allocate(allocList, 1); 85 | 86 | const int depth = 3; /* 32 voxels per side */ 87 | Node * node = oct.fetch_octant(vox.x, vox.y, vox.z, 3); 88 | 89 | EXPECT_NE(node, nullptr); 90 | } 91 | 92 | TEST(AllocationTest, MortonPrefixMask) { 93 | 94 | const unsigned int max_bits = 21; 95 | const unsigned int block_side = 8; 96 | const unsigned int size = std::pow(2, max_bits); 97 | std::random_device rd; 98 | std::mt19937 gen(rd()); 99 | std::uniform_int_distribution dis(0, size); 100 | 101 | constexpr int num_samples = 10; 102 | octlib::key_t keys[num_samples]; 103 | octlib::key_t tempkeys[num_samples]; 104 | int3 coordinates[num_samples]; 105 | 106 | for(int i = 0; i < num_samples; ++i) { 107 | const uint3 vox = {dis(gen), dis(gen), dis(gen)}; 108 | coordinates[i] = make_int3(vox); 109 | const octlib::key_t code = compute_morton(vox.x, vox.y, vox.z); 110 | keys[i] = code; 111 | } 112 | 113 | const int max_level = log2(size); 114 | const int leaf_level = max_level - log2(block_side); 115 | const unsigned int shift = max_bits - max_level; 116 | int edge = size/2; 117 | for (int level = 0; level <= leaf_level; level++){ 118 | const octlib::key_t mask = MASK[level + shift]; 119 | compute_prefix(keys, tempkeys, num_samples, mask); 120 | for(int i = 0; i < num_samples; ++i) { 121 | const uint3 masked_vox = unpack_morton(tempkeys[i]); 122 | ASSERT_EQ(masked_vox.x % edge, 0); 123 | ASSERT_EQ(masked_vox.y % edge, 0); 124 | ASSERT_EQ(masked_vox.z % edge, 0); 125 | const int3 vox = coordinates[i]; 126 | // printf("vox: %d, %d, %d\n", vox.x, vox.y, vox.z); 127 | // printf("masked level %d: %d, %d, %d\n", level, masked_vox.x, masked_vox.y, masked_vox.z ); 128 | } 129 | edge = edge/2; 130 | } 131 | } 132 | -------------------------------------------------------------------------------- /lib/test/functor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | set(PROJECT_TEST_NAME functors-unittest) 4 | 5 | set(UNIT_TEST_NAME aa-functor-unittest) 6 | add_executable(${UNIT_TEST_NAME} axisaligned_unittest.cpp) 7 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 8 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 9 | 10 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 11 | -------------------------------------------------------------------------------- /lib/test/functor/axisaligned_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include "octree.hpp" 33 | #include "math_utils.h" 34 | #include "gtest/gtest.h" 35 | #include "functors/axis_aligned_functor.hpp" 36 | 37 | typedef float testT; 38 | template <> 39 | struct voxel_traits { 40 | typedef float ComputeType; 41 | typedef float StoredType; 42 | static inline ComputeType empty(){ return 0.f; } 43 | static inline ComputeType initValue(){ return 0.f; } 44 | static inline StoredType translate(const ComputeType value) { 45 | return value; 46 | } 47 | }; 48 | 49 | class AxisAlignedTest : public ::testing::Test { 50 | protected: 51 | virtual void SetUp() { 52 | unsigned size = 256; 53 | float dim = 5.f; 54 | oct_.init(size, dim); // 5 meters 55 | 56 | const unsigned center = 2.5f; 57 | 58 | const float voxelsize = oct_.dim()/oct_.size(); 59 | const float inverse_voxelsize = 1.f/voxelsize; 60 | const int band = 1 * inverse_voxelsize; 61 | const int3 offset = make_int3(oct_.size()/2 - band/2); 62 | unsigned leaf_level = log2(size) - log2(Octree::blockSide); 63 | for(int z = 0; z < band; ++z) { 64 | for(int y = 0; y < band; ++y) { 65 | for(int x = 0; x < band; ++x) { 66 | const int3 vox = make_int3(x + offset.x, y + offset.y, z + offset.z); 67 | alloc_list.push_back(oct_.hash(vox.x, vox.y, vox.z, leaf_level)); 68 | } 69 | } 70 | } 71 | oct_.alloc_update(alloc_list.data(), alloc_list.size()); 72 | } 73 | 74 | typedef Octree OctreeF; 75 | OctreeF oct_; 76 | std::vector alloc_list; 77 | }; 78 | 79 | TEST_F(AxisAlignedTest, Init) { 80 | 81 | auto initialise = [](auto& handler, const int3&) { 82 | handler.set(voxel_traits::initValue()); 83 | }; 84 | 85 | auto test = [](auto& handler, const int3&) { 86 | auto data = handler.get(); 87 | ASSERT_EQ(data, voxel_traits::initValue()); 88 | }; 89 | 90 | iterators::functor::axis_aligned 91 | funct(oct_, initialise); 92 | funct.apply(); 93 | 94 | iterators::functor::axis_aligned 95 | funct_test(oct_, test); 96 | funct_test.apply(); 97 | } 98 | 99 | TEST_F(AxisAlignedTest, BBoxTest) { 100 | 101 | auto set_to_ten = [](auto& handler, const int3&) { 102 | handler.set(10.f); 103 | }; 104 | 105 | iterators::functor::axis_aligned 106 | funct(oct_, set_to_ten, make_int3(100), make_int3(151)); 107 | funct.apply(); 108 | 109 | for(int z = 50; z < 200; ++z) 110 | for(int y = 50; y < 200; ++y) 111 | for(int x = 50; x < 200; ++x) { 112 | auto * block = oct_.fetch(x, y, z); 113 | if(block && in(x, 100, 150) && in(y, 100, 150) && in(z, 100, 150)){ 114 | ASSERT_EQ(block->data(make_int3(x, y, z)), 10.f); 115 | } 116 | else if(block) { 117 | ASSERT_EQ(block->data(make_int3(x, y, z)), voxel_traits::initValue()); 118 | } 119 | } 120 | } 121 | -------------------------------------------------------------------------------- /lib/test/geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(octree_lib) 3 | 4 | 5 | set(PROJECT_TEST_NAME collision) 6 | set(UNIT_TEST_NAME aabb-${PROJECT_TEST_NAME}-unittest) 7 | add_executable(${UNIT_TEST_NAME} aabb_collision_unittest.cpp) 8 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 9 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 10 | 11 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 12 | 13 | set(UNIT_TEST_NAME octree-${PROJECT_TEST_NAME}-unittest) 14 | add_executable(${UNIT_TEST_NAME} octree_collision_unittest.cpp) 15 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 16 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 17 | 18 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 19 | -------------------------------------------------------------------------------- /lib/test/geometry/aabb_collision_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include "math_utils.h" 33 | #include "geometry/aabb_collision.hpp" 34 | #include "gtest/gtest.h" 35 | 36 | TEST(AABBAABBTest, SquareOverlap) { 37 | 38 | const int3 a = make_int3(5, 6, 9); 39 | const int3 a_edge = make_int3(3); 40 | const int3 b = make_int3(4, 4, 8); 41 | const int3 b_edge = make_int3(3); 42 | int overlaps = geometry::aabb_aabb_collision(a, a_edge, b, b_edge); 43 | ASSERT_EQ(overlaps, 1); 44 | } 45 | 46 | TEST(AABBAABBTest, SquareDisjoint1Axis) { 47 | 48 | const int3 a = make_int3(5, 6, 9); 49 | const int3 a_edge = make_int3(3); 50 | const int3 b = make_int3(4, 4, 1); 51 | const int3 b_edge = make_int3(3); 52 | int overlaps = geometry::aabb_aabb_collision(a, a_edge, b, b_edge); 53 | ASSERT_EQ(overlaps, 0); 54 | } 55 | 56 | TEST(AABBAABBTest, SquareDisjoint2Axis) { 57 | /* Disjoint on y and z */ 58 | const int3 a = make_int3(5, 6, 9); 59 | const int3 a_edge = make_int3(3); 60 | const int3 b = make_int3(6, 22, 13); 61 | const int3 b_edge = make_int3(10); 62 | 63 | int overlapx = geometry::axis_overlap(a.x, a_edge.x, b.x, b_edge.x); 64 | int overlapy = geometry::axis_overlap(a.y, a_edge.y, b.y, b_edge.y); 65 | int overlapz = geometry::axis_overlap(a.z, a_edge.z, b.z, b_edge.z); 66 | 67 | ASSERT_EQ(overlapx, 1); 68 | ASSERT_EQ(overlapy, 0); 69 | ASSERT_EQ(overlapz, 0); 70 | 71 | int overlaps = geometry::aabb_aabb_collision(a, a_edge, b, b_edge); 72 | ASSERT_EQ(overlaps, 0); 73 | } 74 | 75 | TEST(AABBAABBTest, SquareDisjoint) { 76 | /* Disjoint on x, y and z */ 77 | const int3 a = make_int3(5, 6, 9); 78 | const int3 a_edge = make_int3(4); 79 | const int3 b = make_int3(12, 22, 43); 80 | const int3 b_edge = make_int3(10); 81 | 82 | int overlapx = geometry::axis_overlap(a.x, a_edge.x, b.x, b_edge.x); 83 | int overlapy = geometry::axis_overlap(a.y, a_edge.y, b.y, b_edge.y); 84 | int overlapz = geometry::axis_overlap(a.z, a_edge.z, b.z, b_edge.z); 85 | 86 | ASSERT_EQ(overlapx, 0); 87 | ASSERT_EQ(overlapy, 0); 88 | ASSERT_EQ(overlapz, 0); 89 | 90 | int overlaps = geometry::aabb_aabb_collision(a, a_edge, b, b_edge); 91 | ASSERT_EQ(overlaps, 0); 92 | } 93 | 94 | TEST(AABBAABBTest, SquareEnclosed) { 95 | /* Disjoint on x, y and z */ 96 | const int3 a = make_int3(5, 6, 9); 97 | const int3 a_edge = make_int3(10); 98 | const int3 b = make_int3(6, 7, 10); 99 | const int3 b_edge = make_int3(2); 100 | 101 | int overlapx = geometry::axis_overlap(a.x, a_edge.x, b.x, b_edge.x); 102 | int overlapy = geometry::axis_overlap(a.y, a_edge.y, b.y, b_edge.y); 103 | int overlapz = geometry::axis_overlap(a.z, a_edge.z, b.z, b_edge.z); 104 | 105 | ASSERT_EQ(overlapx, 1); 106 | ASSERT_EQ(overlapy, 1); 107 | ASSERT_EQ(overlapz, 1); 108 | 109 | int overlaps = geometry::aabb_aabb_collision(a, a_edge, b, b_edge); 110 | ASSERT_EQ(overlaps, 1); 111 | } 112 | 113 | TEST(AABBAABBTest, Inclusion) { 114 | const int3 a = make_int3(2, 1, 3); 115 | const int3 a_edge = make_int3(10); 116 | const int3 b = make_int3(3, 4, 5); 117 | const int3 b_edge = make_int3(2); 118 | int included = geometry::aabb_aabb_inclusion(a, a_edge, b, b_edge); 119 | ASSERT_EQ(included, 1); 120 | } 121 | -------------------------------------------------------------------------------- /lib/test/interp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | set(PROJECT_TEST_NAME interpolation_unittest) 4 | 5 | add_library(lodepng SHARED ../../../apps/kfusion/thirdparty/lodepng.cpp) 6 | 7 | set(UNIT_TEST_NAME gather-unittest) 8 | add_executable(${UNIT_TEST_NAME} gather_unittest.cpp) 9 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 10 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 11 | 12 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 13 | 14 | set(UNIT_TEST_NAME interpolation-unittest) 15 | add_executable(${UNIT_TEST_NAME} interpolation_unittest.cpp) 16 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS} ../../../apps/kfusion/thirdparty) 17 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread lodepng) 18 | 19 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 20 | 21 | -------------------------------------------------------------------------------- /lib/test/interp/interpolation_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include 33 | #include "octree.hpp" 34 | #include "math_utils.h" 35 | #include "gtest/gtest.h" 36 | #include "functors/axis_aligned_functor.hpp" 37 | #include "../../../apps/kfusion/include/vtk-io.h" 38 | 39 | typedef float testT; 40 | template <> 41 | struct voxel_traits { 42 | typedef float1 ComputeType; 43 | typedef float1 StoredType; 44 | static inline ComputeType empty(){ return {0.f}; } 45 | static inline ComputeType initValue(){ return {1.f}; } 46 | static inline StoredType translate(const ComputeType value) { 47 | return value; 48 | } 49 | }; 50 | 51 | float test_fun(float x, float y, float z) { 52 | return sq(z) + std::sin(2*x + y); 53 | } 54 | 55 | class InterpolationTest : public ::testing::Test { 56 | protected: 57 | virtual void SetUp() { 58 | unsigned size = 512; 59 | float dim = 5.f; 60 | oct_.init(size, dim); // 5 meters 61 | 62 | const unsigned center = 2.5f; 63 | const unsigned radius = center + 0.5f; 64 | 65 | const float voxelsize = oct_.dim()/oct_.size(); 66 | const float inverse_voxelsize = 1.f/voxelsize; 67 | const int band = 1 * inverse_voxelsize; 68 | const int3 offset = make_int3(oct_.size()/2 - band/2); 69 | unsigned leaf_level = log2(size) - log2(Octree::blockSide); 70 | for(int z = 0; z < band; ++z) { 71 | for(int y = 0; y < band; ++y) { 72 | for(int x = 0; x < band; ++x) { 73 | const int3 vox = make_int3(x + offset.x, y + offset.y, z + offset.z); 74 | alloc_list.push_back(oct_.hash(vox.x, vox.y, vox.z, leaf_level)); 75 | } 76 | } 77 | } 78 | oct_.alloc_update(alloc_list.data(), alloc_list.size()); 79 | } 80 | 81 | typedef Octree OctreeF; 82 | OctreeF oct_; 83 | std::vector alloc_list; 84 | }; 85 | 86 | TEST_F(InterpolationTest, Init) { 87 | 88 | auto initialise = [](auto& handler, const int3& v) { 89 | float1 data; 90 | data.x = test_fun(v.x, v.y, v.z); 91 | handler.set(data); 92 | }; 93 | 94 | iterators::functor::axis_aligned 95 | funct(oct_, initialise); 96 | funct.apply(); 97 | 98 | auto test = [](auto& handler, const int3& v) { 99 | auto data = handler.get(); 100 | ASSERT_EQ(data.x, test_fun(v.x, v.y, v.z)); 101 | }; 102 | iterators::functor::axis_aligned 103 | funct_test(oct_, test); 104 | funct_test.apply(); 105 | 106 | std::stringstream f; 107 | f << "./analytical_function.vtk"; 108 | save3DSlice(oct_, make_int3(0, oct_.size()/2, 0), 109 | make_int3(oct_.size(), oct_.size()/2 + 1, oct_.size()), make_int3(oct_.size()), f.str().c_str()); 110 | f.str(""); 111 | f.clear(); 112 | } 113 | 114 | // TEST_F(InterpolationTest, InterpAtPoints) { 115 | // 116 | // auto test = [this](auto& handler, const int3& v) { 117 | // auto data = handler.get(); 118 | // float interpolated = oct_.interp(make_float3(v.x, v.y, v.z), [](const auto& val){ return val.x; }); 119 | // ASSERT_EQ(data.x, interpolated); 120 | // }; 121 | // 122 | // iterators::functor::axis_aligned 123 | // funct_test(oct_, test); 124 | // funct_test.apply(); 125 | // } 126 | -------------------------------------------------------------------------------- /lib/test/multiscale/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(octree_lib) 3 | 4 | 5 | set(PROJECT_TEST_NAME multiscale_unittest) 6 | add_executable(${PROJECT_TEST_NAME} multiscale_unittest.cpp) 7 | target_include_directories(${PROJECT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 8 | target_link_libraries(${PROJECT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 9 | 10 | GTEST_ADD_TESTS(multiscale_unittest "" multiscale_unittest.cpp) 11 | -------------------------------------------------------------------------------- /lib/test/multiscale/multiscale_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include "octree.hpp" 33 | #include "math_utils.h" 34 | #include "gtest/gtest.h" 35 | 36 | typedef float testT; 37 | 38 | template <> 39 | struct voxel_traits { 40 | typedef float ComputeType; 41 | typedef float StoredType; 42 | static inline ComputeType empty(){ return 0.f; } 43 | static inline ComputeType initValue(){ return 1.f; } 44 | static inline StoredType translate(const ComputeType value) { 45 | return value; 46 | } 47 | }; 48 | 49 | class MultiscaleTest : public ::testing::Test { 50 | protected: 51 | virtual void SetUp() { 52 | oct_.init(512, 5); 53 | 54 | } 55 | 56 | typedef Octree OctreeF; 57 | OctreeF oct_; 58 | }; 59 | 60 | TEST_F(MultiscaleTest, Init) { 61 | EXPECT_EQ(oct_.get(137, 138, 130), voxel_traits::initValue()); 62 | } 63 | 64 | TEST_F(MultiscaleTest, PlainAlloc) { 65 | const int3 blocks[2] = {{56, 12, 254}, {87, 32, 423}}; 66 | octlib::key_t alloc_list[2]; 67 | for(int i = 0; i < 2; ++i) { 68 | alloc_list[i] = oct_.hash(blocks[i].x, blocks[i].y, blocks[i].z); 69 | } 70 | oct_.allocate(alloc_list, 2); 71 | 72 | oct_.set(56, 12, 254, 3.f); 73 | 74 | EXPECT_EQ(oct_.get(56, 12, 254), 3.f); 75 | EXPECT_EQ(oct_.get(106, 12, 254), voxel_traits::initValue()); 76 | EXPECT_NE(oct_.get(106, 12, 254), 3.f); 77 | } 78 | 79 | TEST_F(MultiscaleTest, ScaledAlloc) { 80 | const int3 blocks[2] = {{200, 12, 25}, {87, 32, 423}}; 81 | octlib::key_t alloc_list[2]; 82 | for(int i = 0; i < 2; ++i) { 83 | alloc_list[i] = oct_.hash(blocks[i].x, blocks[i].y, blocks[i].z, 5); 84 | } 85 | 86 | oct_.alloc_update(alloc_list, 2); 87 | Node* n = oct_.fetch_octant(87, 32, 420, 5); 88 | ASSERT_TRUE(n != NULL); 89 | n->value_[0] = 10.f; 90 | EXPECT_EQ(oct_.get(87, 32, 420), 10.f); 91 | } 92 | 93 | TEST_F(MultiscaleTest, Iterator) { 94 | const int3 blocks[1] = {{56, 12, 254}}; 95 | octlib::key_t alloc_list[1]; 96 | alloc_list[0] = oct_.hash(blocks[0].x, blocks[0].y, blocks[0].z); 97 | 98 | oct_.alloc_update(alloc_list, 1); 99 | leaf_iterator it(oct_); 100 | 101 | typedef std::tuple::compute_type> it_result; 102 | it_result node = it.next(); 103 | for(int i = 256; std::get<1>(node) > 0; node = it.next(), i /= 2){ 104 | const int3 coords = std::get<0>(node); 105 | const int side = std::get<1>(node); 106 | const Octree::compute_type val = std::get<2>(node); 107 | EXPECT_EQ(side, i); 108 | } 109 | } 110 | 111 | TEST_F(MultiscaleTest, ChildrenMaskTest) { 112 | const int3 blocks[10] = {{56, 12, 254}, {87, 32, 423}, {128, 128, 128}, 113 | {136, 128, 128}, {128, 136, 128}, {136, 136, 128}, 114 | {128, 128, 136}, {136, 128, 136}, {128, 136, 136}, {136, 136, 136}}; 115 | octlib::key_t alloc_list[10]; 116 | for(int i = 0; i < 10; ++i) { 117 | alloc_list[i] = oct_.hash(blocks[i].x, blocks[i].y, blocks[i].z, 5); 118 | } 119 | 120 | oct_.alloc_update(alloc_list, 10); 121 | const MemoryPool >& nodes = oct_.getNodesBuffer(); 122 | const size_t num_nodes = nodes.size(); 123 | for(size_t i = 0; i < num_nodes; ++i) { 124 | Node* n = nodes[i]; 125 | for(int c = 0; c < 8; ++c) { 126 | if(n->child(c)) { 127 | ASSERT_TRUE(n->children_mask_ & (1 << c)); 128 | } 129 | } 130 | } 131 | } 132 | 133 | TEST_F(MultiscaleTest, OctantAlloc) { 134 | const int3 blocks[10] = {{56, 12, 254}, {87, 32, 423}, {128, 128, 128}, 135 | {136, 128, 128}, {128, 136, 128}, {136, 136, 128}, 136 | {128, 128, 136}, {136, 128, 136}, {128, 136, 136}, {136, 136, 136}}; 137 | octlib::key_t alloc_list[10]; 138 | for(int i = 0; i < 10; ++i) { 139 | alloc_list[i] = oct_.hash(blocks[i].x, blocks[i].y, blocks[i].z); 140 | } 141 | 142 | alloc_list[2] = alloc_list[2] | 3; 143 | alloc_list[9] = alloc_list[2] | 5; 144 | oct_.alloc_update(alloc_list, 10); 145 | Node * octant = oct_.fetch_octant(blocks[4].x, blocks[4].y, 146 | blocks[4].z, 3); 147 | ASSERT_TRUE(octant != NULL); 148 | octant = oct_.fetch_octant(blocks[9].x, blocks[9].y, 149 | blocks[9].z, 6); 150 | ASSERT_TRUE(octant == NULL); 151 | } 152 | -------------------------------------------------------------------------------- /lib/test/octree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(octree_lib) 3 | 4 | 5 | set(PROJECT_TEST_NAME octree) 6 | set(UNIT_TEST_NAME ${PROJECT_TEST_NAME}-unittest) 7 | add_executable(${UNIT_TEST_NAME} octree_unittest.cpp) 8 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 9 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 10 | 11 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 12 | 13 | -------------------------------------------------------------------------------- /lib/test/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(octree_lib) 3 | 4 | 5 | set(PROJECT_TEST_NAME utils) 6 | set(UNIT_TEST_NAME ${PROJECT_TEST_NAME}-morton-unittest) 7 | add_executable(${UNIT_TEST_NAME} morton_unittest.cpp) 8 | target_include_directories(${UNIT_TEST_NAME} PUBLIC ${GTEST_INCLUDE_DIRS}) 9 | target_link_libraries(${UNIT_TEST_NAME} ${GTEST_BOTH_LIBRARIES} pthread) 10 | 11 | GTEST_ADD_TESTS(${UNIT_TEST_NAME} "" AUTO) 12 | 13 | -------------------------------------------------------------------------------- /lib/test/utils/morton_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include 33 | #include "math_utils.h" 34 | #include "utils/morton_utils.hpp" 35 | #include "octree_defines.h" 36 | #include "gtest/gtest.h" 37 | 38 | TEST(MortonCoding, RandomInts) { 39 | 40 | std::random_device rd; 41 | std::mt19937 gen(rd()); 42 | std::uniform_int_distribution dis(0, 4096); 43 | 44 | for(int i = 0; i < 1000; ++i) { 45 | const uint3 vox = {dis(gen), dis(gen), dis(gen)}; 46 | const octlib::key_t code = compute_morton(vox.x, vox.y, vox.z); 47 | const uint3 decoded = unpack_morton(code); 48 | ASSERT_EQ(decoded.x, vox.x); 49 | ASSERT_EQ(decoded.y, vox.y); 50 | ASSERT_EQ(decoded.z, vox.z); 51 | } 52 | 53 | } 54 | 55 | TEST(MortonCoding, ExhaustiveTest) { 56 | 57 | for(unsigned int z = 2048; z < 4096; ++z) 58 | for(unsigned int y = 2048; y < 2050; ++y) 59 | for(unsigned int x = 0; x < 4096; ++x){ 60 | const uint3 vox = {x, y, z}; 61 | const octlib::key_t code = compute_morton(vox.x, vox.y, vox.z); 62 | const uint3 decoded = unpack_morton(code); 63 | ASSERT_EQ(decoded.x, vox.x); 64 | ASSERT_EQ(decoded.y, vox.y); 65 | ASSERT_EQ(decoded.z, vox.z); 66 | } 67 | } 68 | 69 | -------------------------------------------------------------------------------- /lib/utils/memory_pool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef MEM_POOL_H 33 | #define MEM_POOL_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | template 44 | class MemoryPool { 45 | 46 | public: 47 | 48 | MemoryPool(){ 49 | current_block_ = 0; 50 | num_pages_ = 0; 51 | reserved_ = 0; 52 | } 53 | 54 | ~MemoryPool(){ 55 | for(auto&& i : pages_){ 56 | delete [] i; 57 | } 58 | } 59 | 60 | size_t size() const { return current_block_; }; 61 | 62 | BlockType* operator[](const size_t i) const { 63 | const int page_idx = i / pagesize_; 64 | const int ptr_idx = i % pagesize_; 65 | return pages_[page_idx] + (ptr_idx); 66 | } 67 | 68 | 69 | void reserve(const size_t n){ 70 | bool requires_realloc = (current_block_ + n) > reserved_; 71 | if(requires_realloc) expand(n); 72 | } 73 | 74 | BlockType * acquire_block(){ 75 | // Fetch-add returns the value before increment 76 | int current = current_block_.fetch_add(1); 77 | const int page_idx = current / pagesize_; 78 | const int ptr_idx = current % pagesize_; 79 | BlockType * ptr = pages_[page_idx] + (ptr_idx); 80 | return ptr; 81 | } 82 | 83 | BlockType * operator[](const int i){ 84 | const int page_idx = i / pagesize_; 85 | const int ptr_idx = i % pagesize_; 86 | BlockType * ptr = pages_[page_idx] + (ptr_idx); 87 | return ptr; 88 | } 89 | 90 | private: 91 | 92 | size_t reserved_; 93 | std::atomic current_block_; 94 | const int pagesize_ = 1024; // # of blocks per page 95 | int num_pages_; 96 | std::vector pages_; 97 | 98 | void expand(const size_t n){ 99 | 100 | // std::cout << "Allocating " << n << " blocks" << std::endl; 101 | const int new_pages = std::ceil(n/pagesize_); 102 | for(int p = 0; p <= new_pages; ++p){ 103 | pages_.push_back(new BlockType[pagesize_]); 104 | ++num_pages_; 105 | reserved_ += pagesize_; 106 | } 107 | // std::cout << "Reserved " << reserved_ << " blocks" << std::endl; 108 | } 109 | 110 | // Disabling copy-constructor 111 | MemoryPool(const MemoryPool& m); 112 | }; 113 | #endif 114 | -------------------------------------------------------------------------------- /lib/utils/morton_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef MORTON_UTILS_HPP 33 | #define MORTON_UTILS_HPP 34 | #include 35 | #include 36 | #include 37 | 38 | inline uint64_t expand(unsigned long long value) { 39 | uint64_t x = value & 0x1fffff; 40 | x = (x | x << 32) & 0x1f00000000ffff; 41 | x = (x | x << 16) & 0x1f0000ff0000ff; 42 | x = (x | x << 8) & 0x100f00f00f00f00f; 43 | x = (x | x << 4) & 0x10c30c30c30c30c3; 44 | x = (x | x << 2) & 0x1249249249249249; 45 | return x; 46 | } 47 | 48 | inline uint64_t compact(uint64_t value) { 49 | uint64_t x = value & 0x1249249249249249; 50 | x = (x | x >> 2) & 0x10c30c30c30c30c3; 51 | x = (x | x >> 4) & 0x100f00f00f00f00f; 52 | x = (x | x >> 8) & 0x1f0000ff0000ff; 53 | x = (x | x >> 16) & 0x1f00000000ffff; 54 | x = (x | x >> 32) & 0x1fffff; 55 | return x; 56 | } 57 | 58 | inline uint3 unpack_morton(uint64_t code){ 59 | return make_uint3(compact(code >> 0ull), compact(code >> 1ull), 60 | compact(code >> 2ull)); 61 | } 62 | 63 | inline uint64_t compute_morton(uint64_t x, 64 | uint64_t y, uint64_t z){ 65 | uint64_t code = 0; 66 | 67 | x = expand(x); 68 | y = expand(y) << 1; 69 | z = expand(z) << 2; 70 | 71 | code = x | y | z; 72 | return code; 73 | } 74 | 75 | static inline void compute_prefix(const octlib::key_t * in, octlib::key_t * out, 76 | unsigned int num_keys, const octlib::key_t mask){ 77 | 78 | #pragma omp parallel for 79 | for (unsigned int i = 0; i < num_keys; i++){ 80 | out[i] = in[i] & mask; 81 | } 82 | } 83 | #endif 84 | -------------------------------------------------------------------------------- /lib/voxel_traits.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright 2016 Emanuele Vespa, Imperial College London 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef _VOXEL_TRAITS_ 33 | #define _VOXEL_TRAITS_ 34 | #include 35 | 36 | template 37 | struct voxel_traits{ }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /teaser/midfusion.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smartroboticslab/mid-fusion/034f1df6eb9f5450ec1db32afc2bcdd0c0a0278d/teaser/midfusion.gif -------------------------------------------------------------------------------- /utils/download_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | list = ['1Z2Dfw0IRFgAlGPz8UNSaJaCY-g9ok3Ml', 4 | '1SZTQwDQRHfrRZbt91MFsFgs8hr8jfWGY', 5 | '1iaJ4PHZmDwX3xWLI94SMdjRycHND-QhM', 6 | '1sF4IjNXFxb_aBLhZ3OZ93ghN8GWZwWzX', 7 | '1AYH51h2aONg0v8_k1rNCh6s4q0T977pG', 8 | ] -------------------------------------------------------------------------------- /utils/timings.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPDX-FileCopyrightText: 2011-2013 Gerhard Reitmayr, TU Graz 3 | * SPDX-FileCopyrightText: 2014 University of Edinburgh, Imperial College, University of Manchester 4 | * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa 5 | * SPDX-License-Identifier: MIT 6 | */ 7 | 8 | #ifndef TIMINGS_H 9 | #define TIMINGS_H 10 | 11 | #include 12 | #include "perfstats.h" 13 | 14 | extern PerfStats Stats; 15 | 16 | #ifdef __APPLE__ 17 | #include 18 | #include 19 | 20 | #define TICK() {if (print_kernel_timing) {\ 21 | host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);\ 22 | clock_get_time(cclock, &tick_clockData);\ 23 | mach_port_deallocate(mach_task_self(), cclock);\ 24 | }} 25 | 26 | #define TOCK(str,size) {if (print_kernel_timing) {\ 27 | host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);\ 28 | clock_get_time(cclock, &tock_clockData);\ 29 | mach_port_deallocate(mach_task_self(), cclock);\ 30 | std::cerr<< str << " ";\ 31 | if((tock_clockData.tv_sec > tick_clockData.tv_sec) && (tock_clockData.tv_nsec >= tick_clockData.tv_nsec)) std::cerr<< tock_clockData.tv_sec - tick_clockData.tv_sec << std::setfill('0') << std::setw(9);\ 32 | std::cerr << (( tock_clockData.tv_nsec - tick_clockData.tv_nsec) + ((tock_clockData.tv_nsec tick_clockData.tv_sec) && (tock_clockData.tv_nsec >= tick_clockData.tv_nsec))*/ { Stats.sample(str, (( tock_clockData.tv_nsec - tick_clockData.tv_nsec) + ((tock_clockData.tv_nsec