├── .clang-format ├── .devcontainer ├── Dockerfile └── devcontainer.json ├── .github └── workflows │ ├── build_test_18.yml │ ├── build_test_20.yml │ └── docker-publish.yml ├── .gitignore ├── Dockerfile ├── LICENSE ├── README.md ├── panoptic_mapping ├── CMakeLists.txt ├── README.md ├── include │ └── panoptic_mapping │ │ ├── 3rd_party │ │ ├── config_utilities.hpp │ │ ├── csv.h │ │ └── nanoflann.hpp │ │ ├── common │ │ ├── camera.h │ │ ├── common.h │ │ ├── globals.h │ │ ├── index_getter.h │ │ ├── input_data.h │ │ └── input_data_user.h │ │ ├── integration │ │ ├── class_projective_tsdf_integrator.h │ │ ├── mesh_integrator.h │ │ ├── projection_interpolators.h │ │ ├── projective_tsdf_integrator.h │ │ ├── single_tsdf_integrator.h │ │ └── tsdf_integrator_base.h │ │ ├── labels │ │ ├── csv_label_handler.h │ │ ├── label_entry.h │ │ ├── label_handler_base.h │ │ ├── null_label_handler.h │ │ └── range_label_handler.h │ │ ├── map │ │ ├── classification │ │ │ ├── binary_count.h │ │ │ ├── class_block.h │ │ │ ├── class_layer.h │ │ │ ├── class_layer_impl.h │ │ │ ├── class_voxel.h │ │ │ ├── fixed_count.h │ │ │ ├── moving_binary_count.h │ │ │ ├── uncertainty.h │ │ │ └── variable_count.h │ │ ├── instance_id.h │ │ ├── scores │ │ │ ├── average.h │ │ │ ├── latest.h │ │ │ ├── score_block.h │ │ │ ├── score_layer.h │ │ │ ├── score_layer_impl.h │ │ │ └── score_voxel.h │ │ ├── submap.h │ │ ├── submap_bounding_volume.h │ │ ├── submap_collection.h │ │ └── submap_id.h │ │ ├── map_management │ │ ├── activity_manager.h │ │ ├── layer_manipulator.h │ │ ├── map_manager.h │ │ ├── map_manager_base.h │ │ ├── null_map_manager.h │ │ └── tsdf_registrator.h │ │ ├── submap_allocation │ │ ├── freespace_allocator_base.h │ │ ├── monolithic_freespace_allocator.h │ │ ├── null_submap_allocator.h │ │ ├── semantic_submap_allocator.h │ │ └── submap_allocator_base.h │ │ ├── test │ │ ├── comparison_utils.h │ │ ├── randomization_utils.h │ │ └── temporary_file.h │ │ ├── tools │ │ ├── coloring.h │ │ ├── data_writer_base.h │ │ ├── evaluation_data_writer.h │ │ ├── log_data_writer.h │ │ ├── map_renderer.h │ │ ├── null_data_writer.h │ │ ├── planning_interface.h │ │ ├── serialization.h │ │ └── thread_safe_submap_collection.h │ │ └── tracking │ │ ├── detectron_id_tracker.h │ │ ├── ground_truth_id_tracker.h │ │ ├── id_tracker_base.h │ │ ├── projective_id_tracker.h │ │ ├── single_tsdf_tracker.h │ │ └── tracking_info.h ├── package.xml ├── proto │ └── panoptic_mapping │ │ ├── ClassLayer.proto │ │ ├── Submap.proto │ │ └── SubmapCollection.proto ├── src │ ├── common │ │ ├── camera.cpp │ │ ├── class_voxel_layer.cpp │ │ └── input_data_user.cpp │ ├── integration │ │ ├── class_projective_tsdf_integrator.cpp │ │ ├── mesh_integrator.cpp │ │ ├── projection_interpolators.cpp │ │ ├── projective_tsdf_integrator.cpp │ │ └── single_tsdf_integrator.cpp │ ├── labels │ │ ├── csv_label_handler.cpp │ │ ├── label_handler_base.cpp │ │ ├── null_label_handler.cpp │ │ └── range_label_handler.cpp │ ├── map │ │ ├── classification │ │ │ ├── binary_count.cpp │ │ │ ├── fixed_count.cpp │ │ │ ├── moving_binary_count.cpp │ │ │ ├── uncertainty.cpp │ │ │ └── variable_count.cpp │ │ ├── instance_id.cpp │ │ ├── scores │ │ │ ├── average.cpp │ │ │ └── latest.cpp │ │ ├── submap.cpp │ │ ├── submap_bounding_volume.cpp │ │ ├── submap_collection.cpp │ │ └── submap_id.cpp │ ├── map_management │ │ ├── activity_manager.cpp │ │ ├── layer_manipulator.cpp │ │ ├── map_manager.cpp │ │ ├── null_map_manager.cpp │ │ └── tsdf_registrator.cpp │ ├── submap_allocation │ │ ├── monolithic_freespace_allocator.cpp │ │ ├── null_submap_allocator.cpp │ │ └── semantic_submap_allocator.cpp │ ├── tools │ │ ├── evaluation_data_writer.cpp │ │ ├── log_data_writer.cpp │ │ ├── map_renderer.cpp │ │ ├── null_data_writer.cpp │ │ ├── planning_interface.cpp │ │ └── serialization.cpp │ └── tracking │ │ ├── detectron_id_tracker.cpp │ │ ├── ground_truth_id_tracker.cpp │ │ ├── projective_id_tracker.cpp │ │ ├── single_tsdf_tracker.cpp │ │ └── tracking_info.cpp └── test │ └── serialization.cpp ├── panoptic_mapping_https.rosinstall ├── panoptic_mapping_msgs ├── CMakeLists.txt ├── README.md ├── msg │ ├── DetectronLabel.msg │ └── DetectronLabels.msg ├── package.xml └── srv │ ├── SaveLoadMap.srv │ └── SetVisualizationMode.srv ├── panoptic_mapping_ros ├── CMakeLists.txt ├── README.md ├── app │ └── panoptic_mapper_node.cpp ├── config │ ├── mapper │ │ ├── flat_detectron.yaml │ │ ├── flat_groundtruth.yaml │ │ ├── rio_detectron.yaml │ │ ├── rio_groundtruth.yaml │ │ └── single_tsdf.yaml │ └── rviz │ │ ├── devel.rviz │ │ ├── test_detectron_ids.rviz │ │ ├── vis.rviz │ │ └── voxblox.rviz ├── include │ └── panoptic_mapping_ros │ │ ├── conversions │ │ └── conversions.h │ │ ├── input │ │ ├── input_subscriber.h │ │ └── input_synchronizer.h │ │ ├── panoptic_mapper.h │ │ └── visualization │ │ ├── planning_visualizer.h │ │ ├── single_tsdf_visualizer.h │ │ ├── submap_visualizer.h │ │ └── tracking_visualizer.h ├── launch │ └── run.launch ├── package.xml └── src │ ├── conversions │ └── conversions.cpp │ ├── input │ └── input_synchronizer.cpp │ ├── panoptic_mapper.cpp │ └── visualization │ ├── planning_visualizer.cpp │ ├── single_tsdf_visualizer.cpp │ ├── submap_visualizer.cpp │ └── tracking_visualizer.cpp ├── panoptic_mapping_ssh.rosinstall └── panoptic_mapping_utils ├── CMakeLists.txt ├── README.md ├── app ├── mesh_saver_node.cpp ├── multi_map_evaluation.cpp └── single_map_evaluation.cpp ├── config └── evaluate_panmap.rviz ├── include └── panoptic_mapping_utils │ ├── evaluation │ ├── map_evaluator.h │ └── progress_bar.h │ └── mesh_saver.h ├── launch ├── evaluate_panmap.launch ├── evaluate_series.launch ├── generate_flat_dataset.launch ├── parse_flat_to_airsim.launch ├── play_flat_dataset.launch ├── play_rio_dataset.launch └── scannnet_visualizer.launch ├── package.xml ├── scripts ├── download_flat_dataset.sh └── setup_airsim.sh └── src ├── detectron2 ├── create_detectron_predictions.py └── detectron_player.py ├── evaluation ├── evaluation_manager.py ├── map_evaluator.cpp └── panoptic_data_reader.py ├── flat_dataset ├── extract_data_from_bag.py ├── flat_data_player.py ├── flat_semantic_labelling.py └── waypoint_dataset_generator.py ├── mesh_saver.cpp ├── rio ├── rio_player.py └── rio_preprocessing.py └── run_helpers ├── map_loader.py └── path_visualizer.py /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | ColumnLimit: 80 5 | DerivePointerAlignment: false 6 | PointerAlignment: Left 7 | IncludeBlocks: Regroup 8 | IncludeCategories: 9 | # Spacers 10 | - Regex: '^$' 11 | Priority: 15 12 | - Regex: '^$' 13 | Priority: 25 14 | - Regex: '^$' 15 | Priority: 35 16 | - Regex: '^$' 17 | Priority: 45 18 | # C system headers 19 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 20 | Priority: 10 21 | # C++ system headers 22 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 23 | Priority: 20 24 | # Other library h files (with angles) 25 | - Regex: '^<' 26 | Priority: 30 27 | # Your project's h files (with angles) 28 | - Regex: '^> $GITHUB_ENV 35 | echo "CCACHE_STAMP=$CCACHE_STAMP" >> $GITHUB_ENV 36 | echo "PATH=$PATH" >> $GITHUB_ENV 37 | mkdir -p $CCACHE_DIR 38 | ccache --max-size=1G 39 | 40 | - name: Cache ccache files 41 | uses: actions/cache@v1.1.0 42 | with: 43 | path: ${{ env.CCACHE_DIR }} 44 | key: ${{ matrix.config.name }}-ccache-${{ env.CCACHE_STAMP }} 45 | restore-keys: | 46 | ${{ matrix.config.name }}-ccache- 47 | 48 | - name: Install System Deps on Melodic 49 | if: ${{ matrix.config.container == 'ros:melodic-ros-base-bionic' }} 50 | run: | 51 | apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 52 | apt update && apt install -y python-wstool python-catkin-tools autoconf libtool git 53 | 54 | - name: Release Build Test 55 | working-directory: 56 | env: 57 | DEBIAN_FRONTEND: noninteractiv 58 | shell: bash 59 | run: | 60 | apt update 61 | mkdir -p $HOME/catkin_ws/src; 62 | cd $HOME/catkin_ws 63 | catkin init 64 | catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}" 65 | catkin config --merge-devel 66 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo 67 | cd $HOME/catkin_ws/src 68 | ln -s $GITHUB_WORKSPACE 69 | wstool init . ./panoptic_mapping/panoptic_mapping_https.rosinstall 70 | wstool update 71 | rosdep update 72 | rosdep install --from-paths . --ignore-src -y --rosdistro ${{matrix.config.rosdistro}} 73 | catkin build -j$(nproc) -l$(nproc) panoptic_mapping_utils 74 | 75 | - name: Run Unit Tests 76 | working-directory: 77 | shell: bash 78 | run: | 79 | cd $HOME/catkin_ws/src 80 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=True 81 | catkin build panoptic_mapping --no-deps -i --catkin-make-args tests 82 | source $HOME/catkin_ws/devel/setup.bash 83 | status=0 && for f in $HOME/catkin_ws/devel/lib/panoptic_mapping/*-test; do $f || exit 1; done 84 | -------------------------------------------------------------------------------- /.github/workflows/build_test_20.yml: -------------------------------------------------------------------------------- 1 | name: 'Ubuntu 20 + ROS Noetic: Build + Tests ' 2 | on: 3 | push: 4 | branches: 5 | - 'main' 6 | pull_request: 7 | branches: 8 | - '*' 9 | 10 | jobs: 11 | build: 12 | runs-on: ubuntu-latest 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | config: 17 | - {name: 'Ubuntu 20 + ROS Noetic: Build + Tests ', rosdistro: 'noetic', container: 'ros:noetic-ros-base-focal'} 18 | container: ${{ matrix.config.container }} 19 | steps: 20 | - uses: actions/checkout@v1 21 | with: 22 | token: ${{ secrets.ACCESS_TOKEN }} 23 | 24 | - name: Setup ccache 25 | shell: bash 26 | run: | 27 | apt update && apt install -y ccache 28 | export PATH="/usr/lib/ccache:$PATH" 29 | export CCACHE_DIR=$HOME/catkin_ws/ccache 30 | export CCACHE_STAMP=$(date +%Y-%m-%d_%H-%M-%S) 31 | echo "Updated PATH=$PATH" 32 | echo "Set CCACHE_DIR=$CCACHE_DIR" 33 | echo "Set CCACHE_STAMP=$CCACHE_STAMP" 34 | echo "CCACHE_DIR=$CCACHE_DIR" >> $GITHUB_ENV 35 | echo "CCACHE_STAMP=$CCACHE_STAMP" >> $GITHUB_ENV 36 | echo "PATH=$PATH" >> $GITHUB_ENV 37 | mkdir -p $CCACHE_DIR 38 | ccache --max-size=1G 39 | 40 | - name: Cache ccache files 41 | uses: actions/cache@v1.1.0 42 | with: 43 | path: ${{ env.CCACHE_DIR }} 44 | key: ${{ matrix.config.name }}-ccache-${{ env.CCACHE_STAMP }} 45 | restore-keys: | 46 | ${{ matrix.config.name }}-ccache- 47 | 48 | - name: Install System Deps on Noetic 49 | if: ${{ matrix.config.container == 'ros:noetic-ros-base-focal' }} 50 | run: | 51 | apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 52 | apt update && apt install -y python3-pip python3-wstool python3-catkin-tools autoconf libtool git 53 | pip3 install osrf-pycommon 54 | 55 | - name: Release Build Test 56 | working-directory: 57 | env: 58 | DEBIAN_FRONTEND: noninteractiv 59 | shell: bash 60 | run: | 61 | apt update 62 | mkdir -p $HOME/catkin_ws/src; 63 | cd $HOME/catkin_ws 64 | catkin init 65 | catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}" 66 | catkin config --merge-devel 67 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo 68 | cd $HOME/catkin_ws/src 69 | ln -s $GITHUB_WORKSPACE 70 | wstool init . ./panoptic_mapping/panoptic_mapping_https.rosinstall 71 | wstool update 72 | rosdep update 73 | rosdep install --from-paths . --ignore-src -y --rosdistro ${{matrix.config.rosdistro}} 74 | catkin build -j$(nproc) -l$(nproc) panoptic_mapping_utils 75 | 76 | - name: Run Unit Tests 77 | working-directory: 78 | shell: bash 79 | run: | 80 | cd $HOME/catkin_ws/src 81 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=True 82 | catkin build panoptic_mapping --no-deps -i --catkin-make-args tests 83 | source $HOME/catkin_ws/devel/setup.bash 84 | status=0 && for f in $HOME/catkin_ws/devel/lib/panoptic_mapping/*-test; do $f || exit 1; done 85 | -------------------------------------------------------------------------------- /.github/workflows/docker-publish.yml: -------------------------------------------------------------------------------- 1 | name: Docker 2 | 3 | # This workflow uses actions that are not certified by GitHub. 4 | # They are provided by a third-party and are governed by 5 | # separate terms of service, privacy policy, and support 6 | # documentation. 7 | 8 | on: 9 | schedule: 10 | - cron: '37 1 * * *' 11 | push: 12 | branches: [ "main" ] 13 | # Publish semver tags as releases. 14 | tags: [ 'v*.*.*' ] 15 | pull_request: 16 | branches: [ "main" ] 17 | 18 | env: 19 | # Use docker.io for Docker Hub if empty 20 | REGISTRY: ghcr.io 21 | # github.repository as / 22 | IMAGE_NAME: ${{ github.repository }} 23 | 24 | 25 | jobs: 26 | build: 27 | 28 | runs-on: ubuntu-latest 29 | permissions: 30 | contents: read 31 | packages: write 32 | # This is used to complete the identity challenge 33 | # with sigstore/fulcio when running outside of PRs. 34 | id-token: write 35 | 36 | steps: 37 | - name: Checkout repository 38 | uses: actions/checkout@v3 39 | 40 | # get the branch name 41 | - name: What 42 | run: echo "BRANCH=${GITHUB_REF##*/}" >> $GITHUB_ENV 43 | 44 | # Workaround: https://github.com/docker/build-push-action/issues/461 45 | - name: Setup Docker buildx 46 | uses: docker/setup-buildx-action@79abd3f86f79a9d68a23c75a09a9a85889262adf 47 | 48 | # Login against a Docker registry except on PR 49 | # https://github.com/docker/login-action 50 | - name: Log into registry ${{ env.REGISTRY }} 51 | if: github.event_name != 'pull_request' 52 | uses: docker/login-action@28218f9b04b4f3f62068d7b6ce6ca5b26e35336c 53 | with: 54 | registry: ${{ env.REGISTRY }} 55 | username: ${{ github.actor }} 56 | password: ${{ secrets.GITHUB_TOKEN }} 57 | 58 | # Extract metadata (tags, labels) for Docker 59 | # https://github.com/docker/metadata-action 60 | - name: Extract Docker metadata 61 | id: meta 62 | uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 63 | with: 64 | images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} 65 | 66 | # Build and push Docker image with Buildx (don't push on PR) 67 | # https://github.com/docker/build-push-action 68 | - name: Build and push Docker image 69 | id: build-and-push 70 | uses: docker/build-push-action@ac9327eae2b366085ac7f6a2d02df8aa8ead720a 71 | with: 72 | context: . 73 | push: ${{ github.event_name != 'pull_request' }} 74 | tags: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}:${{ env.BRANCH }} 75 | cache-from: type=gha 76 | cache-to: type=gha,mode=max 77 | 78 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # clion stuff 33 | cmake-build-debug/ 34 | .idea/ 35 | 36 | # VS Code stuff 37 | .vscode/ 38 | 39 | # qcreator stuff 40 | CMakeLists.txt.user 41 | 42 | srv/_*.py 43 | *.pcd 44 | *.pyc 45 | qtcreator-* 46 | *.user 47 | 48 | *~ 49 | 50 | # Emacs 51 | .#* 52 | 53 | # Catkin custom files 54 | CATKIN_IGNORE 55 | 56 | # Local utilities 57 | Notes.txt 58 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | ENV TZ=Europe/Zurich 3 | RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime \ 4 | && echo $TZ > /etc/timezone 5 | ENV DEBIAN_FRONTEND="noninteractive" 6 | RUN apt update \ 7 | && apt install -y python3-catkin-tools git 8 | RUN mkdir -p /catkin_ws/src; 9 | WORKDIR /catkin_ws 10 | RUN catkin init 11 | RUN catkin config --extend "/opt/ros/noetic" 12 | RUN catkin config --merge-devel 13 | RUN catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo 14 | COPY . /catkin_ws/src/panoptic_mapping 15 | WORKDIR /catkin_ws/src 16 | RUN wstool init . ./panoptic_mapping/panoptic_mapping_https.rosinstall 17 | RUN wstool update 18 | RUN rosdep update 19 | RUN apt update \ 20 | && rosdep install --from-paths . --ignore-src -y --rosdistro noetic 21 | RUN catkin build -j$(nproc) -l$(nproc) panoptic_mapping_utils 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, ETHZ ASL 4 | All rights reserved. 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. 30 | -------------------------------------------------------------------------------- /panoptic_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(panoptic_mapping) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | # add_definitions(-Wall) 6 | find_package(catkin_simple REQUIRED) 7 | catkin_simple(ALL_DEPS_REQUIRED) 8 | catkin_package() 9 | 10 | ############ 11 | # PROTOBUF # 12 | ############ 13 | 14 | set(PROTO_DEFNS 15 | proto/panoptic_mapping/Submap.proto 16 | proto/panoptic_mapping/SubmapCollection.proto) 17 | set(BASE_PATH "proto") 18 | PROTOBUF_CATKIN_GENERATE_CPP2(${BASE_PATH} PROTO_SRCS PROTO_HDRS ${PROTO_DEFNS}) 19 | 20 | cs_add_library(${PROJECT_NAME}_proto ${PROTO_SRCS}) 21 | target_link_libraries(${PROJECT_NAME}_proto ${PROTOBUF_LIBRARIES}) 22 | 23 | ############# 24 | # Libraries # 25 | ############# 26 | 27 | cs_add_library(${PROJECT_NAME} 28 | src/common/camera.cpp 29 | src/common/input_data_user.cpp 30 | src/map/submap.cpp 31 | src/map/submap_collection.cpp 32 | src/map/submap_id.cpp 33 | src/map/instance_id.cpp 34 | src/map/submap_bounding_volume.cpp 35 | src/map/classification/binary_count.cpp 36 | src/map/classification/moving_binary_count.cpp 37 | src/map/classification/fixed_count.cpp 38 | src/map/classification/variable_count.cpp 39 | src/map/classification/uncertainty.cpp 40 | src/map/scores/average.cpp 41 | src/map/scores/latest.cpp 42 | src/labels/label_handler_base.cpp 43 | src/labels/null_label_handler.cpp 44 | src/labels/csv_label_handler.cpp 45 | src/labels/range_label_handler.cpp 46 | src/tracking/tracking_info.cpp 47 | src/tracking/single_tsdf_tracker.cpp 48 | src/tracking/ground_truth_id_tracker.cpp 49 | src/tracking/projective_id_tracker.cpp 50 | src/tracking/detectron_id_tracker.cpp 51 | src/submap_allocation/null_submap_allocator.cpp 52 | src/submap_allocation/semantic_submap_allocator.cpp 53 | src/submap_allocation/monolithic_freespace_allocator.cpp 54 | src/integration/projective_tsdf_integrator.cpp 55 | src/integration/class_projective_tsdf_integrator.cpp 56 | src/integration/single_tsdf_integrator.cpp 57 | src/integration/projection_interpolators.cpp 58 | src/integration/mesh_integrator.cpp 59 | src/map_management/map_manager.cpp 60 | src/map_management/null_map_manager.cpp 61 | src/map_management/activity_manager.cpp 62 | src/map_management/tsdf_registrator.cpp 63 | src/map_management/layer_manipulator.cpp 64 | src/tools/planning_interface.cpp 65 | src/tools/map_renderer.cpp 66 | src/tools/null_data_writer.cpp 67 | src/tools/log_data_writer.cpp 68 | src/tools/evaluation_data_writer.cpp 69 | src/tools/serialization.cpp 70 | ) 71 | target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_proto stdc++fs) 72 | 73 | ########## 74 | # Tests # 75 | ########## 76 | 77 | if (CATKIN_ENABLE_TESTING) 78 | catkin_add_gtest(serialization-test test/serialization.cpp) 79 | target_link_libraries(serialization-test ${catkin_LIBRARIES} ${PROJECT_NAME}) 80 | endif() 81 | 82 | ########## 83 | # Export # 84 | ########## 85 | 86 | cs_install() 87 | cs_export(INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include) 88 | -------------------------------------------------------------------------------- /panoptic_mapping/README.md: -------------------------------------------------------------------------------- 1 | # Panoptic Mapping 2 | Core C++ Library of **panoptic_mapping**. -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/common/globals.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_COMMON_GLOBALS_H_ 2 | #define PANOPTIC_MAPPING_COMMON_GLOBALS_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/common/camera.h" 8 | #include "panoptic_mapping/labels/label_handler_base.h" 9 | 10 | namespace panoptic_mapping { 11 | 12 | /** 13 | * @brief Utility class that provides an interface to globally used components 14 | * of the system. 15 | */ 16 | class Globals { 17 | public: 18 | Globals(std::shared_ptr camera, 19 | std::shared_ptr label_handler) 20 | : camera_(std::move(camera)), label_handler_(std::move(label_handler)) {} 21 | virtual ~Globals() = default; 22 | 23 | // Access. 24 | const std::shared_ptr& camera() const { return camera_; } 25 | const std::shared_ptr& labelHandler() const { 26 | return label_handler_; 27 | } 28 | 29 | private: 30 | // Components. 31 | std::shared_ptr camera_; 32 | std::shared_ptr label_handler_; 33 | }; 34 | 35 | } // namespace panoptic_mapping 36 | 37 | #endif // PANOPTIC_MAPPING_COMMON_GLOBALS_H_ 38 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/common/index_getter.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_COMMON_INDEX_GETTER_H_ 2 | #define PANOPTIC_MAPPING_COMMON_INDEX_GETTER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "panoptic_mapping/common/common.h" 9 | 10 | namespace panoptic_mapping { 11 | 12 | /** 13 | * Thread safe queue where threads can get the next index to process. 14 | */ 15 | template 16 | class IndexGetter { 17 | public: 18 | explicit IndexGetter(std::vector indices) 19 | : indices_(std::move(indices)), current_index_(0) {} 20 | bool getNextIndex(IndexT* index) { 21 | CHECK_NOTNULL(index); 22 | mutex_.lock(); 23 | if (current_index_ >= indices_.size()) { 24 | mutex_.unlock(); 25 | return false; 26 | } 27 | *index = indices_[current_index_]; 28 | current_index_++; 29 | mutex_.unlock(); 30 | return true; 31 | } 32 | 33 | private: 34 | std::mutex mutex_; 35 | std::vector indices_; 36 | size_t current_index_; 37 | }; 38 | 39 | typedef IndexGetter SubmapIndexGetter; 40 | 41 | } // namespace panoptic_mapping 42 | 43 | #endif // PANOPTIC_MAPPING_COMMON_INDEX_GETTER_H_ 44 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/common/input_data_user.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_COMMON_INPUT_DATA_USER_H_ 2 | #define PANOPTIC_MAPPING_COMMON_INPUT_DATA_USER_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/common/common.h" 7 | #include "panoptic_mapping/common/input_data.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * @brief Utility base class that manages interactions with input data 13 | * generation and checking, for modules using the InputData. 14 | */ 15 | class InputDataUser { 16 | public: 17 | InputDataUser() = default; 18 | virtual ~InputDataUser() = default; 19 | 20 | /** 21 | * @brief Get the current set of required inputs. 22 | */ 23 | const InputData::InputTypes& getRequiredInputs() const { 24 | return required_inputs_; 25 | } 26 | 27 | /** 28 | * @brief Check whether the provided set of inputs matches all requirements of 29 | * this module. 30 | * 31 | * @param input_data Input data to check. 32 | * @param raise_warning Whether to log which data are missing if any. 33 | * @return Whether all required inputs are contained in the data. 34 | */ 35 | bool inputIsValid(const InputData& input_data, bool raise_warning = true); 36 | 37 | protected: 38 | /** 39 | * @brief Add an input type to the set of required inputs. Duplicates are 40 | * automatically managed. 41 | * 42 | * @param type Input to add. 43 | */ 44 | void addRequiredInput(InputData::InputType type); 45 | 46 | /** 47 | * @brief Add mulitple input types to the set of required inputs. Duplicates 48 | * are automatically managed. 49 | * 50 | * @param types Inputs to add. 51 | */ 52 | void addRequiredInputs(const InputData::InputTypes& types); 53 | 54 | /** 55 | * @brief Sets the required inputs, erasing all previously stored input types. 56 | * 57 | * @param types Inputs to add. 58 | */ 59 | void setRequiredInputs(const InputData::InputTypes& types); 60 | 61 | private: 62 | InputData::InputTypes required_inputs_; 63 | }; 64 | 65 | } // namespace panoptic_mapping 66 | 67 | #endif // PANOPTIC_MAPPING_COMMON_INPUT_DATA_USER_H_ 68 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/integration/class_projective_tsdf_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_INTEGRATION_CLASS_PROJECTIVE_TSDF_INTEGRATOR_H_ 2 | #define PANOPTIC_MAPPING_INTEGRATION_CLASS_PROJECTIVE_TSDF_INTEGRATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 11 | #include "panoptic_mapping/common/camera.h" 12 | #include "panoptic_mapping/common/common.h" 13 | #include "panoptic_mapping/integration/projection_interpolators.h" 14 | #include "panoptic_mapping/integration/projective_tsdf_integrator.h" 15 | #include "panoptic_mapping/integration/tsdf_integrator_base.h" 16 | 17 | namespace panoptic_mapping { 18 | 19 | /** 20 | * @brief Allocate blocks based on the 3D points and project all visible blocks 21 | * into the image for updates. Also update a separate class layer to estimate 22 | * which voxels belong to the current submap. 23 | */ 24 | class ClassProjectiveIntegrator : public ProjectiveIntegrator { 25 | public: 26 | struct Config : public config_utilities::Config { 27 | int verbosity = 4; 28 | 29 | bool use_binary_classification = false; // false: use a counter per id. 30 | bool use_instance_classification = false; // false: use class id. 31 | bool update_only_tracked_submaps = true; 32 | 33 | // Integration params. 34 | ProjectiveIntegrator::Config pi_config; 35 | 36 | Config() { setConfigName("ClassProjectiveTsdfIntegrator"); } 37 | 38 | protected: 39 | void setupParamsAndPrinting() override; 40 | void checkParams() const override; 41 | }; 42 | 43 | ClassProjectiveIntegrator(const Config& config, 44 | std::shared_ptr globals); 45 | ~ClassProjectiveIntegrator() override = default; 46 | 47 | void processInput(SubmapCollection* submaps, InputData* input) override; 48 | 49 | protected: 50 | void updateBlock(Submap* submap, InterpolatorBase* interpolator, 51 | const voxblox::BlockIndex& block_index, 52 | const Transformation& T_C_S, 53 | const InputData& input) const override; 54 | 55 | bool updateVoxel(InterpolatorBase* interpolator, TsdfVoxel* voxel, 56 | const Point& p_C, const InputData& input, 57 | const int submap_id, const bool is_free_space_submap, 58 | const float truncation_distance, const float voxel_size, 59 | ClassVoxel* class_voxel = nullptr, 60 | ScoreVoxel* score_voxel = nullptr) const override; 61 | 62 | void updateClassVoxel(InterpolatorBase* interpolator, ClassVoxel* voxel, 63 | const InputData& input, const int submap_id) const; 64 | 65 | private: 66 | const Config config_; 67 | static config_utilities::Factory::RegistrationRos< 68 | TsdfIntegratorBase, ClassProjectiveIntegrator, std::shared_ptr> 69 | registration_; 70 | 71 | // Cached data. 72 | std::unordered_map id_to_class_; 73 | size_t num_classes_; 74 | }; 75 | 76 | } // namespace panoptic_mapping 77 | 78 | #endif // PANOPTIC_MAPPING_INTEGRATION_CLASS_PROJECTIVE_TSDF_INTEGRATOR_H_ 79 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/integration/tsdf_integrator_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_INTEGRATION_TSDF_INTEGRATOR_BASE_H_ 2 | #define PANOPTIC_MAPPING_INTEGRATION_TSDF_INTEGRATOR_BASE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "panoptic_mapping/common/common.h" 11 | #include "panoptic_mapping/common/globals.h" 12 | #include "panoptic_mapping/common/input_data_user.h" 13 | #include "panoptic_mapping/map/submap_collection.h" 14 | 15 | namespace panoptic_mapping { 16 | 17 | /** 18 | * Interface for TSDF integrators. 19 | */ 20 | class TsdfIntegratorBase : public InputDataUser { 21 | public: 22 | explicit TsdfIntegratorBase(std::shared_ptr globals) 23 | : globals_(std::move(globals)) {} 24 | ~TsdfIntegratorBase() override = default; 25 | 26 | virtual void processInput(SubmapCollection* submaps, InputData* input) = 0; 27 | 28 | protected: 29 | std::shared_ptr globals_; 30 | }; 31 | 32 | } // namespace panoptic_mapping 33 | 34 | #endif // PANOPTIC_MAPPING_INTEGRATION_TSDF_INTEGRATOR_BASE_H_ 35 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/labels/csv_label_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_LABELS_CSV_LABEL_HANDLER_H_ 2 | #define PANOPTIC_MAPPING_LABELS_CSV_LABEL_HANDLER_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 7 | #include "panoptic_mapping/common/common.h" 8 | #include "panoptic_mapping/labels/label_entry.h" 9 | #include "panoptic_mapping/labels/label_handler_base.h" 10 | 11 | namespace panoptic_mapping { 12 | 13 | /** 14 | * @brief This label handler reads a csv file to get the labels. 15 | */ 16 | class CsvLabelHandler : public LabelHandlerBase { 17 | public: 18 | struct Config : public config_utilities::Config { 19 | int verbosity = 4; 20 | 21 | // File name to read the labels from. 22 | std::string file_name; 23 | Config() { setConfigName("CsvLabelHandler"); } 24 | 25 | protected: 26 | void setupParamsAndPrinting() override; 27 | void checkParams() const override; 28 | }; 29 | 30 | explicit CsvLabelHandler(const Config& config, bool print_config = true); 31 | ~CsvLabelHandler() override = default; 32 | 33 | private: 34 | const Config config_; 35 | static config_utilities::Factory::RegistrationRos 37 | registration_; 38 | void readLabelsFromFile(); 39 | }; 40 | 41 | } // namespace panoptic_mapping 42 | 43 | #endif // PANOPTIC_MAPPING_LABELS_CSV_LABEL_HANDLER_H_ 44 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/labels/label_entry.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_LABELS_LABEL_ENTRY_H_ 2 | #define PANOPTIC_MAPPING_LABELS_LABEL_ENTRY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "panoptic_mapping/common/common.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * @brief Labels are read from external sources to interpret the semantic 16 | meaning of the segmentation inputs. Depending on the source of panoptic input 17 | data, different labels might be needed. The LabelEntry is the base struct 18 | containing minimal information that needs to be provided to panoptic mapping. 19 | The default values represent an unknown label. 20 | */ 21 | struct LabelEntry { 22 | // Required fields. 23 | // The ID of the input data referring to this label. 24 | int segmentation_id = -1; 25 | 26 | // The ID of the semantic class. 27 | int class_id = -1; 28 | 29 | // The panoptic category of the label. 30 | PanopticLabel label = PanopticLabel::kUnknown; 31 | 32 | // Print the contents of the label. 33 | virtual std::string toString() const { 34 | std::stringstream ss; 35 | ss << "SegmentationID: " << segmentation_id << ", ClassID: " << class_id 36 | << ", PanopticID: " << panopticLabelToString(label) 37 | << ", Color: " << static_cast(color.r) << " " 38 | << static_cast(color.g) << " " << static_cast(color.b) 39 | << ", Name: " << name << ", Size: " << size; 40 | return ss.str(); 41 | } 42 | 43 | // Optional fields. 44 | // Human readable or interpretable class name. 45 | std::string name = "UninitializedName"; 46 | 47 | // Associated size of (S,M,L) of the object. Used by specific submap 48 | // allocators. 49 | std::string size = "Unknown"; 50 | 51 | // Color to be used to display this label. 52 | Color color = Color(80, 80, 80); 53 | }; 54 | 55 | } // namespace panoptic_mapping 56 | 57 | #endif // PANOPTIC_MAPPING_LABELS_LABEL_ENTRY_H_ 58 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/labels/label_handler_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_LABELS_LABEL_HANDLER_BASE_H_ 2 | #define PANOPTIC_MAPPING_LABELS_LABEL_HANDLER_BASE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "panoptic_mapping/common/common.h" 9 | #include "panoptic_mapping/labels/label_entry.h" 10 | 11 | namespace panoptic_mapping { 12 | 13 | /** 14 | * @brief Class that enables look-ups on labels via the segmentation ID. Exposes 15 | * the minimum required fields explicitly, however returning the LabelEntry 16 | * directly is more efficient for multiple lookups. 17 | */ 18 | class LabelHandlerBase { 19 | public: 20 | LabelHandlerBase() = default; 21 | virtual ~LabelHandlerBase() = default; 22 | 23 | // This returns true if the id was found. 24 | bool segmentationIdExists(int segmentation_id) const; 25 | 26 | // These acessors assume that the segmentation_id exists. 27 | int getClassID(int segmentation_id) const; 28 | bool isBackgroundClass(int segmentation_id) const; 29 | bool isInstanceClass(int segmentation_id) const; 30 | bool isUnknownClass(int segmentation_id) const; 31 | bool isSpaceClass(int segmentation_id) const; 32 | PanopticLabel getPanopticLabel(int segmentation_id) const; 33 | const voxblox::Color& getColor(int segmentation_id) const; 34 | const std::string& getName(int segmentation_id) const; 35 | const LabelEntry& getLabelEntry(int segmentation_id) const; 36 | 37 | /** 38 | * @brief Get the LabelEntry if it exists in a combined lookup. 39 | * 40 | * @param segmentation_id Segmentation ID to look up. 41 | * @param label_entry Resulting LabelEntry, set if the look-up was successful. 42 | * @return True if the segmentation ID was found. False otherwise. 43 | */ 44 | bool getLabelEntryIfExists(int segmentation_id, 45 | LabelEntry* label_entry) const; 46 | 47 | // Get the number of stored labels. 48 | size_t numberOfLabels() const; 49 | 50 | protected: 51 | // List of the labels associated with each segmentation ID. Labels are stored 52 | // by pointer such that derived label types can also be stored here. 53 | std::unordered_map> labels_; 54 | }; 55 | 56 | } // namespace panoptic_mapping 57 | 58 | #endif // PANOPTIC_MAPPING_LABELS_LABEL_HANDLER_BASE_H_ 59 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/labels/null_label_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_LABELS_NULL_LABEL_HANDLER_H_ 2 | #define PANOPTIC_MAPPING_LABELS_NULL_LABEL_HANDLER_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 7 | #include "panoptic_mapping/labels/label_handler_base.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * @brief This label handler does not read any labels. 13 | */ 14 | class NullLabelHandler : public LabelHandlerBase { 15 | public: 16 | struct Config : public config_utilities::Config { 17 | int verbosity = 4; 18 | 19 | Config() { setConfigName("NullLabelHandler"); } 20 | 21 | protected: 22 | void setupParamsAndPrinting() override; 23 | }; 24 | 25 | explicit NullLabelHandler(const Config& config); 26 | ~NullLabelHandler() override = default; 27 | 28 | private: 29 | const Config config_; 30 | static config_utilities::Factory::RegistrationRos 32 | registration_[[gnu::used]]; 33 | }; 34 | 35 | } // namespace panoptic_mapping 36 | 37 | #endif // PANOPTIC_MAPPING_LABELS_NULL_LABEL_HANDLER_H_ 38 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/labels/range_label_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_LABELS_RANGE_LABEL_HANDLER_H_ 2 | #define PANOPTIC_MAPPING_LABELS_RANGE_LABEL_HANDLER_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 7 | #include "panoptic_mapping/common/common.h" 8 | #include "panoptic_mapping/labels/label_entry.h" 9 | #include "panoptic_mapping/labels/label_handler_base.h" 10 | 11 | namespace panoptic_mapping { 12 | 13 | /** 14 | * @brief This basic label handler allocates labels in an integer range. 15 | */ 16 | class RangeLabelHandler : public LabelHandlerBase { 17 | public: 18 | struct Config : public config_utilities::Config { 19 | int verbosity = 4; 20 | 21 | // Number of labels, initialises labels in range [0, num_labels - 1] 22 | int num_labels; 23 | Config() { setConfigName("RangeLabelHandler"); } 24 | 25 | protected: 26 | void setupParamsAndPrinting() override; 27 | void checkParams() const override; 28 | }; 29 | 30 | explicit RangeLabelHandler(const Config& config, bool print_config = true); 31 | ~RangeLabelHandler() override = default; 32 | 33 | private: 34 | const Config config_; 35 | static config_utilities::Factory::RegistrationRos 37 | registration_; 38 | void initialiseLabels(); 39 | }; 40 | 41 | } // namespace panoptic_mapping 42 | 43 | #endif // PANOPTIC_MAPPING_LABELS_RANGE_LABEL_HANDLER_H_ 44 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/classification/binary_count.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_CLASSIFICATION_BINARY_COUNT_H_ 2 | #define PANOPTIC_MAPPING_MAP_CLASSIFICATION_BINARY_COUNT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/Submap.pb.h" 9 | #include "panoptic_mapping/map/classification/class_layer_impl.h" 10 | #include "panoptic_mapping/map/classification/class_voxel.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * @brief Binary classification by simple counting, where ID 0 indicates the 16 | * voxel belongs. 17 | */ 18 | struct BinaryCountVoxel : public ClassVoxel { 19 | public: 20 | // Implement interfaces. 21 | ClassVoxelType getVoxelType() const override; 22 | bool isObserverd() const override; 23 | bool belongsToSubmap() const override; 24 | float getBelongingProbability() const override; 25 | int getBelongingID() const override; 26 | float getProbability(const int id) const override; 27 | void incrementCount(const int id, const float weight = 1.f) override; 28 | bool mergeVoxel(const ClassVoxel& other) override; 29 | std::vector serializeVoxelToInt() const override; 30 | bool deseriliazeVoxelFromInt(const std::vector& data, 31 | size_t* data_index) override; 32 | 33 | // Data. 34 | ClassificationCount belongs_count = 0u; 35 | ClassificationCount foreign_count = 0u; 36 | }; 37 | 38 | class BinaryCountLayer : public ClassLayerImpl { 39 | public: 40 | struct Config : public config_utilities::Config { 41 | Config() { setConfigName("BinaryCountLayer"); } 42 | 43 | protected: 44 | void fromRosParam() override {} 45 | void printFields() const override {} 46 | }; 47 | 48 | BinaryCountLayer(const Config& config, const float voxel_size, 49 | const int voxels_per_side); 50 | 51 | ClassVoxelType getVoxelType() const override; 52 | std::unique_ptr clone() const override; 53 | static std::unique_ptr loadFromStream( 54 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 55 | uint64_t* /* tmp_byte_offset_ptr */); 56 | 57 | protected: 58 | const Config config_; 59 | static config_utilities::Factory::RegistrationRos< 60 | ClassLayer, BinaryCountLayer, float, int> 61 | registration_; 62 | }; 63 | 64 | } // namespace panoptic_mapping 65 | 66 | #endif // PANOPTIC_MAPPING_MAP_CLASSIFICATION_BINARY_COUNT_H_ 67 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/classification/class_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_CLASSIFICATION_CLASS_LAYER_H_ 2 | #define PANOPTIC_MAPPING_MAP_CLASSIFICATION_CLASS_LAYER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "panoptic_mapping/common/common.h" 10 | #include "panoptic_mapping/map/classification/class_block.h" 11 | #include "panoptic_mapping/map/classification/class_voxel.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief General interface to classification voxel layers. Wraps the voxblox 17 | * layer to allow substituting different classification layer types. 18 | */ 19 | class ClassLayer { 20 | public: 21 | virtual ~ClassLayer() = default; 22 | 23 | // Implement this to expose the voxel type to objects that use a specific 24 | // voxel type. 25 | virtual ClassVoxelType getVoxelType() const = 0; 26 | 27 | // Voxblox interfaces. 28 | virtual ClassBlock::ConstPtr getBlockConstPtrByIndex( 29 | const BlockIndex& index) const = 0; 30 | virtual ClassBlock::Ptr getBlockPtrByIndex(const BlockIndex& index) = 0; 31 | virtual ClassBlock::Ptr allocateBlockPtrByIndex(const BlockIndex& index) = 0; 32 | virtual ClassBlock::ConstPtr getBlockPtrByCoordinates( 33 | const Point& coords) const = 0; 34 | virtual ClassBlock::Ptr getBlockPtrByCoordinates(const Point& coords) = 0; 35 | virtual ClassBlock::Ptr allocateBlockPtrByCoordinates( 36 | const Point& coords) = 0; 37 | virtual ClassBlock::Ptr allocateNewBlock(const BlockIndex& index) = 0; 38 | virtual ClassBlock::Ptr allocateNewBlockByCoordinates( 39 | const Point& coords) = 0; 40 | virtual void removeBlock(const BlockIndex& index) = 0; 41 | virtual void removeAllBlocks() = 0; 42 | virtual void removeBlockByCoordinates(const Point& coords) = 0; 43 | virtual void getAllAllocatedBlocks(voxblox::BlockIndexList* blocks) const = 0; 44 | virtual void getAllUpdatedBlocks(voxblox::Update::Status bit, 45 | voxblox::BlockIndexList* blocks) const = 0; 46 | virtual size_t getNumberOfAllocatedBlocks() const = 0; 47 | virtual bool hasBlock(const BlockIndex& block_index) const = 0; 48 | virtual size_t getMemorySize() const = 0; 49 | virtual size_t voxels_per_side() const = 0; 50 | virtual FloatingPoint voxel_size() const = 0; 51 | virtual FloatingPoint block_size() const = 0; 52 | virtual std::unique_ptr clone() const = 0; 53 | 54 | // Serialization 55 | virtual bool saveBlockToStream(BlockIndex block_index, 56 | std::fstream* outfile_ptr) const = 0; 57 | virtual bool saveBlocksToStream(bool include_all_blocks, 58 | voxblox::BlockIndexList blocks_to_include, 59 | std::fstream* outfile_ptr) const = 0; 60 | virtual bool addBlockFromProto(const voxblox::BlockProto& block_proto) = 0; 61 | 62 | /** 63 | * @brief Directly returns the voxel if it exists. Returns nullpointer 64 | * otherwise. Prefer this when looking up sparse voxels since it avoids the 65 | * ClassBlock intermediate step. 66 | */ 67 | virtual ClassVoxel* getVoxelPtrByCoordinates(const Point& coords) = 0; 68 | virtual const ClassVoxel* getVoxelPtrByCoordinates( 69 | const Point& coords) const = 0; 70 | }; 71 | 72 | } // namespace panoptic_mapping 73 | 74 | #endif // PANOPTIC_MAPPING_MAP_CLASSIFICATION_CLASS_LAYER_H_ 75 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/classification/fixed_count.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_CLASSIFICATION_FIXED_COUNT_H_ 2 | #define PANOPTIC_MAPPING_MAP_CLASSIFICATION_FIXED_COUNT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/Submap.pb.h" 9 | #include "panoptic_mapping/map/classification/class_layer_impl.h" 10 | #include "panoptic_mapping/map/classification/class_voxel.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * @brief Classification by counting the occurences of each label. The index 0 16 | * is generally reserved for the belonging submap by shifting all IDs by 1. The 17 | * memory for counting is lazily allocated since often only surface voxels are 18 | * relevant. 19 | */ 20 | struct FixedCountVoxel : public ClassVoxel { 21 | public: 22 | // Implement interfaces. 23 | ClassVoxelType getVoxelType() const override; 24 | bool isObserverd() const override; 25 | bool belongsToSubmap() const override; 26 | float getBelongingProbability() const override; 27 | int getBelongingID() const override; 28 | float getProbability(const int id) const override; 29 | void incrementCount(const int id, const float weight = 1.f) override; 30 | bool mergeVoxel(const ClassVoxel& other) override; 31 | std::vector serializeVoxelToInt() const override; 32 | bool deseriliazeVoxelFromInt(const std::vector& data, 33 | size_t* data_index) override; 34 | // Data. 35 | std::vector counts; 36 | int current_index = 0; 37 | ClassificationCount current_count = 0; 38 | ClassificationCount total_count = 0; 39 | 40 | static size_t numCounts(); 41 | static void setNumCounts(size_t num_counts); 42 | 43 | private: 44 | // Fixed count voxels store a fixed number of labels, which is currently set 45 | // via this global setting. 46 | static size_t kNumCounts; 47 | }; 48 | 49 | class FixedCountLayer : public ClassLayerImpl { 50 | public: 51 | struct Config : public config_utilities::Config { 52 | Config() { setConfigName("FixedCountLayer"); } 53 | 54 | protected: 55 | void fromRosParam() override {} 56 | void printFields() const override {} 57 | }; 58 | 59 | FixedCountLayer(const Config& config, const float voxel_size, 60 | const int voxels_per_side); 61 | 62 | ClassVoxelType getVoxelType() const override; 63 | std::unique_ptr clone() const override; 64 | static std::unique_ptr loadFromStream( 65 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 66 | uint64_t* /* tmp_byte_offset_ptr */); 67 | 68 | protected: 69 | const Config config_; 70 | static config_utilities::Factory::RegistrationRos 72 | registration_; 73 | }; 74 | 75 | } // namespace panoptic_mapping 76 | 77 | #endif // PANOPTIC_MAPPING_MAP_CLASSIFICATION_FIXED_COUNT_H_ 78 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/classification/moving_binary_count.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_CLASSIFICATION_MOVING_BINARY_COUNT_H_ 2 | #define PANOPTIC_MAPPING_MAP_CLASSIFICATION_MOVING_BINARY_COUNT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/Submap.pb.h" 9 | #include "panoptic_mapping/map/classification/class_layer_impl.h" 10 | #include "panoptic_mapping/map/classification/class_voxel.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * @brief Binary classification by simple counting, where ID 0 indicates the 16 | * voxel belongs. Uses a reduced datatype to save memory, where older counts are 17 | * de-weighted to prevent overflow. 18 | */ 19 | struct MovingBinaryCountVoxel : public ClassVoxel { 20 | public: 21 | // Implement interfaces. 22 | ClassVoxelType getVoxelType() const override; 23 | bool isObserverd() const override; 24 | bool belongsToSubmap() const override; 25 | float getBelongingProbability() const override; 26 | int getBelongingID() const override; 27 | float getProbability(const int id) const override; 28 | void incrementCount(const int id, const float weight = 1.f) override; 29 | bool mergeVoxel(const ClassVoxel& other) override; 30 | std::vector serializeVoxelToInt() const override; 31 | bool deseriliazeVoxelFromInt(const std::vector& data, 32 | size_t* data_index) override; 33 | // Data. 34 | uint8_t belongs_count = 0u; 35 | uint8_t foreign_count = 0u; 36 | }; 37 | 38 | class MovingBinaryCountLayer : public ClassLayerImpl { 39 | public: 40 | struct Config : public config_utilities::Config { 41 | Config() { setConfigName("MovingBinaryCountLayer"); } 42 | 43 | protected: 44 | void fromRosParam() override {} 45 | void printFields() const override {} 46 | }; 47 | 48 | MovingBinaryCountLayer(const Config& config, const float voxel_size, 49 | const int voxels_per_side); 50 | 51 | // Overwrite these method since we only need half a word per voxel. 52 | bool saveBlockToStream(BlockIndex block_index, 53 | std::fstream* outfile_ptr) const override; 54 | bool addBlockFromProto(const voxblox::BlockProto& block_proto) override; 55 | 56 | ClassVoxelType getVoxelType() const override; 57 | std::unique_ptr clone() const override; 58 | static std::unique_ptr loadFromStream( 59 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 60 | uint64_t* /* tmp_byte_offset_ptr */); 61 | 62 | protected: 63 | const Config config_; 64 | static config_utilities::Factory::RegistrationRos< 65 | ClassLayer, MovingBinaryCountLayer, float, int> 66 | registration_; 67 | }; 68 | 69 | } // namespace panoptic_mapping 70 | 71 | #endif // PANOPTIC_MAPPING_MAP_CLASSIFICATION_MOVING_BINARY_COUNT_H_ 72 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/classification/uncertainty.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_CLASSIFICATION_UNCERTAINTY_H_ 2 | #define PANOPTIC_MAPPING_MAP_CLASSIFICATION_UNCERTAINTY_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/Submap.pb.h" 9 | #include "panoptic_mapping/map/classification/class_layer_impl.h" 10 | #include "panoptic_mapping/map/classification/class_voxel.h" 11 | #include "panoptic_mapping/map/classification/fixed_count.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief Specialization of the fixed count voxel that additionally stores an 17 | * aggregated uncertainty and can be labeled as groudn truth. 18 | */ 19 | struct UncertaintyVoxel : public FixedCountVoxel { 20 | public: 21 | // Implement interfaces. 22 | ClassVoxelType getVoxelType() const override; 23 | bool mergeVoxel(const ClassVoxel& other) override; 24 | std::vector serializeVoxelToInt() const override; 25 | bool deseriliazeVoxelFromInt(const std::vector& data, 26 | size_t* data_index) override; 27 | // Data. 28 | float uncertainty = 0.f; 29 | bool is_ground_truth = false; 30 | }; 31 | 32 | class UncertaintyLayer : public ClassLayerImpl { 33 | public: 34 | struct Config : public config_utilities::Config { 35 | Config() { setConfigName("UncertaintyLayer"); } 36 | 37 | protected: 38 | void fromRosParam() override {} 39 | void printFields() const override {} 40 | }; 41 | 42 | UncertaintyLayer(const Config& config, const float voxel_size, 43 | const int voxels_per_side); 44 | 45 | ClassVoxelType getVoxelType() const override; 46 | std::unique_ptr clone() const override; 47 | static std::unique_ptr loadFromStream( 48 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 49 | uint64_t* /* tmp_byte_offset_ptr */); 50 | 51 | protected: 52 | const Config config_; 53 | static config_utilities::Factory::RegistrationRos< 54 | ClassLayer, UncertaintyLayer, float, int> 55 | registration_; 56 | }; 57 | 58 | } // namespace panoptic_mapping 59 | 60 | #endif // PANOPTIC_MAPPING_MAP_CLASSIFICATION_UNCERTAINTY_H_ 61 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/classification/variable_count.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_CLASSIFICATION_VARIABLE_COUNT_H_ 2 | #define PANOPTIC_MAPPING_MAP_CLASSIFICATION_VARIABLE_COUNT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 9 | #include "panoptic_mapping/Submap.pb.h" 10 | #include "panoptic_mapping/map/classification/class_layer_impl.h" 11 | #include "panoptic_mapping/map/classification/class_voxel.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief Keep track of arbitrary number of IDs in an unordered map. ID 0 is 17 | * generally used to store the belonging submap and shifting other IDs by 1. 18 | */ 19 | struct VariableCountVoxel : public ClassVoxel { 20 | public: 21 | // Implement interfaces. 22 | ClassVoxelType getVoxelType() const override; 23 | bool isObserverd() const override; 24 | bool belongsToSubmap() const override; 25 | float getBelongingProbability() const override; 26 | int getBelongingID() const override; 27 | float getProbability(const int id) const override; 28 | void incrementCount(const int id, const float weight = 1.f) override; 29 | bool mergeVoxel(const ClassVoxel& other) override; 30 | std::vector serializeVoxelToInt() const override; 31 | bool deseriliazeVoxelFromInt(const std::vector& data, 32 | size_t* data_index) override; 33 | // Data. 34 | std::unordered_map counts; 35 | int current_index = 0; 36 | ClassificationCount current_count = 0; 37 | ClassificationCount total_count = 0; 38 | }; 39 | 40 | class VariableCountLayer : public ClassLayerImpl { 41 | public: 42 | struct Config : public config_utilities::Config { 43 | Config() { setConfigName("VariableCountLayer"); } 44 | 45 | protected: 46 | void fromRosParam() override {} 47 | void printFields() const override {} 48 | }; 49 | 50 | VariableCountLayer(const Config& config, const float voxel_size, 51 | const int voxels_per_side); 52 | 53 | ClassVoxelType getVoxelType() const override; 54 | std::unique_ptr clone() const override; 55 | static std::unique_ptr loadFromStream( 56 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 57 | uint64_t* /* tmp_byte_offset_ptr */); 58 | 59 | protected: 60 | const Config config_; 61 | static config_utilities::Factory::RegistrationRos< 62 | ClassLayer, VariableCountLayer, float, int> 63 | registration_; 64 | }; 65 | 66 | } // namespace panoptic_mapping 67 | 68 | #endif // PANOPTIC_MAPPING_MAP_CLASSIFICATION_VARIABLE_COUNT_H_ 69 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/instance_id.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_INSTANCE_ID_H_ 2 | #define PANOPTIC_MAPPING_MAP_INSTANCE_ID_H_ 3 | 4 | #include 5 | 6 | namespace panoptic_mapping { 7 | 8 | /** 9 | * Class for ID management. Default uses a global singleton but e.g. individual 10 | * submap collections are allowed to have separate. 11 | */ 12 | class InstanceIDManager { 13 | public: 14 | InstanceIDManager(); 15 | 16 | // Default is a global instance id manager, as singleton. 17 | static InstanceIDManager* getGlobalInstance() { 18 | static InstanceIDManager instance; 19 | return &instance; 20 | } 21 | 22 | private: 23 | friend class InstanceID; 24 | 25 | // Interaction 26 | int requestID(); 27 | void registerID(int id); 28 | void releaseID(int id); 29 | 30 | private: 31 | static const bool kAllowIDReuse = false; 32 | 33 | // Tracking. 34 | int current_id_; 35 | std::unordered_map used_ids_; 36 | 37 | void increment(int id); 38 | bool decrement(int id); 39 | }; 40 | 41 | /** 42 | * This class instantiates and tracks instance IDs. Instance IDs are allowed to 43 | * be duplicates but by default generate new unique ids. 44 | */ 45 | class InstanceID { 46 | public: 47 | // controlled con- and destruction. 48 | explicit InstanceID( 49 | InstanceIDManager* manager = InstanceIDManager::getGlobalInstance()); 50 | explicit InstanceID(int id, InstanceIDManager* manager = 51 | InstanceIDManager::getGlobalInstance()); 52 | InstanceID(const InstanceID& other); 53 | ~InstanceID(); 54 | 55 | // Conversion and assignment. 56 | operator int() const { return id_; } 57 | InstanceID& operator=(const int& id); 58 | 59 | private: 60 | int id_; 61 | InstanceIDManager* const manager_; 62 | }; 63 | 64 | } // namespace panoptic_mapping 65 | 66 | #endif // PANOPTIC_MAPPING_MAP_INSTANCE_ID_H_ 67 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/scores/average.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_SCORES_AVERAGE_H_ 2 | #define PANOPTIC_MAPPING_MAP_SCORES_AVERAGE_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/Submap.pb.h" 9 | #include "panoptic_mapping/map/scores/score_layer_impl.h" 10 | #include "panoptic_mapping/map/scores/score_voxel.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * @brief Classification by counting the occurences of each label. The index 0 16 | * is generally reserved for the belonging submap by shifting all IDs by 1. The 17 | * memory for counting is lazily allocated since often only surface voxels are 18 | * relevant. 19 | */ 20 | struct AverageScoreVoxel : public ScoreVoxel { 21 | public: 22 | // Implement interfaces. 23 | ScoreVoxelType getVoxelType() const override; 24 | bool isObserverd() const override; 25 | float getScore() const override; 26 | void addMeasurement(const float score, const float weight = 1.0) override; 27 | bool mergeVoxel(const ScoreVoxel& other) override; 28 | std::vector serializeVoxelToInt() const override; 29 | bool deseriliazeVoxelFromInt(const std::vector& data, 30 | size_t* data_index) override; 31 | // Data. 32 | float accumulated_weight = 0; 33 | float average_score; 34 | }; 35 | 36 | class AverageScoreLayer : public ScoreLayerImpl { 37 | public: 38 | struct Config : public config_utilities::Config { 39 | Config() { setConfigName("AverageScoreLayer"); } 40 | 41 | protected: 42 | void fromRosParam() override {} 43 | void printFields() const override {} 44 | }; 45 | 46 | AverageScoreLayer(const Config& config, const float voxel_size, 47 | const int voxels_per_side); 48 | 49 | ScoreVoxelType getVoxelType() const override; 50 | std::unique_ptr clone() const override; 51 | static std::unique_ptr loadFromStream( 52 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 53 | uint64_t* /* tmp_byte_offset_ptr */); 54 | 55 | protected: 56 | const Config config_; 57 | static config_utilities::Factory::RegistrationRos< 58 | ScoreLayer, AverageScoreLayer, float, int> 59 | registration_; 60 | }; 61 | 62 | } // namespace panoptic_mapping 63 | 64 | #endif // PANOPTIC_MAPPING_MAP_SCORES_AVERAGE_H_ 65 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/scores/latest.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_SCORES_LATEST_H_ 2 | #define PANOPTIC_MAPPING_MAP_SCORES_LATEST_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/Submap.pb.h" 9 | #include "panoptic_mapping/map/scores/score_layer_impl.h" 10 | #include "panoptic_mapping/map/scores/score_voxel.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * @brief Classification by counting the occurences of each label. The index 0 16 | * is generally reserved for the belonging submap by shifting all IDs by 1. The 17 | * memory for counting is lazily allocated since often only surface voxels are 18 | * relevant. 19 | */ 20 | struct LatestScoreVoxel : public ScoreVoxel { 21 | public: 22 | // Implement interfaces. 23 | ScoreVoxelType getVoxelType() const override; 24 | bool isObserverd() const override; 25 | float getScore() const override; 26 | void addMeasurement(const float score, const float weight = 1.0) override; 27 | bool mergeVoxel(const ScoreVoxel& other) override; 28 | std::vector serializeVoxelToInt() const override; 29 | bool deseriliazeVoxelFromInt(const std::vector& data, 30 | size_t* data_index) override; 31 | // Data. 32 | float latest_score = NAN; 33 | }; 34 | 35 | class LatestScoreLayer : public ScoreLayerImpl { 36 | public: 37 | struct Config : public config_utilities::Config { 38 | Config() { setConfigName("LatestScoreLayer"); } 39 | 40 | protected: 41 | void fromRosParam() override {} 42 | void printFields() const override {} 43 | }; 44 | 45 | LatestScoreLayer(const Config& config, const float voxel_size, 46 | const int voxels_per_side); 47 | 48 | ScoreVoxelType getVoxelType() const override; 49 | std::unique_ptr clone() const override; 50 | static std::unique_ptr loadFromStream( 51 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 52 | uint64_t* /* tmp_byte_offset_ptr */); 53 | 54 | protected: 55 | const Config config_; 56 | static config_utilities::Factory::RegistrationRos< 57 | ScoreLayer, LatestScoreLayer, float, int> 58 | registration_; 59 | }; 60 | 61 | } // namespace panoptic_mapping 62 | 63 | #endif // PANOPTIC_MAPPING_MAP_SCORES_LATEST_H_ 64 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/scores/score_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_SCORES_SCORE_LAYER_H_ 2 | #define PANOPTIC_MAPPING_MAP_SCORES_SCORE_LAYER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "panoptic_mapping/common/common.h" 10 | #include "panoptic_mapping/map/scores/score_block.h" 11 | #include "panoptic_mapping/map/scores/score_voxel.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief General interface to score voxel layers. Wraps the voxblox 17 | * layer to allow substituting different score layer types. 18 | */ 19 | class ScoreLayer { 20 | public: 21 | virtual ~ScoreLayer() = default; 22 | 23 | // Implement this to expose the voxel type to objects that use a specific 24 | // voxel type. 25 | virtual ScoreVoxelType getVoxelType() const = 0; 26 | 27 | // Voxblox interfaces. 28 | virtual ScoreBlock::ConstPtr getBlockConstPtrByIndex( 29 | const BlockIndex& index) const = 0; 30 | virtual ScoreBlock::Ptr getBlockPtrByIndex(const BlockIndex& index) = 0; 31 | virtual ScoreBlock::Ptr allocateBlockPtrByIndex(const BlockIndex& index) = 0; 32 | virtual ScoreBlock::ConstPtr getBlockPtrByCoordinates( 33 | const Point& coords) const = 0; 34 | virtual ScoreBlock::Ptr getBlockPtrByCoordinates(const Point& coords) = 0; 35 | virtual ScoreBlock::Ptr allocateBlockPtrByCoordinates( 36 | const Point& coords) = 0; 37 | virtual ScoreBlock::Ptr allocateNewBlock(const BlockIndex& index) = 0; 38 | virtual ScoreBlock::Ptr allocateNewBlockByCoordinates( 39 | const Point& coords) = 0; 40 | virtual void removeBlock(const BlockIndex& index) = 0; 41 | virtual void removeAllBlocks() = 0; 42 | virtual void removeBlockByCoordinates(const Point& coords) = 0; 43 | virtual void getAllAllocatedBlocks(voxblox::BlockIndexList* blocks) const = 0; 44 | virtual void getAllUpdatedBlocks(voxblox::Update::Status bit, 45 | voxblox::BlockIndexList* blocks) const = 0; 46 | virtual size_t getNumberOfAllocatedBlocks() const = 0; 47 | virtual bool hasBlock(const BlockIndex& block_index) const = 0; 48 | virtual size_t getMemorySize() const = 0; 49 | virtual size_t voxels_per_side() const = 0; 50 | virtual FloatingPoint voxel_size() const = 0; 51 | virtual FloatingPoint block_size() const = 0; 52 | virtual std::unique_ptr clone() const = 0; 53 | 54 | // Serialization 55 | virtual bool saveBlockToStream(BlockIndex block_index, 56 | std::fstream* outfile_ptr) const = 0; 57 | virtual bool saveBlocksToStream(bool include_all_blocks, 58 | voxblox::BlockIndexList blocks_to_include, 59 | std::fstream* outfile_ptr) const = 0; 60 | virtual bool addBlockFromProto(const voxblox::BlockProto& block_proto) = 0; 61 | 62 | /** 63 | * @brief Directly returns the voxel if it exists. Returns nullpointer 64 | * otherwise. Prefer this when looking up sparse voxels since it avoids the 65 | * ScoreBlock intermediate step. 66 | */ 67 | virtual ScoreVoxel* getVoxelPtrByCoordinates(const Point& coords) = 0; 68 | virtual const ScoreVoxel* getVoxelPtrByCoordinates( 69 | const Point& coords) const = 0; 70 | }; 71 | 72 | } // namespace panoptic_mapping 73 | 74 | #endif // PANOPTIC_MAPPING_MAP_SCORES_SCORE_LAYER_H_ 75 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/scores/score_voxel.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_SCORES_SCORE_VOXEL_H_ 2 | #define PANOPTIC_MAPPING_MAP_SCORES_SCORE_VOXEL_H_ 3 | 4 | #include 5 | 6 | namespace panoptic_mapping { 7 | 8 | // Enumerate all implemented classification voxel types for objects that need to 9 | // operate on specific voxel types. 10 | enum class ScoreVoxelType { 11 | kLatest, 12 | kAverage, 13 | }; 14 | 15 | /** 16 | * @brief General interface for voxels that aggregate a floating point number, 17 | * e.g. an uncertainty, novelty score or the likes. 18 | */ 19 | struct ScoreVoxel { 20 | virtual ~ScoreVoxel() = default; 21 | 22 | /** 23 | * @brief Expose the contained type of the voxel for objects that need to 24 | * operate on specific voxel types. 25 | */ 26 | virtual ScoreVoxelType getVoxelType() const = 0; 27 | 28 | /** 29 | * @brief Return true if the voxel has received any measurements yet. 30 | */ 31 | virtual bool isObserverd() const = 0; 32 | 33 | /** 34 | * @brief Return the current score value in the voxel. 35 | * @return float The score. 36 | */ 37 | virtual float getScore() const = 0; 38 | 39 | /** 40 | * @brief Add a measurement to this voxel that will be aggregated dependent on 41 | * the specified voxel type. 42 | * 43 | * @param float The measurement. 44 | * @param weight Optional. The weight by which this measurement is weighted in 45 | * the aggregation scheme. 46 | */ 47 | virtual void addMeasurement(const float score, const float weight = 1.f) = 0; 48 | 49 | /** 50 | * @brief Merge another voxel into this voxel. If the default merging 51 | * behavior is not desired, the voxel members are exposed to custom 52 | * manipulation. 53 | * 54 | * @param other Voxel to merge into this one. 55 | * @return True if the merging was successful. 56 | */ 57 | virtual bool mergeVoxel(const ScoreVoxel& other) = 0; 58 | 59 | /** 60 | * @brief Serialization tool to serialize voxels and layers to integer data. 61 | * 62 | * @param data The current binary data to append the voxel to. 63 | */ 64 | virtual std::vector serializeVoxelToInt() const = 0; 65 | 66 | /** 67 | * @brief De-serialize the voxel from integer data. 68 | * 69 | * @param data The data to read from. 70 | * @param data_index Index from where to read the data. This index should be 71 | * updated to point to the end of the serialized data such that the next voxel 72 | * can be read from there. 73 | * @return True if the voxel was correctly de-serialized, false otherwise. 74 | */ 75 | virtual bool deseriliazeVoxelFromInt(const std::vector& data, 76 | size_t* data_index) = 0; 77 | }; 78 | 79 | } // namespace panoptic_mapping 80 | 81 | #endif // PANOPTIC_MAPPING_MAP_SCORES_SCORE_VOXEL_H_ 82 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/submap_bounding_volume.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_SUBMAP_BOUNDING_VOLUME_H_ 2 | #define PANOPTIC_MAPPING_MAP_SUBMAP_BOUNDING_VOLUME_H_ 3 | 4 | #include "panoptic_mapping/common/common.h" 5 | 6 | namespace panoptic_mapping { 7 | 8 | class Submap; 9 | 10 | /** 11 | * This class interfaces conservative bounding volumes to hierarchically 12 | * prune submaps. Implemented using an inexact conservative sphere 13 | * approximation. The bounding volume is owned by the submap it references. 14 | */ 15 | class SubmapBoundingVolume { 16 | public: 17 | explicit SubmapBoundingVolume(const Submap& submap); 18 | ~SubmapBoundingVolume() = default; 19 | 20 | // Interaction. 21 | void update(); 22 | bool contains_S(const Point& point_S) const; 23 | bool contains_M(const Point& point_M) const; 24 | bool intersects(const SubmapBoundingVolume& other) const; 25 | bool isInsidePlane_S(const Point& normal_S) const; 26 | bool isInsidePlane_M(const Point& normal_M) const; 27 | 28 | // Access. 29 | FloatingPoint getRadius() const { return radius_; } 30 | const Point& getCenter() const { return center_; } 31 | 32 | private: 33 | const Submap* const submap_; 34 | Point center_; // This is in submap frame. 35 | FloatingPoint radius_; 36 | size_t num_previous_blocks_; 37 | }; 38 | 39 | } // namespace panoptic_mapping 40 | 41 | #endif // PANOPTIC_MAPPING_MAP_SUBMAP_BOUNDING_VOLUME_H_ 42 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map/submap_id.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_SUBMAP_ID_H_ 2 | #define PANOPTIC_MAPPING_MAP_SUBMAP_ID_H_ 3 | 4 | #include 5 | 6 | namespace panoptic_mapping { 7 | 8 | /** 9 | * @brief Class for ID management. Default uses a global singleton but e.g. 10 | * individual submap collections are allowed to have separate. 11 | */ 12 | class SubmapIDManager { 13 | public: 14 | SubmapIDManager(); 15 | 16 | // Access to the global id manager via singleton. 17 | static SubmapIDManager* getGlobalInstance() { 18 | static SubmapIDManager instance; 19 | return &instance; 20 | } 21 | 22 | private: 23 | friend class SubmapID; 24 | 25 | // Interaction. 26 | int requestID(); 27 | void releaseID(int id); 28 | 29 | private: 30 | // Currently submap IDs are assumed to be unique over the lifetime of a run. 31 | static const bool kAllowIDReuse = false; 32 | 33 | int current_id_; 34 | std::queue vacant_ids_; 35 | }; 36 | 37 | /** 38 | * @brief This class instantiates and tracks unique submap IDs. 39 | */ 40 | class SubmapID { 41 | public: 42 | // Controlled con- and destruction. 43 | explicit SubmapID( 44 | SubmapIDManager* manager = SubmapIDManager::getGlobalInstance()); 45 | ~SubmapID(); 46 | 47 | SubmapID(const SubmapID&) = delete; 48 | SubmapID& operator=(const SubmapID&) = delete; 49 | 50 | operator int() const { return id_; } 51 | 52 | private: 53 | friend class Submap; 54 | const int id_; 55 | SubmapIDManager* const manager_; 56 | 57 | // This constructor is specifically for copying submap ids from one id manager 58 | // to the other and does not double check uniqueness of the ids. 59 | SubmapID(int id, SubmapIDManager* manager); 60 | }; 61 | 62 | } // namespace panoptic_mapping 63 | 64 | #endif // PANOPTIC_MAPPING_MAP_SUBMAP_ID_H_ 65 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map_management/activity_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_MANAGEMENT_ACTIVITY_MANAGER_H_ 2 | #define PANOPTIC_MAPPING_MAP_MANAGEMENT_ACTIVITY_MANAGER_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 7 | #include "panoptic_mapping/common/common.h" 8 | #include "panoptic_mapping/map/submap.h" 9 | #include "panoptic_mapping/map/submap_collection.h" 10 | 11 | namespace panoptic_mapping { 12 | 13 | /** 14 | * This class checks when to deactivate inactive submaps 15 | */ 16 | 17 | class ActivityManager { 18 | public: 19 | struct Config : public config_utilities::Config { 20 | int verbosity = 4; 21 | 22 | // New Submaps. 23 | int required_reobservations = 0; // Remove submaps that are not observed in 24 | // x consecutive frames after allocation. 25 | 26 | // Deactivation. 27 | int deactivate_after_missed_detections = 28 | 0; // Deactivate submaps when not observed for x frames. Use 0 to 29 | // ignore. 30 | 31 | Config() { setConfigName("ActivityManager"); } 32 | 33 | protected: 34 | void setupParamsAndPrinting() override; 35 | void checkParams() const override; 36 | }; 37 | 38 | explicit ActivityManager(const Config& config); 39 | virtual ~ActivityManager() = default; 40 | 41 | // Check all criteria. 42 | void processSubmaps(SubmapCollection* submaps); 43 | 44 | private: 45 | bool checkRequiredRedetection(Submap* submap); 46 | void checkMissedDetections(Submap* submap); 47 | 48 | private: 49 | const Config config_; 50 | 51 | // Tracking data. 52 | std::unordered_map submap_redetection_counts_; 53 | std::unordered_map submap_missed_detection_counts_; 54 | }; 55 | 56 | } // namespace panoptic_mapping 57 | 58 | #endif // PANOPTIC_MAPPING_MAP_MANAGEMENT_ACTIVITY_MANAGER_H_ 59 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map_management/layer_manipulator.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_MANAGEMENT_LAYER_MANIPULATOR_H_ 2 | #define PANOPTIC_MAPPING_MAP_MANAGEMENT_LAYER_MANIPULATOR_H_ 3 | 4 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 5 | #include "panoptic_mapping/common/common.h" 6 | #include "panoptic_mapping/map/submap.h" 7 | #include "panoptic_mapping/map/submap_collection.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * @brief This class performs all sort of submap and layer manipulations, such 13 | * as smoothing, merging, pruning, integrating classification into TSDF. 14 | */ 15 | 16 | class LayerManipulator { 17 | public: 18 | struct Config : public config_utilities::Config { 19 | int verbosity = 4; 20 | 21 | bool use_instance_classification = 22 | false; // How to interpret the class layer data. 23 | 24 | Config() { setConfigName("LayerManipulator"); } 25 | 26 | protected: 27 | void setupParamsAndPrinting() override; 28 | void checkParams() const override; 29 | }; 30 | 31 | explicit LayerManipulator(const Config& config); 32 | virtual ~LayerManipulator() = default; 33 | 34 | /* Tools */ 35 | // Trim the TSDF layer according to the provided class layer. Tsdf and class 36 | // layer are expected to have identical layout, extent and transformation. 37 | void applyClassificationLayer(TsdfLayer* tsdf_layer, 38 | const ClassLayer& class_layer, 39 | float truncation_distance) const; 40 | 41 | void mergeSubmapAintoB(const Submap& A, Submap* B) const; 42 | 43 | /** 44 | * @brief Convert a TSDF into an ESDF layer, propagating shortest distances 45 | * through the projective TSDF. 46 | * 47 | * @param layer Layer to ESDFify. 48 | */ 49 | void unprojectTsdfLayer(TsdfLayer* layer) const; 50 | 51 | private: 52 | const Config config_; 53 | }; 54 | 55 | } // namespace panoptic_mapping 56 | 57 | #endif // PANOPTIC_MAPPING_MAP_MANAGEMENT_LAYER_MANIPULATOR_H_ 58 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map_management/map_manager_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_MANAGEMENT_MAP_MANAGER_BASE_H_ 2 | #define PANOPTIC_MAPPING_MAP_MANAGEMENT_MAP_MANAGER_BASE_H_ 3 | 4 | #include "panoptic_mapping/map/submap_collection.h" 5 | 6 | namespace panoptic_mapping { 7 | 8 | /** 9 | * @brief Interface for a high level class that wraps all map management actions 10 | * and tools. 11 | */ 12 | class MapManagerBase { 13 | public: 14 | MapManagerBase() = default; 15 | virtual ~MapManagerBase() = default; 16 | 17 | // Perform all actions when with specified timings. 18 | virtual void tick(SubmapCollection* submaps) = 0; 19 | virtual void finishMapping(SubmapCollection* submaps) = 0; 20 | }; 21 | 22 | } // namespace panoptic_mapping 23 | 24 | #endif // PANOPTIC_MAPPING_MAP_MANAGEMENT_MAP_MANAGER_BASE_H_ 25 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map_management/null_map_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_MANAGEMENT_NULL_MAP_MANAGER_H_ 2 | #define PANOPTIC_MAPPING_MAP_MANAGEMENT_NULL_MAP_MANAGER_H_ 3 | 4 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 5 | #include "panoptic_mapping/map/submap_collection.h" 6 | #include "panoptic_mapping/map_management/map_manager_base.h" 7 | 8 | namespace panoptic_mapping { 9 | 10 | /** 11 | * @brief The null map manager is a default module that does not execute any 12 | * actions. 13 | */ 14 | class NullMapManager : public MapManagerBase { 15 | public: 16 | struct Config : public config_utilities::Config { 17 | Config() { setConfigName("NullMapManager"); } 18 | 19 | protected: 20 | void setupParamsAndPrinting() override; 21 | }; 22 | 23 | explicit NullMapManager(const Config& config) : config_(config) {} 24 | virtual ~NullMapManager() = default; 25 | 26 | // The null map manager does not execute any actions. 27 | void tick(SubmapCollection* submaps) override {} 28 | void finishMapping(SubmapCollection* submaps) override {} 29 | 30 | private: 31 | static config_utilities::Factory::RegistrationRos 33 | registration_; 34 | const Config config_; 35 | }; 36 | 37 | } // namespace panoptic_mapping 38 | 39 | #endif // PANOPTIC_MAPPING_MAP_MANAGEMENT_NULL_MAP_MANAGER_H_ 40 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/map_management/tsdf_registrator.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_MAP_MANAGEMENT_TSDF_REGISTRATOR_H_ 2 | #define PANOPTIC_MAPPING_MAP_MANAGEMENT_TSDF_REGISTRATOR_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 9 | #include "panoptic_mapping/common/common.h" 10 | #include "panoptic_mapping/map/submap.h" 11 | #include "panoptic_mapping/map/submap_collection.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief This class directly compares TSDF volumes of two submaps with each 17 | * other, using the registration constraints adapted from voxgraph to detect 18 | * matches and solve for transformations using ceres. Due to the current 19 | * implementation this is also the de-facto change detection module. 20 | */ 21 | 22 | class TsdfRegistrator { 23 | public: 24 | struct Config : public config_utilities::Config { 25 | int verbosity = 4; 26 | 27 | // Voxels and iso-surface points with lower weights are ignored. 28 | float min_voxel_weight = 1e-6; 29 | 30 | // Allowed error distance in meters where a point is still considered 31 | // belonging to the submap. Negative values are multiples of the voxel_size. 32 | float error_threshold = -1; 33 | 34 | // Minimum number of weight-adjusted points required for a submap to 35 | // conflict. 36 | int match_rejection_points = 50; 37 | 38 | // Minimum percentage of weight-adjusted points required for a submap to 39 | // conflict. 40 | float match_rejection_percentage = 0.1; 41 | 42 | // Minimum number of weight-adjusted points required for a submap to match. 43 | int match_acceptance_points = 50; 44 | 45 | // Minimum percentage of weight-adjusted points required for a submap to 46 | // match. 47 | float match_acceptance_percentage = 0.1; 48 | 49 | // If true normalize all points by their combined TSDF weights. 50 | bool normalize_by_voxel_weight = true; 51 | 52 | // Maximum weight used for weight normalization. Points with weights > 53 | // normalization_max_weight will contribute a constant weight of 1.0. 54 | float normalization_max_weight = 5000.f; 55 | 56 | // Number of threads used to perform change detection. Change detection is 57 | // submap-parallel. 58 | int integration_threads = std::thread::hardware_concurrency(); 59 | 60 | Config() { setConfigName("TsdfRegistrator"); } 61 | 62 | protected: 63 | void setupParamsAndPrinting() override; 64 | void checkParams() const override; 65 | }; 66 | 67 | explicit TsdfRegistrator(const Config& config); 68 | virtual ~TsdfRegistrator() = default; 69 | 70 | void checkSubmapCollectionForChange(SubmapCollection* submaps) const; 71 | 72 | void mergeMatchingSubmaps(SubmapCollection* submaps); 73 | 74 | // Check whether there is significant difference between the two submaps. 75 | bool submapsConflict(const Submap& reference, const Submap& other, 76 | bool* submaps_match = nullptr) const; 77 | 78 | private: 79 | const Config config_; 80 | 81 | // Methods. 82 | bool getDistanceAndWeightAtPoint( 83 | float* distance, float* weight, const IsoSurfacePoint& point, 84 | const Transformation& T_P_S, 85 | const voxblox::Interpolator& interpolator) const; 86 | 87 | float computeCombinedWeight(float w1, float w2) const; 88 | 89 | // For parallel change detection. 90 | std::string checkSubmapForChange(const SubmapCollection& submaps, 91 | Submap* submap) const; 92 | }; 93 | 94 | } // namespace panoptic_mapping 95 | 96 | #endif // PANOPTIC_MAPPING_MAP_MANAGEMENT_TSDF_REGISTRATOR_H_ 97 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/submap_allocation/freespace_allocator_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_SUBMAP_ALLOCATION_FREESPACE_ALLOCATOR_BASE_H_ 2 | #define PANOPTIC_MAPPING_SUBMAP_ALLOCATION_FREESPACE_ALLOCATOR_BASE_H_ 3 | 4 | #include "panoptic_mapping/common/common.h" 5 | #include "panoptic_mapping/common/input_data_user.h" 6 | #include "panoptic_mapping/map/submap.h" 7 | #include "panoptic_mapping/map/submap_collection.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * @brief Interface for freespace allocators. Allocators are called by the ID 13 | * Trackers to create new submaps if suitable. 14 | */ 15 | class FreespaceAllocatorBase : public InputDataUser { 16 | public: 17 | FreespaceAllocatorBase() = default; 18 | ~FreespaceAllocatorBase() override = default; 19 | 20 | // Interface. 21 | /** 22 | * @brief Allocate a new submap based on the provided input data. 23 | * 24 | * @param submaps Collection to allocate the submap in. 25 | * @param input The input data based on which the submap should be allocated. 26 | * @return Pointer to the newly allocated submap, nullptr if submap allocation 27 | * failed. 28 | */ 29 | virtual Submap* allocateSubmap(SubmapCollection* submaps, 30 | InputData* input) = 0; 31 | }; 32 | 33 | } // namespace panoptic_mapping 34 | 35 | #endif // PANOPTIC_MAPPING_SUBMAP_ALLOCATION_FREESPACE_ALLOCATOR_BASE_H_ 36 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/submap_allocation/monolithic_freespace_allocator.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_SUBMAP_ALLOCATION_MONOLITHIC_FREESPACE_ALLOCATOR_H_ 2 | #define PANOPTIC_MAPPING_SUBMAP_ALLOCATION_MONOLITHIC_FREESPACE_ALLOCATOR_H_ 3 | 4 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 5 | #include "panoptic_mapping/labels/label_handler_base.h" 6 | #include "panoptic_mapping/submap_allocation/freespace_allocator_base.h" 7 | 8 | namespace panoptic_mapping { 9 | 10 | /** 11 | * @brief This submap allocator creates a monolithic submap to cover all 12 | * freespace. 13 | */ 14 | class MonolithicFreespaceAllocator : public FreespaceAllocatorBase { 15 | public: 16 | // Config. 17 | struct Config : public config_utilities::Config { 18 | int verbosity = 4; 19 | 20 | // All values for the submap config except the voxel sizes. Use negative 21 | // truncation distance to make them multiples of the voxelsizes. 22 | Submap::Config submap; 23 | 24 | Config() { setConfigName("MonolithicFreespaceAllocator"); } 25 | 26 | protected: 27 | void setupParamsAndPrinting() override; 28 | void checkParams() const override; 29 | }; 30 | 31 | // Construction. 32 | explicit MonolithicFreespaceAllocator(const Config& config, 33 | bool print_config = true); 34 | ~MonolithicFreespaceAllocator() override = default; 35 | 36 | Submap* allocateSubmap(SubmapCollection* submaps, 37 | InputData* /* input */) override; 38 | 39 | private: 40 | static config_utilities::Factory::RegistrationRos< 41 | FreespaceAllocatorBase, MonolithicFreespaceAllocator> 42 | registration_; 43 | const Config config_; 44 | }; 45 | } // namespace panoptic_mapping 46 | 47 | #endif // PANOPTIC_MAPPING_SUBMAP_ALLOCATION_MONOLITHIC_FREESPACE_ALLOCATOR_H_ 48 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/submap_allocation/null_submap_allocator.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_SUBMAP_ALLOCATION_NULL_SUBMAP_ALLOCATOR_H_ 2 | #define PANOPTIC_MAPPING_SUBMAP_ALLOCATION_NULL_SUBMAP_ALLOCATOR_H_ 3 | 4 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 5 | #include "panoptic_mapping/labels/label_entry.h" 6 | #include "panoptic_mapping/submap_allocation/freespace_allocator_base.h" 7 | #include "panoptic_mapping/submap_allocation/submap_allocator_base.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * @brief This submap allocator does not allocate any submaps. 13 | */ 14 | class NullSubmapAllocator : public SubmapAllocatorBase { 15 | public: 16 | struct Config : public config_utilities::Config { 17 | int verbosity = 4; 18 | 19 | Config() { setConfigName("NullSubmapAllocator"); } 20 | 21 | protected: 22 | void setupParamsAndPrinting() override; 23 | }; 24 | explicit NullSubmapAllocator(const Config& config); 25 | ~NullSubmapAllocator() override = default; 26 | 27 | Submap* allocateSubmap(SubmapCollection* /* submaps */, 28 | InputData* /* input */, int /* input_id */, 29 | const LabelEntry& label) override { 30 | return nullptr; 31 | } 32 | 33 | private: 34 | static config_utilities::Factory::RegistrationRos 36 | registration_; 37 | const Config config_; 38 | }; 39 | 40 | /** 41 | * @brief This free space allocator does not allocate any submap. 42 | */ 43 | class NullFreespaceAllocator : public FreespaceAllocatorBase { 44 | public: 45 | struct Config : public config_utilities::Config { 46 | int verbosity = 4; 47 | 48 | Config() { setConfigName("NullFreespaceAllocator"); } 49 | 50 | protected: 51 | void setupParamsAndPrinting() override; 52 | }; 53 | 54 | // Construction. 55 | explicit NullFreespaceAllocator(const Config& config); 56 | ~NullFreespaceAllocator() override = default; 57 | 58 | Submap* allocateSubmap(SubmapCollection* /* submaps */, 59 | InputData* /* input */) override { 60 | return nullptr; 61 | } 62 | 63 | private: 64 | static config_utilities::Factory::RegistrationRos 66 | registration_; 67 | const Config config_; 68 | }; 69 | 70 | } // namespace panoptic_mapping 71 | 72 | #endif // PANOPTIC_MAPPING_SUBMAP_ALLOCATION_NULL_SUBMAP_ALLOCATOR_H_ 73 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/submap_allocation/semantic_submap_allocator.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_SUBMAP_ALLOCATION_SEMANTIC_SUBMAP_ALLOCATOR_H_ 2 | #define PANOPTIC_MAPPING_SUBMAP_ALLOCATION_SEMANTIC_SUBMAP_ALLOCATOR_H_ 3 | 4 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 5 | #include "panoptic_mapping/labels/label_entry.h" 6 | #include "panoptic_mapping/submap_allocation/submap_allocator_base.h" 7 | 8 | namespace panoptic_mapping { 9 | 10 | /** 11 | * @brief This submap allocator chooses a voxel size based on the semantic label 12 | * of submaps. 13 | */ 14 | class SemanticSubmapAllocator : public SubmapAllocatorBase { 15 | public: 16 | // Config. 17 | struct Config : public config_utilities::Config { 18 | int verbosity = 4; 19 | 20 | // All values for the submap config except the voxel sizes and truncation 21 | // distance, these are individually set below. 22 | Submap::Config submap; 23 | 24 | // Voxel size for each label in meters. 25 | float small_instance_voxel_size = 0.03; 26 | float medium_instance_voxel_size = 0.05; 27 | float large_instance_voxel_size = 0.07; 28 | float background_voxel_size = 0.1; 29 | float unknown_voxel_size = 0.1; 30 | 31 | // Truncation distance for all labels in meters. This overwrites the 32 | // submap. Negative values are multiples of the voxel size. 33 | float truncation_distance = -2.f; 34 | 35 | Config() { setConfigName("SemanticSubmapAllocator"); } 36 | 37 | protected: 38 | void setupParamsAndPrinting() override; 39 | void checkParams() const override; 40 | }; 41 | 42 | // Construction. 43 | explicit SemanticSubmapAllocator(const Config& config, 44 | bool print_config = true); 45 | ~SemanticSubmapAllocator() override = default; 46 | 47 | Submap* allocateSubmap(SubmapCollection* submaps, InputData* /* input */, 48 | int /* input_id */, const LabelEntry& label) override; 49 | 50 | private: 51 | static config_utilities::Factory::RegistrationRos 53 | registration_; 54 | const Config config_; 55 | }; 56 | } // namespace panoptic_mapping 57 | 58 | #endif // PANOPTIC_MAPPING_SUBMAP_ALLOCATION_SEMANTIC_SUBMAP_ALLOCATOR_H_ 59 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/submap_allocation/submap_allocator_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_SUBMAP_ALLOCATION_SUBMAP_ALLOCATOR_BASE_H_ 2 | #define PANOPTIC_MAPPING_SUBMAP_ALLOCATION_SUBMAP_ALLOCATOR_BASE_H_ 3 | 4 | #include "panoptic_mapping/common/common.h" 5 | #include "panoptic_mapping/common/input_data_user.h" 6 | #include "panoptic_mapping/labels/label_entry.h" 7 | #include "panoptic_mapping/map/submap.h" 8 | #include "panoptic_mapping/map/submap_collection.h" 9 | 10 | namespace panoptic_mapping { 11 | 12 | /** 13 | * @brief Interface for submap allocators. Allocators are called by the ID 14 | * Trackers to create new submaps if suitable. 15 | */ 16 | class SubmapAllocatorBase : public InputDataUser { 17 | public: 18 | SubmapAllocatorBase() = default; 19 | ~SubmapAllocatorBase() override = default; 20 | 21 | // Interface. 22 | /** 23 | * @brief Allocate a new submap based on the provided input data and label. 24 | * 25 | * @param submaps Collection to allocate the submap in. 26 | * @param input The input data based on which the submap should be allocated. 27 | * @param input_id The id in the id image associated with the submap to be 28 | * allocated. 29 | * @param label The label data to be used to create this submap. 30 | * @return Pointer to the newly allocated submap, nullptr if submap allocation 31 | * failed. 32 | */ 33 | virtual Submap* allocateSubmap(SubmapCollection* submaps, InputData* input, 34 | int input_id, const LabelEntry& label) = 0; 35 | }; 36 | 37 | } // namespace panoptic_mapping 38 | 39 | #endif // PANOPTIC_MAPPING_SUBMAP_ALLOCATION_SUBMAP_ALLOCATOR_BASE_H_ 40 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/test/temporary_file.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TEST_TEMPORARY_FILE_H_ 2 | #define PANOPTIC_MAPPING_TEST_TEMPORARY_FILE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace panoptic_mapping { 13 | namespace test { 14 | 15 | /** 16 | * @brief Opens and manages the lifetime of a temporary file for testing. 17 | * 18 | */ 19 | class TempFile { 20 | public: 21 | explicit TempFile(const std::string& name = "") { 22 | // For Ubuntu /tmp/ should always exist and no subdirectories are used. 23 | auto t = std::time(nullptr); 24 | auto tm = *std::localtime(&t); 25 | std::stringstream ss; 26 | ss << std::put_time(&tm, "%d-%m-%Y_%H-%M-%S"); 27 | file_name_ = "/tmp/" + name + ss.str() + ".tmp"; 28 | stream_.open(file_name_, std::fstream::in | std::fstream::out | 29 | std::fstream::trunc | std::fstream::binary); 30 | } 31 | ~TempFile() { 32 | // Close and remove the temporary file. 33 | if (stream_.is_open()) { 34 | stream_.close(); 35 | } 36 | if (std::experimental::filesystem::exists(file_name_)) { 37 | std::experimental::filesystem::remove(file_name_); 38 | } 39 | } 40 | operator bool() const { return stream_.is_open(); } 41 | std::fstream& stream() { return stream_; } 42 | const std::fstream& stream() const { return stream_; } 43 | 44 | private: 45 | std::fstream stream_; 46 | std::string file_name_; 47 | }; 48 | 49 | } // namespace test 50 | } // namespace panoptic_mapping 51 | 52 | #endif // PANOPTIC_MAPPING_TEST_TEMPORARY_FILE_H_ 53 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/coloring.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_COLORING_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_COLORING_H_ 3 | 4 | #include "panoptic_mapping/common/common.h" 5 | 6 | namespace panoptic_mapping { 7 | /** 8 | * @brief Maps a value in [0, 1] to a color from red (1) via yellow (0.5) to 9 | * green (1). 10 | * 11 | * @param value Input value in [0, 1] 12 | * @return Color 13 | */ 14 | inline Color redToGreenGradient(const float value) { 15 | Color color; 16 | color.b = 0u; 17 | if (value > 0.5f) { 18 | color.r = ((1.f - value) * 2.f * 255.f); 19 | color.g = 255u; 20 | } else { 21 | color.r = 255u; 22 | color.g = (value * 2.f * 255.f); 23 | } 24 | return color; 25 | } 26 | 27 | } // namespace panoptic_mapping 28 | 29 | #endif // PANOPTIC_MAPPING_TOOLS_COLORING_H_ 30 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/data_writer_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_DATA_WRITER_BASE_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_DATA_WRITER_BASE_H_ 3 | 4 | #include "panoptic_mapping/map/submap_collection.h" 5 | 6 | namespace panoptic_mapping { 7 | 8 | /** 9 | * @brief Interface for a utility class that evaluates certain metrics during 10 | * experiments and writes them to a log file. 11 | */ 12 | class DataWriterBase { 13 | public: 14 | DataWriterBase() = default; 15 | virtual ~DataWriterBase() = default; 16 | 17 | virtual void writeData(double time_stamp, 18 | const SubmapCollection& submaps) = 0; 19 | }; 20 | 21 | } // namespace panoptic_mapping 22 | 23 | #endif // PANOPTIC_MAPPING_TOOLS_DATA_WRITER_BASE_H_ 24 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/evaluation_data_writer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_EVALUATION_DATA_WRITER_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_EVALUATION_DATA_WRITER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 9 | #include "panoptic_mapping/common/common.h" 10 | #include "panoptic_mapping/map/submap_collection.h" 11 | #include "panoptic_mapping/tools/log_data_writer.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief Utility class that evaluates allocates a full evaluation folder to 17 | * this run. In that folder, certain evaluated metrics will be logged and the 18 | * map stored periodically for further evaluations. 19 | */ 20 | class EvaluationDataWriter : public LogDataWriter { 21 | public: 22 | struct Config : public config_utilities::Config { 23 | int verbosity = 4; 24 | 25 | // Log data writer config. 26 | LogDataWriter::Config log_data_writer_config; 27 | 28 | // Evaluation Fields. 29 | int store_map_every_n_frames = 0; 30 | 31 | Config() { setConfigName("EvaluationDataWriter"); } 32 | 33 | protected: 34 | void setupParamsAndPrinting() override; 35 | void checkParams() const override; 36 | }; 37 | 38 | explicit EvaluationDataWriter(const Config& config); 39 | ~EvaluationDataWriter() override = default; 40 | 41 | private: 42 | static config_utilities::Factory::RegistrationRos 44 | registration_; 45 | const Config config_; 46 | 47 | // Tracking variables. 48 | int store_submap_frame_ = 0; 49 | int store_submap_counter_ = 0; 50 | 51 | // Methods. 52 | void setupLogFile() override; 53 | void setupEvaluations() override; 54 | 55 | // Evaluations. The data are always preceded by a separating comma. 56 | void storeSubmaps(const SubmapCollection& submaps); 57 | }; 58 | 59 | } // namespace panoptic_mapping 60 | 61 | #endif // PANOPTIC_MAPPING_TOOLS_EVALUATION_DATA_WRITER_H_ 62 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/log_data_writer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_LOG_DATA_WRITER_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_LOG_DATA_WRITER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 10 | #include "panoptic_mapping/common/common.h" 11 | #include "panoptic_mapping/map/submap_collection.h" 12 | #include "panoptic_mapping/tools/data_writer_base.h" 13 | 14 | namespace panoptic_mapping { 15 | 16 | /** 17 | * @brief Utility class that evaluates certain metrics during experiments and 18 | * writes them to a log file. 19 | */ 20 | class LogDataWriter : public DataWriterBase { 21 | public: 22 | struct Config : public config_utilities::Config { 23 | int verbosity = 4; 24 | 25 | // Target directory to write the output to. This needs to exist already. 26 | std::string output_directory = ""; 27 | 28 | // File name of the log file without suffix. 29 | std::string file_name = "panoptic_mapping_data"; 30 | 31 | // Which fields to evaluate Fields. 32 | bool evaluate_number_of_submaps = true; 33 | bool evaluate_number_of_active_submaps = true; 34 | bool evaluate_number_of_objects = true; 35 | 36 | Config() { setConfigName("LogDataWriter"); } 37 | 38 | protected: 39 | void setupParamsAndPrinting() override; 40 | void checkParams() const override; 41 | }; 42 | 43 | explicit LogDataWriter(const Config& config, bool print_config = true); 44 | ~LogDataWriter() override; 45 | 46 | void writeData(double time_stamp, const SubmapCollection& submaps) override; 47 | 48 | private: 49 | static config_utilities::Factory::RegistrationRos 51 | registration_; 52 | const Config config_; 53 | 54 | protected: 55 | // Data. 56 | bool is_setup_ = false; 57 | std::string output_path_; 58 | std::string outfile_name_; 59 | std::ofstream outfile_; 60 | std::vector> evaluations_; 61 | 62 | // Methods. 63 | virtual void setup(); 64 | virtual void setupLogFile(); 65 | virtual void setupEvaluations(); 66 | void writeEntry(const std::string& value); 67 | 68 | // Evaluations. The data are always preceded by a separating comma. 69 | void evaluateNumberOfSubmaps(const SubmapCollection& submaps); 70 | void evaluateNumberOfActiveSubmaps(const SubmapCollection& submaps); 71 | void evaluateNumberOfObjects(const SubmapCollection& submaps); 72 | }; 73 | 74 | } // namespace panoptic_mapping 75 | 76 | #endif // PANOPTIC_MAPPING_TOOLS_LOG_DATA_WRITER_H_ 77 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/map_renderer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_MAP_RENDERER_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_MAP_RENDERER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 8 | #include "panoptic_mapping/common/camera.h" 9 | #include "panoptic_mapping/common/common.h" 10 | #include "panoptic_mapping/map/submap.h" 11 | #include "panoptic_mapping/map/submap_collection.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * Preliminary tool to visualize the map for debugging. Not very efficient or 17 | * sophisticated. 18 | */ 19 | class MapRenderer { 20 | public: 21 | struct Config : public config_utilities::Config { 22 | int verbosity = 4; 23 | 24 | bool impaint_voxel_size = false; 25 | 26 | Config() { setConfigName("MapRenderer"); } 27 | 28 | protected: 29 | void setupParamsAndPrinting() override; 30 | void checkParams() const override; 31 | }; 32 | 33 | MapRenderer(const Config& config, const Camera::Config& camera, 34 | bool print_config = true); 35 | virtual ~MapRenderer() = default; 36 | 37 | // Tools. 38 | // Approximate rendering. 39 | cv::Mat renderActiveSubmapIDs(const SubmapCollection& submaps, 40 | const Transformation& T_M_C); 41 | cv::Mat renderActiveSubmapClasses(const SubmapCollection& submaps, 42 | const Transformation& T_M_C); 43 | cv::Mat colorIdImage(const cv::Mat& id_image, int colors_per_revolution = 20); 44 | 45 | private: 46 | const Config config_; 47 | Camera camera_; 48 | 49 | // 50 | Eigen::MatrixXf range_image_; 51 | voxblox::ExponentialOffsetIdColorMap id_color_map_; 52 | 53 | // Methods. 54 | cv::Mat render(const SubmapCollection& submaps, const Transformation& T_M_C, 55 | bool only_active_submaps, int (*paint)(const Submap&)); 56 | static int paintSubmapID(const Submap& submap); 57 | static int paintClass(const Submap& submap); 58 | }; 59 | 60 | } // namespace panoptic_mapping 61 | 62 | #endif // PANOPTIC_MAPPING_TOOLS_MAP_RENDERER_H_ 63 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/null_data_writer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_NULL_DATA_WRITER_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_NULL_DATA_WRITER_H_ 3 | 4 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 5 | #include "panoptic_mapping/map/submap_collection.h" 6 | #include "panoptic_mapping/tools/data_writer_base.h" 7 | 8 | namespace panoptic_mapping { 9 | 10 | /** 11 | * @brief The null data writer is a default modulethat does not write any data. 12 | */ 13 | class NullDataWriter : public DataWriterBase { 14 | public: 15 | struct Config : public config_utilities::Config { 16 | Config() { setConfigName("NullDataWriter"); } 17 | 18 | protected: 19 | void setupParamsAndPrinting() override; 20 | }; 21 | 22 | explicit NullDataWriter(const Config& config) : config_(config) {} 23 | virtual ~NullDataWriter() = default; 24 | 25 | // The null data writer does not write any data. 26 | void writeData(double time_stamp, const SubmapCollection& submaps) override{}; 27 | 28 | private: 29 | static config_utilities::Factory::RegistrationRos 31 | registration_; 32 | const Config config_; 33 | }; 34 | 35 | } // namespace panoptic_mapping 36 | 37 | #endif // PANOPTIC_MAPPING_TOOLS_NULL_DATA_WRITER_H_ 38 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/planning_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_PLANNING_INTERFACE_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_PLANNING_INTERFACE_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/map/submap.h" 7 | #include "panoptic_mapping/map/submap_collection.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * @brief This class implements a high level interfaces for lookups on the 13 | * submap collection. 14 | */ 15 | class PlanningInterface { 16 | public: 17 | explicit PlanningInterface(std::shared_ptr submaps); 18 | 19 | enum class VoxelState { 20 | kUnknown = 0, 21 | kKnownFree, 22 | kKnownOccupied, 23 | kPersistentOccupied, 24 | kExpectedFree, 25 | kExpectedOccupied, 26 | }; 27 | 28 | // Access. 29 | const SubmapCollection& getSubmapCollection() const { return *submaps_; } 30 | 31 | // Lookups. 32 | /** 33 | * @brief Check if a point is observed in the map. 34 | * 35 | * @param position Position of point in world frame. 36 | * @param consider_change_state If false considers only submaps considered 37 | * present, i.e. whose ChangeState is active or persistent. 38 | * @param include_inactive_maps If false considers only active submaps. 39 | * @return True if the point was observed. 40 | */ 41 | bool isObserved(const Point& position, bool consider_change_state = true, 42 | bool include_inactive_maps = true) const; 43 | 44 | /** 45 | * @brief Compute the voxel state of a point in the map for planning. 46 | * 47 | * @param position Position of point in world frame. 48 | * @return VoxelState of the given point. 49 | */ 50 | VoxelState getVoxelState(const Point& position) const; 51 | 52 | /** 53 | * @brief Computes the truncated signed distance function (TSDF) at a point in 54 | * the multi-resolution map. 55 | * 56 | * @param position Position of point in world frame. 57 | * @param distance Pointer to value to store the distance in. 58 | * @param consider_change_state If true considers only submaps considered 59 | * present, i.e. whose ChangeState is active or persistent. 60 | * @param include_free_space If true points lying only in free space are 61 | * considered observed. 62 | * @return True if the point was observed and thus a distance returned. 63 | */ 64 | bool getDistance(const Point& position, float* distance, 65 | bool consider_change_state = true, 66 | bool include_free_space = true) const; 67 | 68 | private: 69 | std::shared_ptr submaps_; 70 | static constexpr float kObservedMinWeight_ = 1e-6; 71 | }; 72 | 73 | } // namespace panoptic_mapping 74 | 75 | #endif // PANOPTIC_MAPPING_TOOLS_PLANNING_INTERFACE_H_ 76 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/serialization.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_SERIALIZATION_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_SERIALIZATION_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "panoptic_mapping/Submap.pb.h" 10 | #include "panoptic_mapping/common/common.h" 11 | #include "panoptic_mapping/map/classification/class_layer.h" 12 | #include "panoptic_mapping/map/scores/score_layer.h" 13 | #include "panoptic_mapping/map/submap.h" 14 | 15 | namespace panoptic_mapping { 16 | 17 | /** 18 | * Various serialization and de-serialization tools. 19 | */ 20 | 21 | // To and from uint32_t serialization. 22 | inline uint32_t int32FromTwoInt16(uint16_t first, uint16_t second) { 23 | return ((static_cast(first) << 16) + second); 24 | } 25 | 26 | inline uint16_t int16FromTwoInt8(uint8_t first, uint8_t second) { 27 | return ((static_cast(first) << 8) + second); 28 | } 29 | 30 | inline std::pair twoInt16FromInt32(uint32_t data) { 31 | return {static_cast(data >> 16), 32 | static_cast(data & 0xFFFF)}; 33 | } 34 | 35 | inline std::pair twoInt8FromInt16(uint16_t data) { 36 | return {static_cast(data >> 8), static_cast(data & 0xFF)}; 37 | } 38 | 39 | template 40 | inline uint32_t int32FromX32(T data) { 41 | if (sizeof(T) != sizeof(uint32_t)) { 42 | LOG(WARNING) << "Can not serialize '" << typeid(T).name() << "' of size '" 43 | << sizeof(T) << "' to uint32_t of size 32."; 44 | return 0; 45 | } 46 | uint32_t result; 47 | memcpy(&result, &data, sizeof(uint32_t)); 48 | return result; 49 | } 50 | 51 | template 52 | inline T x32FromInt32(uint32_t data) { 53 | T result; 54 | if (sizeof(T) != sizeof(uint32_t)) { 55 | LOG(WARNING) << "Can not deserialize '" << typeid(T).name() << "' of size '" 56 | << sizeof(T) << "' from uint32_t of size 32."; 57 | return result; 58 | } 59 | memcpy(&result, &data, sizeof(uint32_t)); 60 | return result; 61 | } 62 | 63 | std::unique_ptr loadClassLayerFromStream( 64 | const SubmapProto& submap_proto, std::istream* proto_file_ptr, 65 | uint64_t* tmp_byte_offset_ptr); 66 | 67 | bool saveClassLayerToStream(const ClassLayer& layer); 68 | 69 | bool loadClassBlocksFromStream(const SubmapProto& submap_proto, 70 | std::istream* proto_file_ptr, 71 | uint64_t* tmp_byte_offset_ptr, 72 | ClassLayer* layer); 73 | 74 | bool isCompatible(const voxblox::BlockProto& block_proto, 75 | const ClassLayer& layer); 76 | 77 | std::unique_ptr loadScoreLayerFromStream( 78 | const SubmapProto& submap_proto, std::istream* proto_file_ptr, 79 | uint64_t* tmp_byte_offset_ptr); 80 | 81 | bool saveScoreLayerToStream(const ScoreLayer& layer); 82 | 83 | bool loadScoreBlocksFromStream(const SubmapProto& submap_proto, 84 | std::istream* proto_file_ptr, 85 | uint64_t* tmp_byte_offset_ptr, 86 | ScoreLayer* layer); 87 | 88 | bool isCompatible(const voxblox::BlockProto& block_proto, 89 | const ScoreLayer& layer); 90 | } // namespace panoptic_mapping 91 | 92 | #endif // PANOPTIC_MAPPING_TOOLS_SERIALIZATION_H_ 93 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tools/thread_safe_submap_collection.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TOOLS_THREAD_SAFE_SUBMAP_COLLECTION_H_ 2 | #define PANOPTIC_MAPPING_TOOLS_THREAD_SAFE_SUBMAP_COLLECTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/common/common.h" 8 | #include "panoptic_mapping/map/submap_collection.h" 9 | 10 | namespace panoptic_mapping { 11 | 12 | /** 13 | * This class wraps a submap collection for thread-safe read-only access by 14 | * simply copying the original submap collection. 15 | */ 16 | 17 | class ThreadSafeSubmapCollection { 18 | public: 19 | /* Construction */ 20 | explicit ThreadSafeSubmapCollection(std::shared_ptr submaps) 21 | : submaps_source_(std::move(submaps)), updated_(new bool()) { 22 | update(); 23 | } 24 | virtual ~ThreadSafeSubmapCollection() = default; 25 | 26 | /* Interaction */ 27 | // Update the lookup submaps based on the source submaps. 28 | void update() { 29 | Timer timer("tools/thread_safe_submap_collection/update"); 30 | submaps_ = submaps_source_->clone(); 31 | timer.Stop(); 32 | *updated_ = true; 33 | } 34 | 35 | // Check whether there were any updates since the last lookup. 36 | bool wasUpdated() const { return *updated_; } 37 | 38 | // Get thread-safe read-only access to the collection 39 | const SubmapCollection& getSubmaps() const { 40 | *updated_ = false; 41 | return *submaps_; 42 | } 43 | 44 | private: 45 | // Reference to the original submap collection. 46 | std::shared_ptr submaps_source_; 47 | 48 | // Deep copy of the submap collection for thread-safe read-only access. 49 | std::unique_ptr submaps_; 50 | 51 | // Track whether the submaps were updated since last lookup. 52 | std::unique_ptr updated_; 53 | }; 54 | 55 | } // namespace panoptic_mapping 56 | 57 | #endif // PANOPTIC_MAPPING_TOOLS_THREAD_SAFE_SUBMAP_COLLECTION_H_ 58 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tracking/detectron_id_tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TRACKING_DETECTRON_ID_TRACKER_H_ 2 | #define PANOPTIC_MAPPING_TRACKING_DETECTRON_ID_TRACKER_H_ 3 | 4 | #include 5 | 6 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 7 | #include "panoptic_mapping/tracking/projective_id_tracker.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | /** 12 | * This id tracker tries to match predictions of the detectron2 panoptic 13 | * semgentation (https://github.com/facebookresearch/detectron2) against the 14 | * map for integration. 15 | */ 16 | class DetectronIDTracker : public ProjectiveIDTracker { 17 | public: 18 | struct Config : public config_utilities::Config { 19 | int verbosity = 4; 20 | ProjectiveIDTracker::Config projective_id_tracker; 21 | 22 | Config() { setConfigName("DetectronIDTracker"); } 23 | 24 | protected: 25 | void setupParamsAndPrinting() override; 26 | void checkParams() const override; 27 | }; 28 | 29 | DetectronIDTracker(const Config& config, std::shared_ptr globals); 30 | ~DetectronIDTracker() override = default; 31 | 32 | void processInput(SubmapCollection* submaps, InputData* input) override; 33 | 34 | protected: 35 | Submap* allocateSubmap(int input_id, SubmapCollection* submaps, 36 | InputData* input) override; 37 | bool classesMatch(int input_id, int submap_class_id) override; 38 | 39 | private: 40 | static config_utilities::Factory::RegistrationRos< 41 | IDTrackerBase, DetectronIDTracker, std::shared_ptr> 42 | registration_; 43 | 44 | // Members. 45 | const Config config_; 46 | 47 | // Cached labels. 48 | const DetectronLabels* labels_; 49 | }; 50 | 51 | } // namespace panoptic_mapping 52 | 53 | #endif // PANOPTIC_MAPPING_TRACKING_DETECTRON_ID_TRACKER_H_ 54 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tracking/ground_truth_id_tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TRACKING_GROUND_TRUTH_ID_TRACKER_H_ 2 | #define PANOPTIC_MAPPING_TRACKING_GROUND_TRUTH_ID_TRACKER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 9 | #include "panoptic_mapping/labels/label_handler_base.h" 10 | #include "panoptic_mapping/tracking/id_tracker_base.h" 11 | 12 | namespace panoptic_mapping { 13 | 14 | /** 15 | * This id tracker looks up the corresponding ground truth instance id for each 16 | * submap. Assumes that the input_ids are unique and persistent instance ids. 17 | */ 18 | class GroundTruthIDTracker : public IDTrackerBase { 19 | public: 20 | struct Config : public config_utilities::Config { 21 | int verbosity = 4; 22 | 23 | Config() { setConfigName("GroundTruthIDTracker"); } 24 | 25 | protected: 26 | void setupParamsAndPrinting() override; 27 | }; 28 | 29 | GroundTruthIDTracker(const Config& config, std::shared_ptr globals, 30 | bool print_config = true); 31 | ~GroundTruthIDTracker() override = default; 32 | 33 | void processInput(SubmapCollection* submaps, InputData* input) override; 34 | 35 | protected: 36 | /** 37 | * @brief Check whether this instance id is already allocated or allocate if 38 | * possible. 39 | * 40 | * @param instance Segmentation input id to use. 41 | * @param submaps Submap collection to allocate the submap in. 42 | * @param input Input data based upon which the submap will be allocated. 43 | * @return True if the submap was allocated or tracked. 44 | */ 45 | bool parseInputInstance(int instance, SubmapCollection* submaps, 46 | InputData* input); 47 | 48 | /** 49 | * @brief Print all warnings of missing submaps to console and reset the 50 | * counter. 51 | */ 52 | void printAndResetWarnings(); 53 | 54 | private: 55 | static config_utilities::Factory::RegistrationRos< 56 | IDTrackerBase, GroundTruthIDTracker, std::shared_ptr> 57 | registration_; 58 | const Config config_; 59 | std::unordered_map instance_to_id_; // Track active maps. 60 | std::unordered_map unknown_ids; // For error handling. 61 | }; 62 | 63 | } // namespace panoptic_mapping 64 | 65 | #endif // PANOPTIC_MAPPING_TRACKING_GROUND_TRUTH_ID_TRACKER_H_ 66 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tracking/id_tracker_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TRACKING_ID_TRACKER_BASE_H_ 2 | #define PANOPTIC_MAPPING_TRACKING_ID_TRACKER_BASE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "panoptic_mapping/common/common.h" 13 | #include "panoptic_mapping/common/globals.h" 14 | #include "panoptic_mapping/common/input_data_user.h" 15 | #include "panoptic_mapping/map/submap_collection.h" 16 | #include "panoptic_mapping/submap_allocation/freespace_allocator_base.h" 17 | #include "panoptic_mapping/submap_allocation/submap_allocator_base.h" 18 | 19 | namespace panoptic_mapping { 20 | 21 | /** 22 | * @brief This class tracks and matches input to submap ids and allocates new 23 | * submaps where necessary using its submap allocators. 24 | */ 25 | class IDTrackerBase : public InputDataUser { 26 | public: 27 | explicit IDTrackerBase(std::shared_ptr globals) 28 | : globals_(std::move(globals)) {} 29 | ~IDTrackerBase() override = default; 30 | 31 | // Interface. 32 | /** 33 | * @brief Changes the id image of the input data to track and match existing 34 | * submaps and allocate new submaps where necessary. 35 | * 36 | * @param submaps The submap collection to track against. 37 | * @param input The input data to process. 38 | */ 39 | virtual void processInput(SubmapCollection* submaps, InputData* input) = 0; 40 | 41 | // Setters for external setup. 42 | /** 43 | * @brief Sets a callback that is called whenever an image needs to be 44 | * visualized. 45 | * 46 | * @param callback The function that is called to visualize images. 47 | */ 48 | void setVisualizationCallback( 49 | std::function callback) { 50 | visualization_callback_ = std::move(callback); 51 | visualize_ = true; 52 | } 53 | 54 | /** 55 | * @brief Set the submap allocator that is used to allocate non-freespace 56 | * maps. 57 | * 58 | * @param submap_allocator The submap allocator to be used. 59 | */ 60 | void setSubmapAllocator( 61 | std::shared_ptr submap_allocator) { 62 | submap_allocator_ = std::move(submap_allocator); 63 | } 64 | 65 | /** 66 | * @brief Set the submap allocator that is used to allocate freespace maps. 67 | * 68 | * @param submap_allocator The submap allocator to be used. 69 | */ 70 | void setFreespaceAllocator( 71 | std::shared_ptr submap_allocator) { 72 | freespace_allocator_ = std::move(submap_allocator); 73 | } 74 | 75 | protected: 76 | // Members for access. 77 | std::shared_ptr globals_; 78 | std::shared_ptr submap_allocator_; 79 | std::shared_ptr freespace_allocator_; 80 | 81 | // Visualization 82 | bool visualizationIsOn() const { return visualize_; } 83 | void visualize(const cv::Mat& image, const std::string& name) { 84 | if (visualize_) { 85 | visualization_callback_(image, name); 86 | } 87 | } 88 | 89 | private: 90 | bool visualize_ = false; 91 | std::function 92 | visualization_callback_; 93 | }; 94 | 95 | } // namespace panoptic_mapping 96 | 97 | #endif // PANOPTIC_MAPPING_TRACKING_ID_TRACKER_BASE_H_ 98 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tracking/single_tsdf_tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TRACKING_SINGLE_TSDF_TRACKER_H_ 2 | #define PANOPTIC_MAPPING_TRACKING_SINGLE_TSDF_TRACKER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "panoptic_mapping/3rd_party/config_utilities.hpp" 9 | #include "panoptic_mapping/labels/label_handler_base.h" 10 | #include "panoptic_mapping/map/classification/class_layer.h" 11 | #include "panoptic_mapping/tracking/id_tracker_base.h" 12 | 13 | namespace panoptic_mapping { 14 | 15 | /** 16 | * @brief Allocates a single submap to emulate running a monolithic TSDF grid as 17 | * map representation. Combine this module with the SingleTsdfIntegrator. 18 | */ 19 | class SingleTSDFTracker : public IDTrackerBase { 20 | public: 21 | struct Config : public config_utilities::Config { 22 | int verbosity = 4; 23 | 24 | // Submap allocation config. Set use class_layer to true to perform label 25 | // integration. 26 | Submap::Config submap; 27 | 28 | // System params. 29 | bool use_detectron = false; 30 | bool use_instance_classification = false; 31 | 32 | Config() { setConfigName("SingleTSDFTracker"); } 33 | 34 | protected: 35 | void setupParamsAndPrinting() override; 36 | void checkParams() const override; 37 | }; 38 | 39 | SingleTSDFTracker(const Config& config, std::shared_ptr globals); 40 | ~SingleTSDFTracker() override = default; 41 | 42 | void processInput(SubmapCollection* submaps, InputData* input) override; 43 | 44 | protected: 45 | // Setup utility. 46 | void setup(SubmapCollection* submaps); 47 | 48 | void parseDetectronClasses(InputData* input); 49 | 50 | private: 51 | static config_utilities::Factory::RegistrationRos< 52 | IDTrackerBase, SingleTSDFTracker, std::shared_ptr> 53 | registration_; 54 | const Config config_; 55 | 56 | int map_id_; 57 | bool is_setup_ = false; 58 | }; 59 | 60 | } // namespace panoptic_mapping 61 | 62 | #endif // PANOPTIC_MAPPING_TRACKING_SINGLE_TSDF_TRACKER_H_ 63 | -------------------------------------------------------------------------------- /panoptic_mapping/include/panoptic_mapping/tracking/tracking_info.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_TRACKING_TRACKING_INFO_H_ 2 | #define PANOPTIC_MAPPING_TRACKING_TRACKING_INFO_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "panoptic_mapping/common/camera.h" 15 | 16 | namespace panoptic_mapping { 17 | 18 | /** 19 | * Tool to aid tracking metrics computation / comparison. 20 | */ 21 | 22 | // NOTE(schmluk): Currently performs input validity checks (max ranges etc) 23 | // independent of other modules, maybe best to do this once and combine. 24 | 25 | class TrackingInfoAggregator; 26 | 27 | class TrackingInfo { 28 | public: 29 | explicit TrackingInfo(int submap_id) : submap_id_(submap_id) {} 30 | TrackingInfo(int submap_id, Camera::Config camera); 31 | ~TrackingInfo() = default; 32 | 33 | // Approximate rendering. 34 | void insertRenderedPoint(int u, int v, int size_x, int size_y); 35 | void evaluate(const cv::Mat& id_image, const cv::Mat& depth_image); 36 | 37 | // Vertex rendering. 38 | void insertVertexPoint(int input_id); 39 | void insertVertexVisualizationPoint(int u, int v) { 40 | points_.push_back({u, v}); 41 | } 42 | const std::vector& getPoints() const { return points_; } 43 | int getSubmapID() const { return submap_id_; } 44 | 45 | private: 46 | friend TrackingInfoAggregator; 47 | const int submap_id_; 48 | std::unordered_map counts_; // 49 | 50 | // Approximate rendering. 51 | const Camera::Config camera_; 52 | int u_min_, u_max_, v_min_, v_max_; 53 | cv::Mat image_; 54 | 55 | // Visualization data vertex rendering. 56 | std::vector points_; 57 | }; 58 | 59 | // Summarize the final tracking data. 60 | class TrackingInfoAggregator { 61 | public: 62 | TrackingInfoAggregator() = default; 63 | ~TrackingInfoAggregator() = default; 64 | 65 | // Input. 66 | void insertTrackingInfos(const std::vector& infos); 67 | void insertTrackingInfo(const TrackingInfo& info); 68 | void insertInputImage(const cv::Mat& id_image, const cv::Mat& depth_image, 69 | const Camera::Config& camera, 70 | int rendering_subsampling); 71 | 72 | // Get results. Requires that all input data is already set. 73 | std::vector getInputIDs() const; 74 | bool getHighestMetric(int input_id, int* submap_id, float* value, 75 | const std::string& metric = "iou") const; 76 | bool getAllMetrics(int input_id, std::vector>* id_value, 77 | const std::string& metric = "iou") const; 78 | 79 | // Get data. 80 | int getNumberOfInputPixels(int input_id) const; 81 | int getNumberOfSubmapPixels(int submap_id) const; 82 | int getNumberOfOverlappingPixels(int input_id, int submap_id) const; 83 | 84 | private: 85 | std::function getComputeValueFunction( 86 | const std::string& metric) const; 87 | float computIoU(int input_id, int submap_id) const; 88 | float computOverlap(int input_id, int submap_id) const; 89 | 90 | // Data. 91 | std::unordered_map> 92 | overlap_; // > 93 | std::unordered_map total_rendered_count_; // 94 | std::unordered_map total_input_count_; // 95 | }; 96 | 97 | } // namespace panoptic_mapping 98 | 99 | #endif // PANOPTIC_MAPPING_TRACKING_TRACKING_INFO_H_ 100 | -------------------------------------------------------------------------------- /panoptic_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panoptic_mapping 4 | 0.0.0 5 | Prototyping a panoptic metric map. 6 | 7 | Lukas Schmid 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | voxblox 16 | cblox 17 | 18 | xmlrpcpp 19 | eigen_catkin 20 | glog_catkin 21 | gflags_catkin 22 | protobuf_catkin 23 | opencv3_catkin 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /panoptic_mapping/proto/panoptic_mapping/ClassLayer.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package panoptic_mapping; 4 | 5 | message ClassLayerProto { 6 | // Directly store binary data. 7 | repeated uint32 voxel_data = 1; 8 | } 9 | -------------------------------------------------------------------------------- /panoptic_mapping/proto/panoptic_mapping/Submap.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package panoptic_mapping; 4 | 5 | import public "cblox/QuatTransformation.proto"; 6 | 7 | message SubmapProto { 8 | // Submap data. 9 | optional int32 instance_id = 1; 10 | optional int32 class_id = 6; 11 | optional int32 panoptic_label = 7; 12 | optional string name = 9; 13 | optional int32 change_state = 11; 14 | 15 | // Tsdf Layer. 16 | optional uint32 num_blocks = 2; 17 | optional float voxel_size = 3; 18 | optional uint32 voxels_per_side = 4; 19 | optional float truncation_distance = 8; 20 | 21 | // Classification Layer. 22 | optional uint32 num_class_blocks = 12; 23 | optional int32 class_voxel_type = 13; 24 | 25 | // Score Layer. 26 | optional uint32 num_score_blocks = 14; 27 | optional int32 score_voxel_type = 15; 28 | 29 | // Submap Transformation. 30 | optional cblox.QuatTransformationProto transform = 5; 31 | optional string frame_name = 10; 32 | } 33 | -------------------------------------------------------------------------------- /panoptic_mapping/proto/panoptic_mapping/SubmapCollection.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package panoptic_mapping; 4 | 5 | message SubmapCollectionProto { 6 | optional uint32 num_submaps = 1; 7 | optional uint32 active_freespace_submap_id = 2; 8 | } 9 | -------------------------------------------------------------------------------- /panoptic_mapping/src/common/class_voxel_layer.cpp: -------------------------------------------------------------------------------- 1 | // #include "panoptic_mapping/common/class_voxel_layer.h" 2 | 3 | #include "panoptic_mapping/common/common.h" 4 | // #include "panoptic_mapping/tools/serialization.h" 5 | 6 | namespace panoptic_mapping { 7 | 8 | // template <> 9 | // bool ClassVoxelLayer::saveBlocksToStream( 10 | // bool include_all_blocks, voxblox::BlockIndexList blocks_to_include, 11 | // std::fstream* outfile_ptr) const { 12 | // CHECK_NOTNULL(outfile_ptr); 13 | // for (const BlockMapPair& pair : block_map_) { 14 | // bool write_block_to_file = include_all_blocks; 15 | // if (!write_block_to_file) { 16 | // voxblox::BlockIndexList::const_iterator it = std::find( 17 | // blocks_to_include.begin(), blocks_to_include.end(), pair.first); 18 | // if (it != blocks_to_include.end()) { 19 | // write_block_to_file = true; 20 | // } 21 | // } 22 | // if (write_block_to_file) { 23 | // voxblox::BlockProto block_proto; 24 | // voxblox::getProtoForBlock( 25 | // pair.second.get(), &block_proto, serialize_top_n_classes_); 26 | 27 | // if (!voxblox::utils::writeProtoMsgToStream(block_proto, outfile_ptr)) { 28 | // LOG(ERROR) << "Could not write block message."; 29 | // return false; 30 | // } 31 | // } 32 | // } 33 | // return true; 34 | // } 35 | 36 | } // namespace panoptic_mapping 37 | -------------------------------------------------------------------------------- /panoptic_mapping/src/common/input_data_user.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/common/input_data_user.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace panoptic_mapping { 7 | 8 | bool InputDataUser::inputIsValid(const InputData& input_data, 9 | bool raise_warning) { 10 | // Check whether all inputs are set and otherwise raise a warning if 11 | // requested. 12 | std::string error_msg; 13 | for (const InputData::InputType& input : required_inputs_) { 14 | if (!input_data.has(input)) { 15 | if (raise_warning) { 16 | if (error_msg.empty()) { 17 | error_msg = "The inputs '" + InputData::inputTypeToString(input); 18 | } else { 19 | error_msg += "', '" + InputData::inputTypeToString(input); 20 | } 21 | } else { 22 | return false; 23 | } 24 | } 25 | } 26 | if (raise_warning) { 27 | if (error_msg.empty()) { 28 | return true; 29 | } 30 | LOG(WARNING) << error_msg << "' are required and not provided."; 31 | return false; 32 | } 33 | return true; 34 | } 35 | 36 | void InputDataUser::addRequiredInput(InputData::InputType type) { 37 | required_inputs_.insert(type); 38 | } 39 | 40 | void InputDataUser::addRequiredInputs(const InputData::InputTypes& types) { 41 | for (const auto& type : types) { 42 | required_inputs_.insert(type); 43 | } 44 | } 45 | 46 | void InputDataUser::setRequiredInputs(const InputData::InputTypes& types) { 47 | required_inputs_.clear(); 48 | for (const auto& type : types) { 49 | required_inputs_.insert(type); 50 | } 51 | } 52 | 53 | } // namespace panoptic_mapping 54 | -------------------------------------------------------------------------------- /panoptic_mapping/src/labels/label_handler_base.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/labels/label_handler_base.h" 2 | 3 | #include 4 | 5 | namespace panoptic_mapping { 6 | 7 | bool LabelHandlerBase::segmentationIdExists(int segmentation_id) const { 8 | return labels_.find(segmentation_id) != labels_.end(); 9 | } 10 | 11 | int LabelHandlerBase::getClassID(int segmentation_id) const { 12 | return labels_.at(segmentation_id)->class_id; 13 | } 14 | 15 | bool LabelHandlerBase::isBackgroundClass(int segmentation_id) const { 16 | return labels_.at(segmentation_id)->label == PanopticLabel::kBackground; 17 | } 18 | 19 | bool LabelHandlerBase::isInstanceClass(int segmentation_id) const { 20 | return labels_.at(segmentation_id)->label == PanopticLabel::kInstance; 21 | } 22 | 23 | bool LabelHandlerBase::isUnknownClass(int segmentation_id) const { 24 | return labels_.at(segmentation_id)->label == PanopticLabel::kUnknown; 25 | } 26 | 27 | bool LabelHandlerBase::isSpaceClass(int segmentation_id) const { 28 | return labels_.at(segmentation_id)->label == PanopticLabel::kFreeSpace; 29 | } 30 | 31 | PanopticLabel LabelHandlerBase::getPanopticLabel(int segmentation_id) const { 32 | return labels_.at(segmentation_id)->label; 33 | } 34 | 35 | const voxblox::Color& LabelHandlerBase::getColor(int segmentation_id) const { 36 | return labels_.at(segmentation_id)->color; 37 | } 38 | 39 | const std::string& LabelHandlerBase::getName(int segmentation_id) const { 40 | return labels_.at(segmentation_id)->name; 41 | } 42 | 43 | const LabelEntry& LabelHandlerBase::getLabelEntry(int segmentation_id) const { 44 | return *labels_.at(segmentation_id); 45 | } 46 | 47 | bool LabelHandlerBase::getLabelEntryIfExists(int segmentation_id, 48 | LabelEntry* label_entry) const { 49 | auto it = labels_.find(segmentation_id); 50 | if (it != labels_.end()) { 51 | CHECK_NOTNULL(label_entry); 52 | *label_entry = *it->second; 53 | return true; 54 | } 55 | return false; 56 | } 57 | 58 | size_t LabelHandlerBase::numberOfLabels() const { return labels_.size(); } 59 | 60 | } // namespace panoptic_mapping 61 | -------------------------------------------------------------------------------- /panoptic_mapping/src/labels/null_label_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/labels/null_label_handler.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | config_utilities::Factory::RegistrationRos 6 | NullLabelHandler::registration_("null"); 7 | 8 | void NullLabelHandler::Config::setupParamsAndPrinting() { 9 | printText("The null label handler does not read any labels."); 10 | } 11 | 12 | NullLabelHandler::NullLabelHandler(const Config& config) 13 | : config_(config.checkValid()) { 14 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 15 | } 16 | 17 | } // namespace panoptic_mapping 18 | -------------------------------------------------------------------------------- /panoptic_mapping/src/labels/range_label_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/labels/range_label_handler.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace panoptic_mapping { 10 | 11 | config_utilities::Factory::RegistrationRos 12 | RangeLabelHandler::registration_("range"); 13 | 14 | void RangeLabelHandler::Config::setupParamsAndPrinting() { 15 | setupParam("verbosity", &verbosity); 16 | setupParam("num_labels", &num_labels); 17 | } 18 | 19 | void RangeLabelHandler::Config::checkParams() const { 20 | checkParamGT(num_labels, 0, "num_labels"); 21 | } 22 | 23 | RangeLabelHandler::RangeLabelHandler(const Config& config, bool print_config) 24 | : config_(config.checkValid()) { 25 | LOG_IF(INFO, config_.verbosity >= 1 && print_config) << "\n" 26 | << config_.toString(); 27 | // Setup the labels from range. 28 | initialiseLabels(); 29 | } 30 | 31 | void RangeLabelHandler::initialiseLabels() { 32 | for (int i; i < config_.num_labels; i++) { 33 | LabelEntry label; 34 | label.segmentation_id = i; 35 | label.class_id = i; 36 | label.label = PanopticLabel::kBackground; 37 | label.color = voxblox::rainbowColorMap(((float)i) / config_.num_labels); 38 | labels_[i] = std::make_unique(label); 39 | } 40 | } 41 | 42 | } // namespace panoptic_mapping 43 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map/classification/uncertainty.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map/classification/uncertainty.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace panoptic_mapping { 8 | 9 | ClassVoxelType UncertaintyVoxel::getVoxelType() const { 10 | return ClassVoxelType::kUncertainty; 11 | } 12 | 13 | bool UncertaintyVoxel::mergeVoxel(const ClassVoxel& other) { 14 | // Check type compatibility. 15 | auto voxel = dynamic_cast(&other); 16 | if (!voxel) { 17 | LOG(WARNING) 18 | << "Can not merge voxels that are not of same type (UncertaintyVoxel)."; 19 | return false; 20 | } 21 | if (is_ground_truth) { 22 | return true; 23 | } 24 | if (voxel->is_ground_truth) { 25 | counts = voxel->counts; 26 | current_index = voxel->current_index; 27 | current_count = voxel->current_count; 28 | uncertainty = voxel->uncertainty; 29 | is_ground_truth = true; 30 | return true; 31 | } 32 | FixedCountVoxel::mergeVoxel(other); 33 | // We assume that uncertainties can be averaged here. 34 | uncertainty = (uncertainty + voxel->uncertainty) / 2.f; 35 | return true; 36 | } 37 | 38 | std::vector UncertaintyVoxel::serializeVoxelToInt() const { 39 | // Serialize the count data. 40 | std::vector result = FixedCountVoxel::serializeVoxelToInt(); 41 | 42 | // Append the added data. 43 | result.push_back(static_cast(is_ground_truth)); 44 | result.push_back(int32FromX32(uncertainty)); 45 | return result; 46 | } 47 | 48 | bool UncertaintyVoxel::deseriliazeVoxelFromInt( 49 | const std::vector& data, size_t* data_index) { 50 | // De-serialize count data. 51 | FixedCountVoxel::deseriliazeVoxelFromInt(data, data_index); 52 | if (*data_index + 1 >= data.size()) { 53 | LOG(WARNING) 54 | << "Can not deserialize voxel from integer data: Out of range (index: " 55 | << *data_index << ", data: " << data.size() << ")"; 56 | return false; 57 | } 58 | 59 | // De-serialize the added data. 60 | is_ground_truth = data[*data_index]; 61 | uncertainty = x32FromInt32(data[*data_index + 1]); 62 | *data_index += 2; 63 | return true; 64 | } 65 | 66 | config_utilities::Factory::RegistrationRos 68 | UncertaintyLayer::registration_("uncertainty"); 69 | 70 | UncertaintyLayer::UncertaintyLayer(const Config& config, const float voxel_size, 71 | const int voxels_per_side) 72 | : config_(config.checkValid()), 73 | ClassLayerImpl(voxel_size, voxels_per_side) {} 74 | 75 | ClassVoxelType UncertaintyLayer::getVoxelType() const { 76 | return ClassVoxelType::kUncertainty; 77 | } 78 | 79 | std::unique_ptr UncertaintyLayer::clone() const { 80 | return std::make_unique(*this); 81 | } 82 | 83 | std::unique_ptr UncertaintyLayer::loadFromStream( 84 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 85 | uint64_t* /* tmp_byte_offset_ptr */) { 86 | // Nothing special needed to configure for binary counts. 87 | return std::make_unique(UncertaintyLayer::Config(), 88 | submap_proto.voxel_size(), 89 | submap_proto.voxels_per_side()); 90 | } 91 | 92 | } // namespace panoptic_mapping 93 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map/instance_id.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map/instance_id.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | // By default create a new unique id. 6 | InstanceID::InstanceID(InstanceIDManager* manager) 7 | : manager_(manager), id_(manager->requestID()) {} 8 | 9 | InstanceID::~InstanceID() { manager_->releaseID(id_); } 10 | 11 | InstanceID::InstanceID(int id, InstanceIDManager* manager) 12 | : manager_(manager_), id_(id) { 13 | manager_->registerID(id); 14 | } 15 | 16 | InstanceID::InstanceID(const InstanceID& other) : manager_(other.manager_) { 17 | manager_->releaseID(id_); 18 | id_ = other.id_; 19 | manager_->registerID(id_); 20 | } 21 | 22 | InstanceID& InstanceID::operator=(const int& id) { 23 | manager_->releaseID(id_); 24 | id_ = id; 25 | manager_->registerID(id_); 26 | return *this; 27 | } 28 | 29 | InstanceIDManager::InstanceIDManager() : current_id_(0) {} 30 | 31 | void InstanceIDManager::increment(int id) { 32 | auto it = used_ids_.find(id); 33 | if (it == used_ids_.end()) { 34 | used_ids_[id] = 1; 35 | } else { 36 | it->second++; 37 | } 38 | } 39 | 40 | bool InstanceIDManager::decrement(int id) { 41 | // Return true if the id was deleted (decrement to 0) 42 | auto it = used_ids_.find(id); 43 | if (it == used_ids_.end()) { 44 | return true; 45 | } 46 | it->second--; 47 | if (it->second <= 0) { 48 | used_ids_.erase(it); 49 | return true; 50 | } 51 | return false; 52 | } 53 | 54 | int InstanceIDManager::requestID() { 55 | // Find the next higher unique ID. 56 | while (used_ids_.find(current_id_) != used_ids_.end()) { 57 | current_id_++; 58 | } 59 | increment(current_id_); 60 | return current_id_; 61 | } 62 | 63 | void InstanceIDManager::registerID(int id) { increment(id); } 64 | 65 | void InstanceIDManager::releaseID(int id) { 66 | const bool id_is_free = decrement(id); 67 | if (id_is_free && id < current_id_ && kAllowIDReuse) { 68 | current_id_ = id; 69 | } 70 | } 71 | 72 | } // namespace panoptic_mapping 73 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map/scores/average.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map/scores/average.h" 2 | 3 | #include 4 | 5 | namespace panoptic_mapping { 6 | 7 | ScoreVoxelType AverageScoreVoxel::getVoxelType() const { 8 | return ScoreVoxelType::kAverage; 9 | } 10 | 11 | bool AverageScoreVoxel::isObserverd() const { return accumulated_weight != 0; } 12 | 13 | float AverageScoreVoxel::getScore() const { 14 | if (!isObserverd()) { 15 | LOG(WARNING) << "Getting score of unobserved voxel"; 16 | } 17 | return average_score; 18 | } 19 | 20 | void AverageScoreVoxel::addMeasurement(const float score, const float weight) { 21 | const float new_weight = accumulated_weight + weight; 22 | average_score = weight * score / new_weight + 23 | accumulated_weight * average_score / new_weight; 24 | accumulated_weight = new_weight; 25 | } 26 | 27 | bool AverageScoreVoxel::mergeVoxel(const ScoreVoxel& other) { 28 | // Check type compatibility. 29 | auto voxel = dynamic_cast(&other); 30 | if (!voxel) { 31 | LOG(WARNING) << "Can not merge voxels that are not of same type " 32 | "(AverageScoreVoxel)."; 33 | return false; 34 | } 35 | addMeasurement(voxel->average_score, voxel->accumulated_weight); 36 | return true; 37 | } 38 | 39 | std::vector AverageScoreVoxel::serializeVoxelToInt() const { 40 | return {int32FromX32(accumulated_weight), 41 | int32FromX32(average_score)}; 42 | } 43 | 44 | bool AverageScoreVoxel::deseriliazeVoxelFromInt( 45 | const std::vector& data, size_t* data_index) { 46 | if (*data_index >= data.size()) { 47 | LOG(WARNING) 48 | << "Can not deserialize voxel from integer data: Out of range (index: " 49 | << *data_index << ", data: " << data.size() << ")"; 50 | return false; 51 | } 52 | accumulated_weight = x32FromInt32(data[*data_index]); 53 | average_score = x32FromInt32(data[*data_index + 1]); 54 | *data_index += 2; 55 | return true; 56 | } 57 | 58 | config_utilities::Factory::RegistrationRos 60 | AverageScoreLayer::registration_("average"); 61 | 62 | AverageScoreLayer::AverageScoreLayer(const Config& config, 63 | const float voxel_size, 64 | const int voxels_per_side) 65 | : config_(config.checkValid()), 66 | ScoreLayerImpl(voxel_size, voxels_per_side) {} 67 | 68 | ScoreVoxelType AverageScoreLayer::getVoxelType() const { 69 | return ScoreVoxelType::kAverage; 70 | } 71 | 72 | std::unique_ptr AverageScoreLayer::clone() const { 73 | return std::make_unique(*this); 74 | } 75 | 76 | std::unique_ptr AverageScoreLayer::loadFromStream( 77 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 78 | uint64_t* /* tmp_byte_offset_ptr */) { 79 | // Nothing special needed to configure for binary counts. 80 | return std::make_unique(AverageScoreLayer::Config(), 81 | submap_proto.voxel_size(), 82 | submap_proto.voxels_per_side()); 83 | } 84 | 85 | } // namespace panoptic_mapping 86 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map/scores/latest.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map/scores/latest.h" 2 | 3 | #include 4 | 5 | namespace panoptic_mapping { 6 | 7 | ScoreVoxelType LatestScoreVoxel::getVoxelType() const { 8 | return ScoreVoxelType::kLatest; 9 | } 10 | 11 | bool LatestScoreVoxel::isObserverd() const { return latest_score != NAN; } 12 | float LatestScoreVoxel::getScore() const { return latest_score; } 13 | void LatestScoreVoxel::addMeasurement(const float score, const float weight) { 14 | latest_score = score; 15 | } 16 | 17 | bool LatestScoreVoxel::mergeVoxel(const ScoreVoxel& other) { 18 | // Check type compatibility. 19 | auto voxel = dynamic_cast(&other); 20 | if (!voxel) { 21 | LOG(WARNING) 22 | << "Can not merge voxels that are not of same type (LatestScoreVoxel)."; 23 | return false; 24 | } 25 | // both not observed -> no changes 26 | if (!isObserverd() && !voxel->isObserverd()) { 27 | return true; 28 | } 29 | // both are observed -> we default to this voxel 30 | if (isObserverd() && voxel->isObserverd()) { 31 | return true; 32 | } 33 | // only one is observed, take its value 34 | if (voxel->isObserverd()) { 35 | addMeasurement(voxel->latest_score); 36 | } 37 | return true; 38 | } 39 | 40 | std::vector LatestScoreVoxel::serializeVoxelToInt() const { 41 | return {int32FromX32(latest_score)}; 42 | } 43 | 44 | bool LatestScoreVoxel::deseriliazeVoxelFromInt( 45 | const std::vector& data, size_t* data_index) { 46 | if (*data_index >= data.size()) { 47 | LOG(WARNING) 48 | << "Can not deserialize voxel from integer data: Out of range (index: " 49 | << *data_index << ", data: " << data.size() << ")"; 50 | return false; 51 | } 52 | latest_score = x32FromInt32(data[*data_index]); 53 | *data_index += 1; 54 | return true; 55 | } 56 | 57 | config_utilities::Factory::RegistrationRos 59 | LatestScoreLayer::registration_("latest"); 60 | 61 | LatestScoreLayer::LatestScoreLayer(const Config& config, const float voxel_size, 62 | const int voxels_per_side) 63 | : config_(config.checkValid()), 64 | ScoreLayerImpl(voxel_size, voxels_per_side) {} 65 | 66 | ScoreVoxelType LatestScoreLayer::getVoxelType() const { 67 | return ScoreVoxelType::kLatest; 68 | } 69 | 70 | std::unique_ptr LatestScoreLayer::clone() const { 71 | return std::make_unique(*this); 72 | } 73 | 74 | std::unique_ptr LatestScoreLayer::loadFromStream( 75 | const SubmapProto& submap_proto, std::istream* /* proto_file_ptr */, 76 | uint64_t* /* tmp_byte_offset_ptr */) { 77 | // Nothing special needed to configure for binary counts. 78 | return std::make_unique(LatestScoreLayer::Config(), 79 | submap_proto.voxel_size(), 80 | submap_proto.voxels_per_side()); 81 | } 82 | 83 | } // namespace panoptic_mapping 84 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map/submap_bounding_volume.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map/submap_bounding_volume.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/map/submap.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | SubmapBoundingVolume::SubmapBoundingVolume(const Submap& submap) 12 | : submap_(&submap), 13 | center_(0.f, 0.f, 0.f), 14 | radius_(0.f), 15 | num_previous_blocks_(0) {} 16 | 17 | void SubmapBoundingVolume::update() { 18 | // A conservative approximation that computes the centroid from the 19 | // grid-aligned bounding box and then shrinks a sphere on it. This is 20 | // generally a significant over-estimation of the sphere, could use an exact 21 | // algorithm here if required (e.g. https://github.com/hbf/miniball or 22 | // https://people.inf.ethz.ch/gaertner/subdir/software/miniball.html). 23 | 24 | // Prevent redundant updates. 25 | if (submap_->getTsdfLayer().getNumberOfAllocatedBlocks() == 26 | num_previous_blocks_) { 27 | return; 28 | } else { 29 | num_previous_blocks_ = submap_->getTsdfLayer().getNumberOfAllocatedBlocks(); 30 | } 31 | 32 | // Setup. 33 | voxblox::BlockIndexList block_indices; 34 | submap_->getTsdfLayer().getAllAllocatedBlocks(&block_indices); 35 | if (block_indices.empty()) { 36 | radius_ = 0.f; 37 | center_ = Point(); 38 | return; 39 | } 40 | std::vector block_centers; 41 | block_centers.reserve(block_indices.size()); 42 | const FloatingPoint grid_size = submap_->getTsdfLayer().block_size(); 43 | Point min_dimension = 44 | voxblox::getCenterPointFromGridIndex(block_indices.front(), grid_size); 45 | Point max_dimension = min_dimension; 46 | 47 | // Get centers and grid aligned bounding box. 48 | for (size_t i = 1; i < block_indices.size(); ++i) { 49 | block_centers.emplace_back( 50 | voxblox::getCenterPointFromGridIndex(block_indices[i], grid_size)); 51 | min_dimension = min_dimension.cwiseMin(block_centers.back()); 52 | max_dimension = max_dimension.cwiseMax(block_centers.back()); 53 | } 54 | 55 | // Compute bounding sphere center and radius. 56 | center_ = (min_dimension + max_dimension) / 2.f; 57 | radius_ = 0.f; 58 | for (const Point& center : block_centers) { 59 | radius_ = std::max(radius_, (center - center_).norm()); 60 | } 61 | radius_ += std::sqrt(3.f) * grid_size / 2.f; // outermost voxel. 62 | } 63 | 64 | bool SubmapBoundingVolume::contains_S(const Point& point_S) const { 65 | // Point is expected in submap frame. 66 | return (center_ - point_S).norm() <= radius_; 67 | } 68 | 69 | bool SubmapBoundingVolume::contains_M(const Point& point_M) const { 70 | return contains_S(submap_->getT_S_M() * point_M); 71 | } 72 | 73 | bool SubmapBoundingVolume::intersects(const SubmapBoundingVolume& other) const { 74 | const Transformation T_S_other = 75 | submap_->getT_S_M() * other.submap_->getT_M_S(); 76 | return (center_ - T_S_other * other.center_).norm() <= 77 | radius_ + other.radius_; 78 | } 79 | 80 | bool SubmapBoundingVolume::isInsidePlane_S(const Point& normal_S) const { 81 | // The normal is expected in submap frame. 82 | return center_.dot(normal_S) >= -radius_; 83 | } 84 | 85 | bool SubmapBoundingVolume::isInsidePlane_M(const Point& normal_S) const { 86 | return isInsidePlane_S(submap_->getT_S_M() * normal_S); 87 | } 88 | 89 | } // namespace panoptic_mapping 90 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map/submap_id.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map/submap_id.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | SubmapID::SubmapID(SubmapIDManager* manager) 6 | : manager_(manager), id_(manager->requestID()) {} 7 | 8 | SubmapID::SubmapID(int id, SubmapIDManager* manager) 9 | : manager_(manager), id_(id) {} 10 | 11 | SubmapID::~SubmapID() { manager_->releaseID(id_); } 12 | 13 | SubmapIDManager::SubmapIDManager() : current_id_(0) {} 14 | 15 | int SubmapIDManager::requestID() { 16 | if (vacant_ids_.empty()) { 17 | return current_id_++; 18 | } else { 19 | int id = vacant_ids_.front(); 20 | vacant_ids_.pop(); 21 | return id; 22 | } 23 | } 24 | 25 | void SubmapIDManager::releaseID(int id) { 26 | if (kAllowIDReuse) { 27 | vacant_ids_.push(id); 28 | } 29 | } 30 | 31 | } // namespace panoptic_mapping 32 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map_management/activity_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map_management/activity_manager.h" 2 | 3 | #include 4 | 5 | namespace panoptic_mapping { 6 | 7 | void ActivityManager::Config::checkParams() const { 8 | // checkParamNE(error_threshold, 0.f, "error_threshold"); 9 | } 10 | 11 | void ActivityManager::Config::setupParamsAndPrinting() { 12 | setupParam("verbosity", &verbosity); 13 | setupParam("required_reobservations", &required_reobservations); 14 | setupParam("deactivate_after_missed_detections", 15 | &deactivate_after_missed_detections); 16 | } 17 | 18 | ActivityManager::ActivityManager(const Config& config) 19 | : config_(config.checkValid()) { 20 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 21 | } 22 | 23 | void ActivityManager::processSubmaps(SubmapCollection* submaps) { 24 | CHECK_NOTNULL(submaps); 25 | std::unordered_set submaps_to_delete; 26 | for (Submap& submap : *submaps) { 27 | // Parse only active object maps. 28 | // NOTE(schmluk): Could be extended to free space for global consistency. 29 | if (!submap.isActive() || submap.getLabel() == PanopticLabel::kFreeSpace) { 30 | continue; 31 | } 32 | 33 | // Check for re-detections of new submaps. 34 | if (!checkRequiredRedetection(&submap)) { 35 | submaps_to_delete.insert(submap.getID()); 36 | continue; 37 | } 38 | 39 | // Check tracking for active submaps. 40 | checkMissedDetections(&submap); 41 | } 42 | 43 | // Remove requested submaps. 44 | for (const int id : submaps_to_delete) { 45 | submaps->removeSubmap(id); 46 | } 47 | 48 | // Reset. 49 | for (Submap& submap : *submaps) { 50 | submap.setWasTracked(false); 51 | } 52 | } 53 | 54 | bool ActivityManager::checkRequiredRedetection(Submap* submap) { 55 | // Check the submap was re-detected in X consecutive frames after allocation. 56 | if (config_.required_reobservations <= 0) { 57 | return true; 58 | } 59 | const int submap_id = submap->getID(); 60 | auto it = submap_redetection_counts_.find(submap_id); 61 | if (it == submap_redetection_counts_.end()) { 62 | // This is a new submap. 63 | submap_redetection_counts_[submap_id] = config_.required_reobservations; 64 | return true; 65 | } 66 | if (it->second <= 0) { 67 | // This submap already passed the re-detection test. 68 | return true; 69 | } 70 | if (submap->wasTracked()) { 71 | // Was re-observed, decrease remaining required re-observations. 72 | it->second--; 73 | return true; 74 | } 75 | // Not detected, remove the submap. 76 | return false; 77 | } 78 | 79 | void ActivityManager::checkMissedDetections(Submap* submap) { 80 | // Check whether a submap was not detected for X consecutive frames. 81 | if (config_.deactivate_after_missed_detections <= 0) { 82 | return; 83 | } 84 | if (submap->wasTracked()) { 85 | // Was tracked so reset the counter. 86 | submap_missed_detection_counts_.erase(submap->getID()); 87 | } else { 88 | auto it = submap_missed_detection_counts_.find(submap->getID()); 89 | if (it == submap_missed_detection_counts_.end()) { 90 | // First missed detection, add to counter. 91 | it = submap_missed_detection_counts_.insert( 92 | submap_missed_detection_counts_.end(), 93 | std::pair(submap->getID(), 94 | config_.deactivate_after_missed_detections)); 95 | } 96 | it->second--; 97 | if (it->second <= 0) { 98 | submap->finishActivePeriod(); 99 | } 100 | } 101 | } 102 | 103 | } // namespace panoptic_mapping 104 | -------------------------------------------------------------------------------- /panoptic_mapping/src/map_management/null_map_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/map_management/null_map_manager.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | config_utilities::Factory::RegistrationRos 6 | NullMapManager::registration_("null"); 7 | 8 | void NullMapManager::Config::setupParamsAndPrinting() { 9 | printText("The null map manager does not execute any actions."); 10 | } 11 | 12 | } // namespace panoptic_mapping 13 | -------------------------------------------------------------------------------- /panoptic_mapping/src/submap_allocation/monolithic_freespace_allocator.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/submap_allocation/monolithic_freespace_allocator.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | config_utilities::Factory::RegistrationRos 7 | MonolithicFreespaceAllocator::registration_("monolithic"); 8 | 9 | void MonolithicFreespaceAllocator::Config::checkParams() const { 10 | checkParamConfig(submap); 11 | } 12 | 13 | void MonolithicFreespaceAllocator::Config::setupParamsAndPrinting() { 14 | setupParam("verbosity", &verbosity); 15 | setupParam("submap", &submap); 16 | } 17 | 18 | MonolithicFreespaceAllocator::MonolithicFreespaceAllocator(const Config& config, 19 | bool print_config) 20 | : config_(config.checkValid()) { 21 | LOG_IF(INFO, config_.verbosity >= 1 && print_config) << "\n" 22 | << config_.toString(); 23 | } 24 | 25 | Submap* MonolithicFreespaceAllocator::allocateSubmap(SubmapCollection* submaps, 26 | InputData* /*input*/) { 27 | if (submaps->getActiveFreeSpaceSubmapID() >= 0) { 28 | // Allocate one free space submap in the beginning. 29 | return nullptr; 30 | } 31 | 32 | // Otherwise create a new freespace submap. 33 | Submap* space_submap = submaps->createSubmap(config_.submap); 34 | space_submap->setLabel(PanopticLabel::kFreeSpace); 35 | space_submap->setInstanceID(-1); // Will never appear in a seg image. 36 | space_submap->setName("FreeSpace"); 37 | submaps->setActiveFreeSpaceSubmapID(space_submap->getID()); 38 | return space_submap; 39 | } 40 | 41 | } // namespace panoptic_mapping 42 | -------------------------------------------------------------------------------- /panoptic_mapping/src/submap_allocation/null_submap_allocator.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/submap_allocation/null_submap_allocator.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | config_utilities::Factory::RegistrationRos 7 | NullSubmapAllocator::registration_("null"); 8 | 9 | void NullSubmapAllocator::Config::setupParamsAndPrinting() { 10 | printText("The null submap allocator does not allocate any submaps."); 11 | } 12 | 13 | NullSubmapAllocator::NullSubmapAllocator(const Config& config) 14 | : config_(config.checkValid()) { 15 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 16 | } 17 | 18 | config_utilities::Factory::RegistrationRos 20 | NullFreespaceAllocator::registration_("null"); 21 | 22 | void NullFreespaceAllocator::Config::setupParamsAndPrinting() { 23 | printText("The null free space allocator does not allocate any submaps."); 24 | } 25 | 26 | NullFreespaceAllocator::NullFreespaceAllocator(const Config& config) 27 | : config_(config.checkValid()) { 28 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 29 | } 30 | 31 | } // namespace panoptic_mapping 32 | -------------------------------------------------------------------------------- /panoptic_mapping/src/submap_allocation/semantic_submap_allocator.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/submap_allocation/semantic_submap_allocator.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | config_utilities::Factory::RegistrationRos 7 | SemanticSubmapAllocator::registration_("semantic"); 8 | 9 | void SemanticSubmapAllocator::Config::checkParams() const { 10 | checkParamConfig(submap); 11 | checkParamGT(small_instance_voxel_size, 0.f, "small_instance_voxel_size"); 12 | checkParamGT(medium_instance_voxel_size, 0.f, "medium_instance_voxel_size"); 13 | checkParamGT(large_instance_voxel_size, 0.f, "large_instance_voxel_size"); 14 | checkParamGT(background_voxel_size, 0.f, "background_voxel_size"); 15 | checkParamGT(unknown_voxel_size, 0.f, "unknown_voxel_size"); 16 | checkParamNE(truncation_distance, 0.f, "truncation_distance"); 17 | } 18 | 19 | void SemanticSubmapAllocator::Config::setupParamsAndPrinting() { 20 | setupParam("verbosity", &verbosity); 21 | setupParam("submap", &submap); 22 | setupParam("small_instance_voxel_size", &small_instance_voxel_size); 23 | setupParam("medium_instance_voxel_size", &medium_instance_voxel_size); 24 | setupParam("large_instance_voxel_size", &large_instance_voxel_size); 25 | setupParam("background_voxel_size", &background_voxel_size); 26 | setupParam("unknown_voxel_size", &unknown_voxel_size); 27 | setupParam("truncation_distance", &truncation_distance); 28 | } 29 | 30 | SemanticSubmapAllocator::SemanticSubmapAllocator(const Config& config, 31 | bool print_config) 32 | : config_(config.checkValid()) { 33 | LOG_IF(INFO, config_.verbosity >= 1 && print_config) << "\n" 34 | << config_.toString(); 35 | } 36 | 37 | Submap* SemanticSubmapAllocator::allocateSubmap(SubmapCollection* submaps, 38 | InputData* /* input */, 39 | int input_id, 40 | const LabelEntry& label) { 41 | Submap::Config config = config_.submap; 42 | 43 | // Setup the voxel size. 44 | switch (label.label) { 45 | case PanopticLabel::kInstance: { 46 | if (label.size == "L") { 47 | config.voxel_size = config_.large_instance_voxel_size; 48 | } else if (label.size == "S") { 49 | config.voxel_size = config_.small_instance_voxel_size; 50 | } else { 51 | config.voxel_size = config_.medium_instance_voxel_size; 52 | } 53 | break; 54 | } 55 | case PanopticLabel::kBackground: { 56 | config.voxel_size = config_.background_voxel_size; 57 | break; 58 | } 59 | default: { 60 | config.voxel_size = config_.unknown_voxel_size; 61 | break; 62 | } 63 | } 64 | 65 | // Set the truncation distance. 66 | config.truncation_distance = config_.truncation_distance; 67 | if (config.truncation_distance < 0.f) { 68 | config.truncation_distance *= -config.voxel_size; 69 | } 70 | 71 | // Create the submap. 72 | Submap* new_submap = submaps->createSubmap(config); 73 | new_submap->setClassID(label.class_id); 74 | new_submap->setLabel(label.label); 75 | new_submap->setName(label.name); 76 | return new_submap; 77 | } 78 | 79 | } // namespace panoptic_mapping 80 | -------------------------------------------------------------------------------- /panoptic_mapping/src/tools/evaluation_data_writer.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/tools/evaluation_data_writer.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace panoptic_mapping { 13 | 14 | config_utilities::Factory::RegistrationRos 15 | EvaluationDataWriter::registration_("evaluation"); 16 | 17 | void EvaluationDataWriter::Config::checkParams() const {} 18 | 19 | void EvaluationDataWriter::Config::setupParamsAndPrinting() { 20 | setupParam("verbosity", &verbosity); 21 | setupParam("log_data_writer_config", &log_data_writer_config); 22 | setupParam("store_map_every_n_frames", &store_map_every_n_frames); 23 | } 24 | 25 | EvaluationDataWriter::EvaluationDataWriter(const Config& config) 26 | : config_(config.checkValid()), 27 | LogDataWriter(config.log_data_writer_config, false) { 28 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 29 | } 30 | 31 | void EvaluationDataWriter::setupLogFile() { 32 | const bool create_directory = config_.store_map_every_n_frames > 0; 33 | if (create_directory) { 34 | // Setup a single directory named with the timestamp. 35 | auto t = std::time(nullptr); 36 | auto tm = *std::localtime(&t); 37 | std::stringstream timestamp; 38 | timestamp << std::put_time(&tm, "%Y_%m_%d-%H_%M_%S"); 39 | output_path_ = 40 | config_.log_data_writer_config.output_directory + "/" + timestamp.str(); 41 | if (!std::experimental::filesystem::create_directory(output_path_)) { 42 | LOG(ERROR) << "Could not create output directory '" << output_path_ 43 | << "'."; 44 | } 45 | outfile_name_ = output_path_ + "/" + 46 | (config_.log_data_writer_config.file_name.empty() 47 | ? "log" 48 | : config_.log_data_writer_config.file_name) + 49 | ".csv"; 50 | // Setup output file. 51 | outfile_.open(outfile_name_); 52 | if (!outfile_.is_open()) { 53 | LOG(ERROR) << "Could not open data output file '" << outfile_name_ 54 | << "'."; 55 | } 56 | } else { 57 | // Setup the standard log file. 58 | LogDataWriter::setupLogFile(); 59 | } 60 | } 61 | 62 | void EvaluationDataWriter::setupEvaluations() { 63 | LogDataWriter::setupEvaluations(); 64 | // Additional evaluations of the evaluation writer. 65 | if (config_.store_map_every_n_frames > 0) { 66 | writeEntry("SavedMapName [-]"); 67 | evaluations_.emplace_back([this](const SubmapCollection& submaps) { 68 | this->storeSubmaps(submaps); 69 | }); 70 | } 71 | } 72 | 73 | void EvaluationDataWriter::storeSubmaps(const SubmapCollection& submaps) { 74 | store_submap_frame_++; 75 | if (store_submap_frame_ < config_.store_map_every_n_frames) { 76 | writeEntry(""); 77 | return; 78 | } 79 | store_submap_frame_ = 0; 80 | std::stringstream ss; 81 | ss << std::setw(6) << std::setfill('0') << store_submap_counter_; 82 | store_submap_counter_++; 83 | submaps.saveToFile(output_path_ + "/" + ss.str()); 84 | writeEntry(ss.str()); 85 | } 86 | 87 | } // namespace panoptic_mapping 88 | -------------------------------------------------------------------------------- /panoptic_mapping/src/tools/null_data_writer.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/tools/null_data_writer.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | config_utilities::Factory::RegistrationRos 6 | NullDataWriter::registration_("null"); 7 | 8 | void NullDataWriter::Config::setupParamsAndPrinting() { 9 | printText("The null data writer does not write any data."); 10 | } 11 | 12 | } // namespace panoptic_mapping 13 | -------------------------------------------------------------------------------- /panoptic_mapping/src/tracking/detectron_id_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping/tracking/detectron_id_tracker.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "panoptic_mapping/common/index_getter.h" 8 | 9 | namespace panoptic_mapping { 10 | 11 | config_utilities::Factory::RegistrationRos> 13 | DetectronIDTracker::registration_("detectron"); 14 | 15 | void DetectronIDTracker::Config::checkParams() const { 16 | checkParamConfig(projective_id_tracker); 17 | } 18 | 19 | void DetectronIDTracker::Config::setupParamsAndPrinting() { 20 | setupParam("verbosity", &verbosity); 21 | setupParam("projective_id_tracker", &projective_id_tracker); 22 | } 23 | 24 | DetectronIDTracker::DetectronIDTracker(const Config& config, 25 | std::shared_ptr globals) 26 | : config_(config.checkValid()), 27 | ProjectiveIDTracker(config.projective_id_tracker, std::move(globals), 28 | false) { 29 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 30 | addRequiredInput(InputData::InputType::kDetectronLabels); 31 | } 32 | 33 | void DetectronIDTracker::processInput(SubmapCollection* submaps, 34 | InputData* input) { 35 | CHECK_NOTNULL(submaps); 36 | CHECK_NOTNULL(input); 37 | CHECK(inputIsValid(*input)); 38 | // Cache the input labels for submap allocation. 39 | labels_ = &(input->detectronLabels()); 40 | 41 | // Track the predicted ids. 42 | ProjectiveIDTracker::processInput(submaps, input); 43 | } 44 | 45 | Submap* DetectronIDTracker::allocateSubmap(int input_id, 46 | SubmapCollection* submaps, 47 | InputData* input) { 48 | if (input_id == 0) { 49 | // The id 0 is used for no-predictions in detectron. 50 | return nullptr; 51 | } 52 | 53 | // Check whether the instance code is known. 54 | auto it = labels_->find(input_id); 55 | if (it == labels_->end()) { 56 | return nullptr; 57 | } 58 | 59 | // Parse detectron label. 60 | LabelEntry label; 61 | const int class_id = it->second.category_id; 62 | if (globals_->labelHandler()->segmentationIdExists(class_id)) { 63 | label = globals_->labelHandler()->getLabelEntry(input_id); 64 | } 65 | if (it->second.is_thing) { 66 | label.label = PanopticLabel::kInstance; 67 | } else { 68 | label.label = PanopticLabel::kBackground; 69 | } 70 | 71 | // Allocate new submap. 72 | Submap* new_submap = 73 | submap_allocator_->allocateSubmap(submaps, input, input_id, label); 74 | new_submap->setClassID(class_id); 75 | if (globals_->labelHandler()->segmentationIdExists(class_id)) { 76 | new_submap->setName(globals_->labelHandler()->getName(class_id)); 77 | } 78 | return new_submap; 79 | } 80 | 81 | bool DetectronIDTracker::classesMatch(int input_id, int submap_class_id) { 82 | if (input_id == 0) { 83 | // The id 0 is used to denote no-predictions by detectron. 84 | return false; 85 | } 86 | auto it = labels_->find(input_id); 87 | if (it == labels_->end()) { 88 | // No known input label. 89 | return false; 90 | } 91 | return it->second.category_id == submap_class_id; 92 | } 93 | 94 | } // namespace panoptic_mapping 95 | -------------------------------------------------------------------------------- /panoptic_mapping_https.rosinstall: -------------------------------------------------------------------------------- 1 | # General Dependencies 2 | - git: 3 | local-name: catkin_simple 4 | uri: https://github.com/catkin/catkin_simple.git 5 | - git: 6 | local-name: eigen_catkin 7 | uri: https://github.com/ethz-asl/eigen_catkin.git 8 | - git: 9 | local-name: eigen_checks 10 | uri: https://github.com/ethz-asl/eigen_checks.git 11 | - git: 12 | local-name: gflags_catkin 13 | uri: https://github.com/ethz-asl/gflags_catkin.git 14 | - git: 15 | local-name: glog_catkin 16 | uri: https://github.com/ethz-asl/glog_catkin.git 17 | - git: 18 | local-name: minkindr 19 | uri: https://github.com/ethz-asl/minkindr.git 20 | - git: 21 | local-name: minkindr_ros 22 | uri: https://github.com/ethz-asl/minkindr_ros.git 23 | - git: 24 | local-name: protobuf_catkin 25 | uri: https://github.com/ethz-asl/protobuf_catkin.git 26 | - git: 27 | local-name: opencv3_catkin 28 | uri: https://github.com/ethz-asl/opencv3_catkin.git 29 | - git: 30 | local-name: numpy_eigen 31 | uri: https://github.com/ethz-asl/numpy_eigen.git 32 | - git: 33 | local-name: catkin_boost_python_buildtool 34 | uri: https://github.com/ethz-asl/catkin_boost_python_buildtool.git 35 | - git: 36 | local-name: catkin_grpc 37 | uri: https://github.com/CogRob/catkin_grpc.git 38 | 39 | # Package Dependencies. Notice that voxblox requires a special branch. 40 | - git: 41 | local-name: voxblox 42 | uri: https://github.com/ethz-asl/voxblox.git 43 | version: 'devel/panoptic_mapping' 44 | - git: 45 | local-name: cblox 46 | uri: https://github.com/ethz-asl/cblox.git 47 | 48 | -------------------------------------------------------------------------------- /panoptic_mapping_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panoptic_mapping_msgs) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(catkin_simple REQUIRED) 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | catkin_package() 8 | 9 | ########### 10 | # Export # 11 | ########### 12 | 13 | cs_install() 14 | cs_export() 15 | -------------------------------------------------------------------------------- /panoptic_mapping_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Panoptic Mapping Msgs 2 | ROS message and service definitions of **panoptic_mapping**. -------------------------------------------------------------------------------- /panoptic_mapping_msgs/msg/DetectronLabel.msg: -------------------------------------------------------------------------------- 1 | int32 id 2 | bool is_thing 3 | float32 score 4 | int32 category_id 5 | int32 instance_id 6 | -------------------------------------------------------------------------------- /panoptic_mapping_msgs/msg/DetectronLabels.msg: -------------------------------------------------------------------------------- 1 | # Header 2 | Header header 3 | 4 | # All labels stored in a vector 5 | panoptic_mapping_msgs/DetectronLabel[] labels 6 | -------------------------------------------------------------------------------- /panoptic_mapping_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panoptic_mapping_msgs 4 | 0.0.0 5 | Utilities to setup and run data for panoptic mapping. 6 | 7 | Lukas Schmid 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | message_generation 16 | message_runtime 17 | std_msgs 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /panoptic_mapping_msgs/srv/SaveLoadMap.srv: -------------------------------------------------------------------------------- 1 | string file_path 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /panoptic_mapping_msgs/srv/SetVisualizationMode.srv: -------------------------------------------------------------------------------- 1 | string visualization_mode 2 | string color_mode 3 | --- 4 | bool visualization_mode_set 5 | bool color_mode_set 6 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(panoptic_mapping_ros) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(catkin_simple REQUIRED) 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | catkin_package() 8 | 9 | ############# 10 | # Libraries # 11 | ############# 12 | 13 | cs_add_library(${PROJECT_NAME} 14 | src/panoptic_mapper.cpp 15 | src/input/input_synchronizer.cpp 16 | src/visualization/submap_visualizer.cpp 17 | src/visualization/single_tsdf_visualizer.cpp 18 | src/visualization/planning_visualizer.cpp 19 | src/visualization/tracking_visualizer.cpp 20 | src/conversions/conversions.cpp 21 | ) 22 | 23 | ############### 24 | # Executables # 25 | ############### 26 | 27 | cs_add_executable(panoptic_mapper_node 28 | app/panoptic_mapper_node.cpp 29 | ) 30 | target_link_libraries(panoptic_mapper_node ${PROJECT_NAME}) 31 | 32 | ########### 33 | # Export # 34 | ########### 35 | 36 | cs_install() 37 | cs_export() 38 | 39 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/README.md: -------------------------------------------------------------------------------- 1 | # Panoptic Mapping ROS 2 | ROS dependent code and executables of **panoptic_mapping**. -------------------------------------------------------------------------------- /panoptic_mapping_ros/app/panoptic_mapper_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "panoptic_mapping_ros/panoptic_mapper.h" 5 | 6 | int main(int argc, char** argv) { 7 | // Start Ros. 8 | ros::init(argc, argv, "panoptic_mapper", ros::init_options::NoSigintHandler); 9 | 10 | // Always add these arguments for proper logging. 11 | config_utilities::RequiredArguments ra( 12 | &argc, &argv, {"--logtostderr", "--colorlogtostderr"}); 13 | 14 | // Setup logging. 15 | google::InitGoogleLogging(argv[0]); 16 | google::InstallFailureSignalHandler(); 17 | google::ParseCommandLineFlags(&argc, &argv, false); 18 | 19 | // Setup node. 20 | ros::NodeHandle nh(""); 21 | ros::NodeHandle nh_private("~"); 22 | panoptic_mapping::PanopticMapper mapper(nh, nh_private); 23 | 24 | // Setup spinning. 25 | ros::AsyncSpinner spinner(mapper.getConfig().ros_spinner_threads); 26 | spinner.start(); 27 | ros::waitForShutdown(); 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/config/mapper/single_tsdf.yaml: -------------------------------------------------------------------------------- 1 | # General Params 2 | verbosity: 3 3 | use_threadsafe_submap_collection: true 4 | use_transform_caching: false 5 | transform_lookup_time: 0.1 6 | global_frame_name: world 7 | visualization_interval: -1.0 8 | data_logging_interval: -1.0 9 | print_timing_interval: 0.0 10 | max_input_queue_length: 1000 11 | ros_spinning_threads: 8 12 | 13 | labels: 14 | type: csv 15 | file_name: /home/lukas/Documents/Datasets/flat_dataset/detectron_labels.csv 16 | 17 | camera: 18 | verbosity: 1 19 | width: 640 # Px 20 | height: 480 21 | vx: 320 22 | vy: 240 23 | fx: 320 24 | fy: 320 25 | max_range: 5 # m 26 | min_range: 0.1 27 | 28 | id_tracker: 29 | type: single_tsdf 30 | verbosity: 1 31 | 32 | # Single TSDF 33 | voxel_size: 0.05 34 | truncation_distance: -2 35 | voxels_per_side: 16 36 | use_class_layer: false 37 | use_detectron: true 38 | use_instance_classification: false 39 | classification: 40 | type: fixed_count 41 | 42 | tsdf_integrator: 43 | type: single_tsdf 44 | verbosity: 1 45 | 46 | # Projective 47 | use_weight_dropoff: true 48 | max_weight: 10000 49 | interpolation_method: adaptive # nearest, bilinear, adaptive 50 | integration_threads: 8 51 | 52 | visualization: 53 | submaps: 54 | type: single_tsdf 55 | verbosity: 1 56 | color_mode: classes # color, normals, classes, instances, classification 57 | visualization_mode: all # all 58 | submap_color_discretization: 20 59 | visualize_mesh: true 60 | visualize_tsdf_blocks: true 61 | visualize_free_space: true 62 | visualize_bounding_volumes: true 63 | include_free_space: true 64 | mesh_min_weight: 0.000001 65 | 66 | planning: 67 | verbosity: 1 68 | visualize_slice: true 69 | slice_resolution: 0.1 70 | slice_height: -0.5 71 | 72 | tracking: 73 | verbosity: 1 74 | visualize_tracking: true 75 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/include/panoptic_mapping_ros/conversions/conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_ROS_CONVERSIONS_CONVERSIONS_H_ 2 | #define PANOPTIC_MAPPING_ROS_CONVERSIONS_CONVERSIONS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace panoptic_mapping { 9 | 10 | DetectronLabel detectronLabelFromMsg( 11 | const panoptic_mapping_msgs::DetectronLabel& msg); 12 | 13 | DetectronLabels detectronLabelsFromMsg( 14 | const panoptic_mapping_msgs::DetectronLabels& msg); 15 | 16 | } // namespace panoptic_mapping 17 | 18 | #endif // PANOPTIC_MAPPING_ROS_CONVERSIONS_CONVERSIONS_H_ 19 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/include/panoptic_mapping_ros/visualization/planning_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_ROS_VISUALIZATION_PLANNING_VISUALIZER_H_ 2 | #define PANOPTIC_MAPPING_ROS_VISUALIZATION_PLANNING_VISUALIZER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace panoptic_mapping { 14 | 15 | class PlanningVisualizer { 16 | public: 17 | // config 18 | struct Config : public config_utilities::Config { 19 | int verbosity = 1; 20 | bool visualize_planning_slice = true; 21 | float slice_resolution = 0.1; // m 22 | float slice_height = 1.0; // m 23 | std::string ros_namespace; 24 | 25 | Config() { setConfigName("PlanningVisualizer"); } 26 | 27 | protected: 28 | void setupParamsAndPrinting() override; 29 | void fromRosParam() override; 30 | void checkParams() const override; 31 | }; 32 | 33 | // Constructors. 34 | explicit PlanningVisualizer( 35 | const Config& config, 36 | std::shared_ptr planning_interface); 37 | virtual ~PlanningVisualizer() = default; 38 | 39 | // Visualization message creation. 40 | visualization_msgs::Marker generateSliceMsg(); 41 | 42 | // Publish visualization requests. 43 | void visualizeAll(); 44 | void visualizePlanningSlice(); 45 | 46 | // Interaction. 47 | void setGlobalFrameName(const std::string& frame_name) { 48 | global_frame_name_ = frame_name; 49 | } 50 | 51 | private: 52 | const Config config_; 53 | 54 | // Members. 55 | std::shared_ptr planning_interface_; 56 | 57 | // Data. 58 | std::string global_frame_name_; 59 | 60 | // Publishers. 61 | ros::NodeHandle nh_; 62 | ros::Publisher slice_pub_; 63 | }; 64 | 65 | } // namespace panoptic_mapping 66 | 67 | #endif // PANOPTIC_MAPPING_ROS_VISUALIZATION_PLANNING_VISUALIZER_H_ 68 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/include/panoptic_mapping_ros/visualization/single_tsdf_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_ROS_VISUALIZATION_SINGLE_TSDF_VISUALIZER_H_ 2 | #define PANOPTIC_MAPPING_ROS_VISUALIZATION_SINGLE_TSDF_VISUALIZER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "panoptic_mapping_ros/visualization/submap_visualizer.h" 15 | 16 | namespace panoptic_mapping { 17 | 18 | /** 19 | * @brief A visualizer tailored to work on the single TSDF emulator. Use 20 | * together with SingleTsdfTracker and SingleTsdfIntegrator. 21 | */ 22 | class SingleTsdfVisualizer : public SubmapVisualizer { 23 | public: 24 | // Config. 25 | struct Config : public config_utilities::Config { 26 | int verbosity = 1; 27 | // Factor which multiplies the entropy value in order to bring it into 28 | // [0,1] range for visualization. 29 | float entropy_factor = 1.0f; 30 | // Normalisation to bing Score into [0, 1] range 31 | float min_score = 0.0f; 32 | float max_score = 1.0f; 33 | 34 | // Standard visualizer config. 35 | SubmapVisualizer::Config submap_visualizer; 36 | 37 | Config() { setConfigName("SingleTsdfVisualizer"); } 38 | 39 | protected: 40 | void setupParamsAndPrinting() override; 41 | void checkParams() const override; 42 | }; 43 | 44 | // Constructors. 45 | SingleTsdfVisualizer(const Config& config, std::shared_ptr globals, 46 | bool print_config = true); 47 | ~SingleTsdfVisualizer() override = default; 48 | 49 | // Visualization message creation. 50 | std::vector generateMeshMsgs( 51 | SubmapCollection* submaps) override; 52 | 53 | // Interaction. 54 | void reset() override; 55 | void clearMesh() override; 56 | void setVisualizationMode(VisualizationMode visualization_mode) override; 57 | void setColorMode(ColorMode color_mode) override; 58 | 59 | protected: 60 | void colorMeshBlockFromClass(const Submap& submap, 61 | voxblox_msgs::MeshBlock* mesh_block); 62 | void colorMeshBlockFromScore(const Submap& submap, 63 | voxblox_msgs::MeshBlock* mesh_block); 64 | std::function getColoring() const; 65 | void updateVisInfos(const SubmapCollection& submaps) override; 66 | 67 | private: 68 | const Config config_; 69 | static config_utilities::Factory::RegistrationRos< 70 | SubmapVisualizer, SingleTsdfVisualizer, std::shared_ptr> 71 | registration_; 72 | 73 | // NOTE(schmluk): This namespace could also be unique per visualizer to allow 74 | // multiple meshes to be shown on the same topic. 75 | const std::string map_name_space_ = "single_tsdf"; 76 | 77 | // Cached / tracked data. 78 | SubmapVisInfo info_; 79 | }; 80 | 81 | } // namespace panoptic_mapping 82 | 83 | #endif // PANOPTIC_MAPPING_ROS_VISUALIZATION_SINGLE_TSDF_VISUALIZER_H_ 84 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/include/panoptic_mapping_ros/visualization/tracking_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_ROS_VISUALIZATION_TRACKING_VISUALIZER_H_ 2 | #define PANOPTIC_MAPPING_ROS_VISUALIZATION_TRACKING_VISUALIZER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace panoptic_mapping { 13 | 14 | class TrackingVisualizer { 15 | public: 16 | // config 17 | struct Config : public config_utilities::Config { 18 | int verbosity = 1; 19 | bool visualize_tracking = true; 20 | std::string ros_namespace; 21 | 22 | Config() { setConfigName("TrackingVisualizer"); } 23 | 24 | protected: 25 | void setupParamsAndPrinting() override; 26 | void fromRosParam() override; 27 | }; 28 | 29 | // Constructors. 30 | explicit TrackingVisualizer(const Config& config); 31 | virtual ~TrackingVisualizer() = default; 32 | 33 | // Setup. 34 | void registerIDTracker(IDTrackerBase* tracker); 35 | 36 | // Publish visualization requests. 37 | void publishImage(const cv::Mat& image, const std::string& name); 38 | 39 | private: 40 | const Config config_; 41 | 42 | // Publishers. 43 | ros::NodeHandle nh_; 44 | std::unordered_map publishers_; 45 | }; 46 | 47 | } // namespace panoptic_mapping 48 | 49 | #endif // PANOPTIC_MAPPING_ROS_VISUALIZATION_TRACKING_VISUALIZER_H_ 50 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panoptic_mapping_ros 4 | 0.0.0 5 | Panoptic mapping for long term scene understanding and consistency. 6 | 7 | Lukas Schmid 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | rospy 16 | roscpp 17 | message_generation 18 | message_runtime 19 | 20 | std_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | rosgraph_msgs 24 | 25 | tf2_ros 26 | eigen_catkin 27 | glog_catkin 28 | gflags_catkin 29 | protobuf_catkin 30 | opencv3_catkin 31 | voxblox 32 | voxblox_ros 33 | 34 | panoptic_mapping 35 | panoptic_mapping_msgs 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/src/conversions/conversions.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping_ros/conversions/conversions.h" 2 | 3 | namespace panoptic_mapping { 4 | 5 | DetectronLabel detectronLabelFromMsg( 6 | const panoptic_mapping_msgs::DetectronLabel& msg) { 7 | DetectronLabel result; 8 | result.id = msg.id; 9 | result.instance_id = msg.instance_id; 10 | result.is_thing = msg.is_thing; 11 | result.category_id = msg.category_id; 12 | result.score = msg.score; 13 | return result; 14 | } 15 | 16 | DetectronLabels detectronLabelsFromMsg( 17 | const panoptic_mapping_msgs::DetectronLabels& msg) { 18 | DetectronLabels result; 19 | for (const panoptic_mapping_msgs::DetectronLabel& label : msg.labels) { 20 | result[label.id] = detectronLabelFromMsg(label); 21 | } 22 | return result; 23 | } 24 | 25 | } // namespace panoptic_mapping 26 | -------------------------------------------------------------------------------- /panoptic_mapping_ros/src/visualization/tracking_visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "panoptic_mapping_ros/visualization/tracking_visualizer.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace panoptic_mapping { 9 | 10 | void TrackingVisualizer::Config::setupParamsAndPrinting() { 11 | setupParam("verbosity", &verbosity); 12 | setupParam("visualize_tracking", &visualize_tracking); 13 | } 14 | 15 | void TrackingVisualizer::Config::fromRosParam() { 16 | ros_namespace = rosParamNameSpace(); 17 | } 18 | 19 | TrackingVisualizer::TrackingVisualizer(const Config& config) 20 | : config_(config.checkValid()) { 21 | // Print config after setting up the modes. 22 | LOG_IF(INFO, config_.verbosity >= 1) << "\n" << config_.toString(); 23 | 24 | // Setup nodehandle. 25 | nh_ = ros::NodeHandle(config_.ros_namespace); 26 | } 27 | 28 | void TrackingVisualizer::registerIDTracker(IDTrackerBase* tracker) { 29 | if (config_.visualize_tracking) { 30 | CHECK_NOTNULL(tracker); 31 | tracker->setVisualizationCallback( 32 | [this](const cv::Mat& image, const std::string& name) { 33 | publishImage(image, name); 34 | }); 35 | } 36 | } 37 | 38 | void TrackingVisualizer::publishImage(const cv::Mat& image, 39 | const std::string& name) { 40 | auto it = publishers_.find(name); 41 | if (it == publishers_.end()) { 42 | // Advertise a new topic if there is no publisher for the given name. 43 | it = publishers_.emplace(name, nh_.advertise(name, 100)) 44 | .first; 45 | } 46 | 47 | // Publish the image, expected as BGR8. 48 | std_msgs::Header header; 49 | header.stamp = ros::Time::now(); 50 | it->second.publish( 51 | cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, image) 52 | .toImageMsg()); 53 | } 54 | 55 | } // namespace panoptic_mapping 56 | -------------------------------------------------------------------------------- /panoptic_mapping_ssh.rosinstall: -------------------------------------------------------------------------------- 1 | # General Dependencies 2 | - git: 3 | local-name: catkin_simple 4 | uri: git@github.com:catkin/catkin_simple.git 5 | - git: 6 | local-name: eigen_catkin 7 | uri: git@github.com:ethz-asl/eigen_catkin.git 8 | - git: 9 | local-name: eigen_checks 10 | uri: git@github.com:ethz-asl/eigen_checks.git 11 | - git: 12 | local-name: gflags_catkin 13 | uri: git@github.com:ethz-asl/gflags_catkin.git 14 | - git: 15 | local-name: glog_catkin 16 | uri: git@github.com:ethz-asl/glog_catkin.git 17 | - git: 18 | local-name: minkindr 19 | uri: git@github.com:ethz-asl/minkindr.git 20 | - git: 21 | local-name: minkindr_ros 22 | uri: git@github.com:ethz-asl/minkindr_ros.git 23 | - git: 24 | local-name: protobuf_catkin 25 | uri: git@github.com:ethz-asl/protobuf_catkin.git 26 | - git: 27 | local-name: opencv3_catkin 28 | uri: git@github.com:ethz-asl/opencv3_catkin.git 29 | - git: 30 | local-name: numpy_eigen 31 | uri: git@github.com:ethz-asl/numpy_eigen.git 32 | - git: 33 | local-name: catkin_boost_python_buildtool 34 | uri: git@github.com:ethz-asl/catkin_boost_python_buildtool.git 35 | - git: 36 | local-name: catkin_grpc 37 | uri: git@github.com:CogRob/catkin_grpc.git 38 | 39 | # Package Dependencies. Notice that voxblox requires a special branch. 40 | - git: 41 | local-name: voxblox 42 | uri: git@github.com:ethz-asl/voxblox.git 43 | version: 'devel/panoptic_mapping' 44 | - git: 45 | local-name: cblox 46 | uri: git@github.com:ethz-asl/cblox.git 47 | 48 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(panoptic_mapping_utils) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(catkin_simple REQUIRED) 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | catkin_package() 8 | 9 | ############# 10 | # Libraries # 11 | ############# 12 | 13 | cs_add_library(${PROJECT_NAME} 14 | src/evaluation/map_evaluator.cpp 15 | src/mesh_saver.cpp 16 | ) 17 | 18 | ############### 19 | # Executables # 20 | ############### 21 | 22 | cs_add_executable(single_map_evaluation 23 | app/single_map_evaluation.cpp 24 | ) 25 | target_link_libraries(single_map_evaluation ${PROJECT_NAME}) 26 | 27 | cs_add_executable(multi_map_evaluation 28 | app/multi_map_evaluation.cpp 29 | ) 30 | target_link_libraries(multi_map_evaluation ${PROJECT_NAME}) 31 | 32 | cs_add_executable(mesh_saver 33 | app/mesh_saver_node.cpp 34 | ) 35 | target_link_libraries(mesh_saver ${PROJECT_NAME}) 36 | 37 | ########## 38 | # Export # 39 | ########## 40 | 41 | cs_install() 42 | cs_export() 43 | 44 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/README.md: -------------------------------------------------------------------------------- 1 | # Panoptic Mapping Utils 2 | Utility tools and scripts around **panoptic_mapping**. Playing and creating datasets and similar. 3 | 4 | ## Mesh Saver 5 | 6 | Run the mesh saver to save the same mesh that is visualised in RVIZ as a `.ply` file: 7 | 8 | ``` 9 | 14 | 15 | 16 | 17 | ``` 18 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/app/mesh_saver_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "panoptic_mapping_utils/mesh_saver.h" 5 | 6 | int main(int argc, char** argv) { 7 | // Start Ros. 8 | ros::init(argc, argv, "mesh_saver", ros::init_options::NoSigintHandler); 9 | 10 | 11 | // Setup logging. 12 | google::InitGoogleLogging(argv[0]); 13 | google::InstallFailureSignalHandler(); 14 | google::ParseCommandLineFlags(&argc, &argv, false); 15 | 16 | // Setup node. 17 | ros::NodeHandle nh_private("~"); 18 | panoptic_mapping::MeshSaver mesh_saver(nh_private); 19 | ros::spin(); 20 | ros::waitForShutdown(); 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/app/multi_map_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "panoptic_mapping_utils/evaluation/map_evaluator.h" 5 | 6 | int main(int argc, char** argv) { 7 | config_utilities::RequiredArguments ra( 8 | &argc, &argv, {"--logtostderr", "--colorlogtostderr"}); 9 | 10 | // Setup logging. 11 | google::InitGoogleLogging(argv[0]); 12 | google::InstallFailureSignalHandler(); 13 | google::ParseCommandLineFlags(&argc, &argv, false); 14 | 15 | // Run ros. 16 | ros::init(argc, argv, "evaluation_node"); 17 | ros::NodeHandle nh; 18 | ros::NodeHandle nh_private("~"); 19 | 20 | // Evaluator. 21 | panoptic_mapping::MapEvaluator evaluator(nh, nh_private); 22 | if (evaluator.setupMultiMapEvaluation()) { 23 | ros::spin(); 24 | } 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/app/single_map_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "panoptic_mapping_utils/evaluation/map_evaluator.h" 5 | 6 | int main(int argc, char** argv) { 7 | config_utilities::RequiredArguments ra( 8 | &argc, &argv, {"--logtostderr", "--colorlogtostderr"}); 9 | 10 | // Setup logging. 11 | google::InitGoogleLogging(argv[0]); 12 | google::InstallFailureSignalHandler(); 13 | google::ParseCommandLineFlags(&argc, &argv, false); 14 | 15 | // Run ros. 16 | ros::init(argc, argv, "evaluation_node"); 17 | ros::NodeHandle nh; 18 | ros::NodeHandle nh_private("~"); 19 | 20 | // Evaluator. 21 | panoptic_mapping::MapEvaluator evaluator(nh, nh_private); 22 | 23 | // Process the request 24 | auto request = config_utilities::getConfigFromRos< 25 | panoptic_mapping::MapEvaluator::EvaluationRequest>(nh_private); 26 | evaluator.evaluate(request); 27 | 28 | // Visualize. 29 | if (request.visualize) { 30 | while (ros::ok()) { 31 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 32 | evaluator.publishVisualization(); 33 | } 34 | } 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/include/panoptic_mapping_utils/evaluation/progress_bar.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_UTILS_EVALUATION_PROGRESS_BAR_H_ 2 | #define PANOPTIC_MAPPING_UTILS_EVALUATION_PROGRESS_BAR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace panoptic_mapping { 9 | 10 | // Print a progress bar to console for slow tasks. 11 | 12 | class ProgressBar { 13 | public: 14 | explicit ProgressBar(int width = 80, char symbol = '=') 15 | : width_(width < 8 ? 8 : width), symbol_(symbol) {} 16 | virtual ~ProgressBar() = default; 17 | 18 | bool display(float percentage) const { 19 | percentage = std::min(std::max(percentage, 0.f), 1.f); 20 | const int value = static_cast(percentage * 100.f); 21 | const int length = static_cast(percentage * (width_ - 7)); 22 | std::printf("\r%3d%% [%s%s]", value, std::string(length, symbol_).c_str(), 23 | std::string(width_ - 7 - length, ' ').c_str()); 24 | if (value >= 100) { 25 | std::cout << std::endl; 26 | return true; // returns whether progress finished. 27 | } else { 28 | std::fflush(stdout); 29 | return false; 30 | } 31 | } 32 | 33 | private: 34 | const char symbol_; 35 | const int width_; 36 | }; 37 | 38 | } // namespace panoptic_mapping 39 | 40 | #endif // PANOPTIC_MAPPING_UTILS_EVALUATION_PROGRESS_BAR_H_ 41 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/include/panoptic_mapping_utils/mesh_saver.h: -------------------------------------------------------------------------------- 1 | #ifndef PANOPTIC_MAPPING_UTILS_SCANNET_MESHSAVER_H_ 2 | #define PANOPTIC_MAPPING_UTILS_SCANNET_MESHSAVER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "voxblox/core/block_hash.h" 10 | #include "voxblox/core/common.h" 11 | #include "voxblox/mesh/mesh.h" 12 | #include "voxblox/mesh/mesh_utils.h" 13 | #include "voxblox/io/mesh_ply.h" 14 | #include 15 | #include 16 | 17 | namespace panoptic_mapping { 18 | 19 | class MeshSaver { 20 | public: 21 | MeshSaver(const ros::NodeHandle& nh); 22 | virtual ~MeshSaver() = default; 23 | void gotMeshCallback(const voxblox_msgs::MultiMesh& msg); 24 | 25 | private: 26 | void setupRos(); 27 | 28 | ros::NodeHandle nh_; 29 | 30 | ros::Subscriber mesh_sub_; 31 | }; 32 | 33 | } // namespace panoptic_mapping 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/evaluate_panmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/evaluate_series.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/generate_flat_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/parse_flat_to_airsim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/play_flat_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/play_rio_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/launch/scannnet_visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panoptic_mapping_utils 4 | 0.0.0 5 | Utilities to setup, run, and evaluate data for panoptic mapping. 6 | 7 | Lukas Schmid 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | rospy 16 | roscpp 17 | 18 | std_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | rosgraph_msgs 22 | panoptic_mapping_msgs 23 | tf2_ros 24 | 25 | panoptic_mapping 26 | panoptic_mapping_ros 27 | voxblox 28 | voxblox_ros 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/scripts/download_flat_dataset.sh: -------------------------------------------------------------------------------- 1 | # Check target dir. 2 | if [ -z $FLAT_DATA_DIR ]; 3 | then 4 | echo "The download destination 'FLAT_DATA_DIR' variable is not set."; 5 | exit 1 6 | else 7 | echo "Download to '$FLAT_DATA_DIR'?"; 8 | read -p "[Y/N]? " -n 1 -r 9 | if [[ ! $REPLY =~ ^[Yy]$ ]] 10 | then 11 | exit 1 12 | fi 13 | fi 14 | 15 | # Download. 16 | mkdir -p $FLAT_DATA_DIR 17 | wget http://robotics.ethz.ch/~asl-datasets/2021_Panoptic_Mapping/flat_dataset.zip -P $FLAT_DATA_DIR 18 | unzip $FLAT_DATA_DIR/flat_dataset.zip -d $FLAT_DATA_DIR 19 | rm $FLAT_DATA_DIR/flat_dataset.zip 20 | 21 | # Finished. 22 | echo "Successfully downloaded the 'flat_dataset' to '$FLAT_DATA_DIR'."; 23 | exit 0 24 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/scripts/setup_airsim.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Run this script to setup semantic labelling. 3 | 4 | python ~/catkin_ws/src/unreal_airsim/app/tools/compute_infrared_compensation.py -d /home/lukas/Documents/PanopticMapping/Data -c Id_cam & 5 | BACK_PID=$! 6 | 7 | while kill -0 $BACK_PID ; do 8 | # Waiting for the previous command to finish. 9 | sleep 0.5 10 | done 11 | 12 | python /home/lukas/catkin_ws/src/panoptic_mapping/panoptic_mapping_ros/app/tools/flat_semantic_labelling.py 13 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/src/evaluation/evaluation_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import rospy 5 | from panoptic_mapping_msgs.srv import SaveLoadMap 6 | 7 | # Hash number for each scene, DATA_IDS[scene_id][scan_id] = hash. 8 | DATA_IDS = [[ 9 | '0cac7578-8d6f-2d13-8c2d-bfa7a04f8af3', 10 | '2451c041-fae8-24f6-9213-b8b6af8d86c1', 11 | 'ddc73793-765b-241a-9ecd-b0cebb7cf916', 12 | 'ddc73795-765b-241a-9c5d-b97744afe077' 13 | ], 14 | [ 15 | '20c9939d-698f-29c5-85c6-3c618e00061f', 16 | 'f62fd5f8-9a3f-2f44-8b1e-1289a3a61e26' 17 | ]] 18 | 19 | 20 | class EvaluationManager(object): 21 | def __init__(self): 22 | """ Initialize ros node and read params """ 23 | # params 24 | self.data_path = rospy.get_param('~map_file', '') 25 | self.eval_srv_name = rospy.get_param( 26 | '~eval_srv_name', '/multi_map_evaluation/process_map') 27 | 28 | # Check params. 29 | if not self.data_path: 30 | rospy.logfatal("The 'map_file' target directory needs to be set.") 31 | if not os.path.isdir(self.data_path): 32 | rospy.logfatal("The 'map_file' must be the target directory.") 33 | 34 | # RIO 35 | use_rio = rospy.get_param('~use_rio', False) 36 | if use_rio: 37 | scene_id = rospy.get_param('~scene_id', 0) 38 | scan_id = rospy.get_param('~scan_id', 0) 39 | if len(DATA_IDS) <= scene_id: 40 | rospy.logfatal("Scene ID %i is out of bounds (%i)." % 41 | (scene_id, len(DATA_IDS) - 1)) 42 | if len(DATA_IDS[scene_id]) <= scan_id: 43 | rospy.logfatal( 44 | "Scan ID %i is out of bounds (%i) for scene %i." % 45 | (scan_id, len(DATA_IDS[scene_id]) - 1, scene_id)) 46 | # NOTE(Schmluk): This is currently just hardcoded. 47 | rospy.set_param( 48 | "/multi_map_evaluation/ground_truth_pointcloud_file", 49 | "/home/lukas/Documents/Datasets/3RScan/%s/gt_10000.ply" % 50 | DATA_IDS[scene_id][scan_id]) 51 | 52 | # Setup rosservice 53 | rospy.wait_for_service(self.eval_srv_name) 54 | self.eval_srv = rospy.ServiceProxy(self.eval_srv_name, SaveLoadMap) 55 | print("Successfully setup the evaluation manager.") 56 | 57 | def evaluate(self): 58 | # Get target maps. 59 | files = [ 60 | x for x in os.listdir(self.data_path) if x.endswith('.panmap') 61 | ] 62 | 63 | for i, f in enumerate(sorted(files)): 64 | print("Evaluating map %i/%i:" % (i, len(files) - 1)) 65 | self.eval_srv(os.path.join(self.data_path, f)) 66 | print("Evaluation finished successfully.") 67 | 68 | 69 | if __name__ == '__main__': 70 | rospy.init_node('evaluation_manager') 71 | em = EvaluationManager() 72 | em.evaluate() 73 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/src/evaluation/panoptic_data_reader.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import csv 3 | import os 4 | import warnings 5 | 6 | 7 | def read_panoptic_mapper_data_log(file_name): 8 | """ A stored data log of the panoptic mapper and converts it into a 9 | dictionary of numpy arrays if numeric or lists of strings otherwise. """ 10 | result = {} 11 | if not os.path.isfile(file_name): 12 | warnings.warn("Target file '%s' does not exist." % file_name) 13 | return result 14 | 15 | with open(file_name, newline='') as f: 16 | reader = csv.reader(f) 17 | data = list(reader) 18 | for col in range(len(data[0])): 19 | header = data[0][col] 20 | values = [] 21 | for row in range(1, len(data)): 22 | values.append(data[row][col]) 23 | try: 24 | result[header] = np.array(values, dtype=float) 25 | except: 26 | result[header] = values 27 | return result 28 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/src/run_helpers/map_loader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Simply tells the mapper to load a .panmap file as soon as possible. 4 | """ 5 | import rospy 6 | from std_srvs.srv import Empty 7 | from panoptic_mapping_msgs.srv import SaveLoadMap 8 | 9 | if __name__ == '__main__': 10 | rospy.init_node('map_loader', anonymous=True) 11 | path = rospy.get_param('~path', "") 12 | map_srv_name = rospy.get_param('~map_srv_name', 13 | "/panoptic_mapper/load_map") 14 | play_srv_name = rospy.get_param('~play_srv_name', 15 | "/flat_data_player/start") 16 | delay = rospy.get_param('~delay', 0) 17 | rospy.loginfo("map_loader: Waiting for service '%s'." % map_srv_name) 18 | rospy.wait_for_service(map_srv_name) 19 | rospy.sleep(delay) 20 | # Load the maps. 21 | srv = rospy.ServiceProxy(map_srv_name, SaveLoadMap) 22 | srv(path) 23 | # Start playing the data. 24 | rospy.sleep(0.5) 25 | rospy.loginfo("map_loader: Waiting for service '%s'." % play_srv_name) 26 | rospy.wait_for_service(play_srv_name) 27 | srv = rospy.ServiceProxy(play_srv_name, Empty) 28 | srv() 29 | rospy.loginfo("[map_loader] Finished after calling service.") 30 | -------------------------------------------------------------------------------- /panoptic_mapping_utils/src/run_helpers/path_visualizer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | 5 | import rospy 6 | from geometry_msgs.msg import PoseStamped 7 | from visualization_msgs.msg import Marker 8 | 9 | 10 | class PathVisualizer(object): 11 | def __init__(self): 12 | """ Initialize ros node and read params """ 13 | # Params 14 | self.red = rospy.get_param('~r', 1) # color 0-1 15 | self.green = rospy.get_param('~g', 0) 16 | self.blue = rospy.get_param('~b', 0) 17 | self.length = rospy.get_param('~length', 0.3) # m 18 | self.width = rospy.get_param('~width', 0.03) # m 19 | self.distance = rospy.get_param('~distance', 0.05) # m 20 | self.use_arrow = rospy.get_param('~use_arrow', True) # m 21 | 22 | # ROS 23 | self.sub = rospy.Subscriber("~pose_in", PoseStamped, self.pose_cb) 24 | self.pub = rospy.Publisher("~path", Marker, queue_size=10) 25 | 26 | # variables 27 | self.previous_pose = None 28 | self.counter = 0 29 | 30 | def pose_cb(self, pose_stamped): 31 | # Init 32 | if self.previous_pose is None: 33 | self.previous_pose = pose_stamped.pose 34 | return 35 | 36 | # Only plot every 'distance' meters 37 | pose = pose_stamped.pose 38 | dist = np.linalg.norm( 39 | np.array([ 40 | self.previous_pose.position.x, self.previous_pose.position.y, 41 | self.previous_pose.position.z 42 | ]) - np.array([pose.position.x, pose.position.y, pose.position.z])) 43 | if dist < self.distance: 44 | return 45 | self.previous_pose = pose_stamped.pose 46 | 47 | # Plot 48 | msg = Marker() 49 | msg.header = pose_stamped.header 50 | msg.pose = pose 51 | msg.color.r = self.red 52 | msg.color.g = self.green 53 | msg.color.b = self.blue 54 | msg.color.a = 1 55 | msg.ns = "path" 56 | if self.use_arrow: 57 | msg.type = 0 58 | msg.scale.x = self.length 59 | msg.scale.y = self.width 60 | msg.scale.z = self.width * 2 61 | else: 62 | # Sphere 63 | msg.type = 2 64 | msg.scale.x = self.width 65 | msg.scale.y = self.width 66 | msg.scale.z = self.width 67 | msg.id = self.counter 68 | self.counter = self.counter + 1 69 | self.pub.publish(msg) 70 | 71 | 72 | if __name__ == '__main__': 73 | rospy.init_node('path_visualizer', anonymous=True) 74 | pv = PathVisualizer() 75 | rospy.spin() 76 | --------------------------------------------------------------------------------