├── .clang-format ├── .github └── workflows │ └── build_test_20.yml ├── .gitignore ├── Dockerfile ├── LICENSE ├── README.md ├── drift_simulation ├── CMakeLists.txt ├── config │ ├── intensities │ │ ├── light.yaml │ │ ├── moderate.yaml │ │ ├── none.yaml │ │ ├── severe.yaml │ │ └── strong.yaml │ └── rollouts │ │ └── doals │ │ ├── hauptgebaeude │ │ ├── sequence_1 │ │ │ ├── light_1.csv │ │ │ ├── light_2.csv │ │ │ ├── light_3.csv │ │ │ ├── moderate_1.csv │ │ │ ├── moderate_2.csv │ │ │ ├── moderate_3.csv │ │ │ ├── none.csv │ │ │ ├── severe_1.csv │ │ │ ├── severe_2.csv │ │ │ ├── severe_3.csv │ │ │ ├── strong_1.csv │ │ │ ├── strong_2.csv │ │ │ └── strong_3.csv │ │ └── sequence_2 │ │ │ ├── light_1.csv │ │ │ ├── light_2.csv │ │ │ ├── light_3.csv │ │ │ ├── moderate_1.csv │ │ │ ├── moderate_2.csv │ │ │ ├── moderate_3.csv │ │ │ ├── none.csv │ │ │ ├── severe_1.csv │ │ │ ├── severe_2.csv │ │ │ ├── severe_3.csv │ │ │ ├── strong_1.csv │ │ │ ├── strong_2.csv │ │ │ └── strong_3.csv │ │ ├── niederdorf │ │ ├── sequence_1 │ │ │ ├── light_1.csv │ │ │ ├── light_2.csv │ │ │ ├── light_3.csv │ │ │ ├── moderate_1.csv │ │ │ ├── moderate_2.csv │ │ │ ├── moderate_3.csv │ │ │ ├── none.csv │ │ │ ├── severe_1.csv │ │ │ ├── severe_2.csv │ │ │ ├── severe_3.csv │ │ │ ├── strong_1.csv │ │ │ ├── strong_2.csv │ │ │ └── strong_3.csv │ │ └── sequence_2 │ │ │ ├── light_1.csv │ │ │ ├── light_2.csv │ │ │ ├── light_3.csv │ │ │ ├── moderate_1.csv │ │ │ ├── moderate_2.csv │ │ │ ├── moderate_3.csv │ │ │ ├── none.csv │ │ │ ├── severe_1.csv │ │ │ ├── severe_2.csv │ │ │ ├── severe_3.csv │ │ │ ├── strong_1.csv │ │ │ ├── strong_2.csv │ │ │ └── strong_3.csv │ │ ├── shopville │ │ ├── sequence_1 │ │ │ ├── light_1.csv │ │ │ ├── light_2.csv │ │ │ ├── light_3.csv │ │ │ ├── moderate_1.csv │ │ │ ├── moderate_2.csv │ │ │ ├── moderate_3.csv │ │ │ ├── none.csv │ │ │ ├── severe_1.csv │ │ │ ├── severe_2.csv │ │ │ ├── severe_3.csv │ │ │ ├── strong_1.csv │ │ │ ├── strong_2.csv │ │ │ └── strong_3.csv │ │ └── sequence_2 │ │ │ ├── light_1.csv │ │ │ ├── light_2.csv │ │ │ ├── light_3.csv │ │ │ ├── moderate_1.csv │ │ │ ├── moderate_2.csv │ │ │ ├── moderate_3.csv │ │ │ ├── none.csv │ │ │ ├── severe_1.csv │ │ │ ├── severe_2.csv │ │ │ ├── severe_3.csv │ │ │ ├── strong_1.csv │ │ │ ├── strong_2.csv │ │ │ └── strong_3.csv │ │ └── station │ │ ├── sequence_1 │ │ ├── light_1.csv │ │ ├── light_2.csv │ │ ├── light_3.csv │ │ ├── moderate_1.csv │ │ ├── moderate_2.csv │ │ ├── moderate_3.csv │ │ ├── none.csv │ │ ├── severe_1.csv │ │ ├── severe_2.csv │ │ ├── severe_3.csv │ │ ├── strong_1.csv │ │ ├── strong_2.csv │ │ └── strong_3.csv │ │ └── sequence_2 │ │ ├── light_1.csv │ │ ├── light_2.csv │ │ ├── light_3.csv │ │ ├── moderate_1.csv │ │ ├── moderate_2.csv │ │ ├── moderate_3.csv │ │ ├── none.csv │ │ ├── severe_1.csv │ │ ├── severe_2.csv │ │ ├── severe_3.csv │ │ ├── strong_1.csv │ │ ├── strong_2.csv │ │ └── strong_3.csv ├── include │ └── drift_simulation │ │ ├── drift_reader.h │ │ ├── normal_distribution.h │ │ └── odometry_drift_simulator.h ├── launch │ └── generate_drift_rollout.launch ├── package.xml ├── scripts │ └── create_drift_rollouts.sh └── src │ ├── drift_reader.cpp │ ├── drift_reader_node.cpp │ ├── normal_distribution.cpp │ ├── odometry_drift_simulator.cpp │ └── odometry_drift_simulator_node.cpp ├── dynablox ├── CMakeLists.txt ├── include │ └── dynablox │ │ ├── 3rd_party │ │ └── config_utilities.hpp │ │ ├── common │ │ ├── index_getter.h │ │ ├── neighborhood_search.h │ │ └── types.h │ │ ├── evaluation │ │ ├── evaluator.h │ │ ├── ground_truth_handler.h │ │ └── io_tools.h │ │ └── processing │ │ ├── clustering.h │ │ ├── ever_free_integrator.h │ │ ├── preprocessing.h │ │ └── tracking.h ├── package.xml └── src │ ├── evaluation │ ├── evaluator.cpp │ ├── ground_truth_handler.cpp │ └── io_tools.cpp │ └── processing │ ├── clustering.cpp │ ├── ever_free_integrator.cpp │ ├── preprocessing.cpp │ └── tracking.cpp ├── dynablox_ros ├── CMakeLists.txt ├── config │ ├── motion_detector │ │ └── default.yaml │ └── rviz │ │ ├── cloud_visualizer.rviz │ │ └── default.rviz ├── include │ └── dynablox_ros │ │ ├── motion_detector.h │ │ └── visualization │ │ ├── cloud_visualizer.h │ │ └── motion_visualizer.h ├── launch │ ├── cloud_visualizer.launch │ ├── play_doals_data.launch │ ├── play_dynablox_data.launch │ ├── run_experiment.launch │ └── visualizer.launch ├── package.xml ├── scripts │ ├── download_doals_data.sh │ └── download_dynablox_data.sh └── src │ ├── cloud_visualizer_node.cpp │ ├── evaluation │ ├── data_tools.py │ └── evaluate_data.py │ ├── motion_detector.cpp │ ├── motion_detector_node.cpp │ └── visualization │ ├── cloud_visualizer.cpp │ └── motion_visualizer.cpp ├── https.rosinstall └── ssh.rosinstall /.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@v4 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 | sudo apt-get install python3-vcstool python3-catkin-tools ros-$ROS_DISTRO-cmake-modules protobuf-compiler autoconf git rsync -y 52 | 53 | - name: Release Build Test 54 | working-directory: 55 | env: 56 | DEBIAN_FRONTEND: noninteractive 57 | shell: bash 58 | run: | 59 | apt update 60 | mkdir -p $HOME/catkin_ws/src; 61 | cd $HOME/catkin_ws 62 | catkin init 63 | catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}" 64 | catkin config --merge-devel 65 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo 66 | cd $HOME/catkin_ws/src 67 | ln -s $GITHUB_WORKSPACE 68 | vcs import . < ./dynablox/https.rosinstall --recursive 69 | rosdep update 70 | rosdep install --from-paths . --ignore-src -y --rosdistro ${{matrix.config.rosdistro}} 71 | catkin build -j$(nproc) -l$(nproc) dynablox_ros 72 | 73 | -------------------------------------------------------------------------------- /.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 | tmp.py -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:noetic-desktop-full 2 | LABEL maintainer="Kin Zhang " 3 | 4 | # Just in case we need it 5 | ENV DEBIAN_FRONTEND noninteractive 6 | 7 | # install zsh 8 | RUN apt update && apt install -y wget git zsh tmux vim g++ 9 | RUN sh -c "$(wget -O- https://github.com/deluan/zsh-in-docker/releases/download/v1.1.5/zsh-in-docker.sh)" -- \ 10 | -t robbyrussell \ 11 | -p git \ 12 | -p ssh-agent \ 13 | -p https://github.com/agkozak/zsh-z \ 14 | -p https://github.com/zsh-users/zsh-autosuggestions \ 15 | -p https://github.com/zsh-users/zsh-completions \ 16 | -p https://github.com/zsh-users/zsh-syntax-highlighting 17 | 18 | RUN echo "source /opt/ros/noetic/setup.zsh" >> ~/.zshrc 19 | RUN echo "source /opt/ros/noetic/setup.bashrc" >> ~/.bashrc 20 | 21 | RUN mkdir -p /workspace/dynablox_ws 22 | RUN git clone --recurse-submodules https://github.com/ethz-asl/dynablox.git /workspace/dynablox_ws/src/dynablox 23 | RUN apt-get install -y python3-vcstool python3-catkin-tools ros-noetic-cmake-modules protobuf-compiler autoconf rsync libtool 24 | # dynablox dependencies 25 | RUN cd /workspace/dynablox_ws/src && vcs import . < ./dynablox/https.rosinstall --recursive 26 | # ouster driver dependencies 27 | RUN cd /workspace && git clone https://github.com/gabime/spdlog.git && cd spdlog && mkdir build && cd build && cmake .. -DCMAKE_CXX_FLAGS=-fPIC && make -j && make install 28 | 29 | WORKDIR /workspace/dynablox_ws -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, ETHZ ASL 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Ubuntu 20.04 + ROS Noetic: Build](https://github.com/ethz-asl/dynablox/actions/workflows/build_test_20.yml/badge.svg) 2 | 3 | # Dynablox 4 | An online volumetric mapping-based approach for real-time detection of diverse dynamic objects in complex environments. 5 | 6 | 7 |

8 | 9 |

10 | 11 | # Table of Contents 12 | **Credits** 13 | * [Paper](#Paper) 14 | * [Video](#Video) 15 | * [News](#News) 16 | 17 | **Setup** 18 | * [Installation](#Installation) 19 | * [Datasets](#Datasets) 20 | 21 | **Examples** 22 | - [Running a DOALS sequence](#Running-a-DOALS-sequence) 23 | - [Running a Dynablox sequence](#Running-a-Dynablox-sequence) 24 | - [Running and Evaluating an Experiment](#Evaluating-an-Experiment) 25 | 26 | # Paper 27 | If you find this package useful for your research, please consider citing our paper: 28 | 29 | * Lukas Schmid, Olov Andersson, Aurelio Sulser, Patrick Pfreundschuh, and Roland Siegwart. "**Dynablox: Real-time Detection of Diverse Dynamic Objects in Complex Environments**" in *IEEE Robotics and Automation Letters (RA-L)*, Vol. 8, No. 10, pp. 6259 - 6266, October 2023. \[ [IEEE](https://ieeexplore.ieee.org/document/10218983) | [ArXiv](https://arxiv.org/abs/2304.10049) | [Video](https://www.youtube.com/watch?v=NA13fRWlqRQ) \] 30 | ```bibtex 31 | @article{schmid2023dynablox, 32 | title={Dynablox: Real-time Detection of Diverse Dynamic Objects in Complex Environments}, 33 | author={Schmid, Lukas, and Andersson, Olov, and Sulser, Aurelio, and Pfreundschuh, Patrick, and Siegwart, Roland}, 34 | booktitle={IEEE Robotics and Automation Letters (RA-L)}, 35 | year={2023}, 36 | volume={8}, 37 | number={10}, 38 | pages={6259 - 6266}, 39 | doi={10.1109/LRA.2023.3305239}} 40 | } 41 | ``` 42 | 43 | ## Video 44 | A brief overview of the problem, approach, and results is available on youtube: 45 | [Dynablox Youtube Video](https://www.youtube.com/watch?v=NA13fRWlqRQ) 46 | 47 | ## News 48 | We were excited to learn that Dynablox has been integrated into NVIDIA's [nvblox](https://github.com/nvidia-isaac/nvblox), where the algorithm's parallelism can make fantastic use of the GPU and detect moving objects fast and at high resolutions! 49 | 50 | # Setup 51 | 52 | There is a [docker image](https://hub.docker.com/repository/docker/zhangkin/dynablox/general) available for this package. Check the usage in the dockerhub page. 53 | 54 | ## Installation 55 | 56 | * **Note on Versioning:** This package was developed using Ubuntu 20.04 using ROS Noetic. Other versions should also work but support can not be guaranteed. 57 | 58 | 1. If not already done so, install [ROS](http://wiki.ros.org/action/fullsearch/melodic/Installation/Ubuntu?action=fullsearch&context=180&value=linkto%3A%22melodic%2FInstallation%2FUbuntu%22). We recommend using `Desktop-Full`. 59 | 60 | 2. If not already done so, setup a catkin workspace: 61 | ```bash 62 | mkdir -p ~/catkin_ws/src 63 | cd ~/catkin_ws 64 | catkin init 65 | catkin config --extend /opt/ros/$ROS_DISTRO 66 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo 67 | catkin config --merge-devel 68 | ``` 69 | 70 | 2. Install system dependencies: 71 | ```bash 72 | sudo apt-get install python3-vcstool python3-catkin-tools ros-$ROS_DISTRO-cmake-modules protobuf-compiler autoconf git rsync -y 73 | ``` 74 | 75 | 3. Clone the repo using [SSH Keys](https://docs.github.com/en/authentication/connecting-to-github-with-ssh): 76 | ```bash 77 | cd ~/catkin_ws/src 78 | git clone git@github.com:ethz-asl/dynablox.git 79 | ``` 80 | 81 | 4. Install ROS dependencies: 82 | ```bash 83 | cd ~/catkin_ws/src 84 | vcs import . < ./dynablox/ssh.rosinstall --recursive 85 | ``` 86 | 87 | 5. Build: 88 | ```bash 89 | catkin build dynablox_ros 90 | ``` 91 | 92 | ## Datasets 93 | To run the demos we use the [Urban Dynamic Objects LiDAR (DOALS) Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=doals). 94 | To download the data and pre-process it for our demos, use the provided script: 95 | ```bash 96 | roscd dynablox_ros/scripts 97 | ./download_doals_data.sh /home/$USER/data/DOALS # Or your preferred data destination. 98 | ``` 99 | 100 | We further collect a new dataset featuring diverse dynamic objects in complex scenes. 101 | The full dataset and description can be found [here](https://projects.asl.ethz.ch/datasets/doku.php?id=dynablox). 102 | To download the processed ready-to-run data for our demos, use the provided script: 103 | ```bash 104 | roscd dynablox_ros/scripts 105 | ./download_dynablox_data.sh /home/$USER/data/Dynablox # Or your preferred data destination. 106 | 107 | ``` 108 | 109 | # Examples 110 | ## Running a DOALS Sequence 111 | 1. If not done so, download the DOALS dataset as explained [here](#datasets). 112 | 113 | 2. Adjust the dataset path in `dynablox_ros/launch/run_experiment.launch`: 114 | ```xml 115 | 116 | ``` 117 | 3. Run 118 | ```bash 119 | roslaunch dynablox_ros run_experiment.launch 120 | ``` 121 | 4. You should now see dynamic objects being detected as the sensor moves through the scene: 122 | 123 | ![Run DOALS Example](https://user-images.githubusercontent.com/36043993/232138501-84250c43-236e-46f6-9b50-af54312215a7.png) 124 | 125 | ## Running a Dynablox Sequence 126 | 127 | 1. If not done so, download the Dynablox dataset as explained [here](#datasets). 128 | 129 | 2. Adjust the dataset path in `dynablox_ros/launch/run_experiment.launch` and set `use_doals` to false: 130 | ```xml 131 | 132 | 133 | ``` 134 | 3. Run 135 | ```bash 136 | roslaunch dynablox_ros run_experiment.launch 137 | ``` 138 | 4. You should now see dynamic objects being detected as the sensor moves through the scene: 139 | ![Run Dynablox Example](https://user-images.githubusercontent.com/36043993/232140093-ee99a919-d2ad-4dc8-95ac-fa047b901f94.png) 140 | 141 | ## Running and Evaluating an Experiment 142 | 143 | ### Running an Experiment 144 | 145 | 1. If not done so, download the DOALS dataset as explained [here](#datasets). 146 | 147 | 2. Adjust the dataset path in `dynablox_ros/launch/run_experiment.launch`: 148 | ```xml 149 | 150 | ``` 151 | 152 | 3. In `dynablox_ros/launch/run_experiment.launch`, set the `evaluate` flag, adjust the ground truth data path, and specify where to store the generated outpuit data: 153 | ```xml 154 | 155 | 156 | 157 | ``` 158 | 3. Run 159 | ```bash 160 | roslaunch dynablox_ros run_experiment.launch 161 | ``` 162 | 163 | 4. Wait till the dataset finished processing. Dynablox should shutdown automatically afterwards. 164 | 165 | ### Analyzing the Data 166 | - **Printing the Detection Performance Metrics:** 167 | 1. Run: 168 | ```bash 169 | roscd dynablox_ros/src/evaluation 170 | python3 evaluate_data.py /home/$USER/dynablox_output 171 | ``` 172 | 2. You should now see the performance statistics for all experiments in that folder: 173 | ``` 174 | 1/1 data entries are complete. 175 | Data object_IoU object_Precision object_Recall 176 | hauptgebaeude_1 89.8 +- 5.6 99.3 +- 0.4 90.3 +- 5.6 177 | All 89.8 +- 5.6 99.3 +- 0.4 90.3 +- 5.6 178 | ``` 179 | 180 | - **Inspecting the Segmentation:** 181 | 1. Run: 182 | ```bash 183 | roslaunch dynablox_ros cloud_visualizer.launch file_path:=/home/$USER/dynablox_output/clouds.csv 184 | ``` 185 | 2. You should now see the segmentation for the annotated ground truth clouds, showing True Positives (green), True Negatives (black), False Positives (blue), False Negatives (red), and out-of-range (gray) points: 186 | ![Evaluation](https://user-images.githubusercontent.com/36043993/232151598-750a6860-e6e6-44bc-89c6-fbc866109019.png) 187 | 188 | - **Inspecting the Run-time and Configuration:** 189 | Additional information is automatically stored in `timings.txt` and `config.txt` for each experiment. 190 | 191 | ### Advanced Options 192 | * **Adding Drift to an Experiment:** 193 | To run an experiment with drift specify one of the pre-computed drift rollouts in `dynablox_ros/launch/run_experiment.launch`: 194 | ```xml 195 | 196 | ``` 197 | All pre-computed rollouts can be found in `drift_simulation/config/rollouts`. Note that the specified sequence needs to match the data being played. For each sequence, there exist 3 rollouts for each intensity. 198 | 199 | Alternatively, use the `drift_simulation/launch/generate_drift_rollout.launch` to create new rollouts for other datasets. 200 | 201 | * **Changing th Configuration of Dynablox:** 202 | All parameters that exist in dynablox are listed in `dynablox_ros/config/motion_detector/default.yaml`, feel free to tune the method for your use case! 203 | 204 | -------------------------------------------------------------------------------- /drift_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(drift_simulation) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | catkin_package() 7 | add_definitions(-std=c++17 -Wall -Wextra) 8 | 9 | cs_add_library(${PROJECT_NAME} 10 | src/normal_distribution.cpp 11 | src/odometry_drift_simulator.cpp 12 | src/drift_reader.cpp 13 | ) 14 | 15 | cs_add_executable(odometry_drift_simulator 16 | src/odometry_drift_simulator_node.cpp 17 | ) 18 | target_link_libraries(odometry_drift_simulator ${PROJECT_NAME}) 19 | 20 | cs_add_executable(drift_reader 21 | src/drift_reader_node.cpp 22 | ) 23 | target_link_libraries(drift_reader ${PROJECT_NAME}) 24 | 25 | cs_install() 26 | cs_export() 27 | -------------------------------------------------------------------------------- /drift_simulation/config/intensities/light.yaml: -------------------------------------------------------------------------------- 1 | # Drift odometry simulator 2 | publish_ground_truth_pose: true 3 | ground_truth_frame_suffix: "_ground_truth" 4 | velocity_noise_frequency_hz: 0.1 5 | velocity_noise: 6 | x: 7 | mean: 0.0 8 | stddev: 0.00375 9 | y: 10 | mean: 0.0 11 | stddev: 0.00375 12 | z: 13 | mean: 0.0 14 | stddev: 0.00125 15 | yaw: 16 | mean: 0.0 17 | stddev: 0.0 ##0.000875 18 | 19 | 20 | # norm of one stddev: about 0.00545 -> 360 frames 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /drift_simulation/config/intensities/moderate.yaml: -------------------------------------------------------------------------------- 1 | # Drift odometry simulator 2 | publish_ground_truth_pose: true 3 | ground_truth_frame_suffix: "_ground_truth" 4 | velocity_noise_frequency_hz: 0.1 5 | velocity_noise: 6 | x: 7 | mean: 0.0 8 | stddev: 0.0075 9 | y: 10 | mean: 0.0 11 | stddev: 0.0075 12 | z: 13 | mean: 0.0 14 | stddev: 0.0025 15 | yaw: 16 | mean: 0.0 17 | stddev: 0.0 #0.00175 18 | 19 | 20 | # norm of one stddev: about 0.0109 -> 180 frames 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /drift_simulation/config/intensities/none.yaml: -------------------------------------------------------------------------------- 1 | # Drift odometry simulator 2 | publish_ground_truth_pose: true 3 | ground_truth_frame_suffix: "_ground_truth" 4 | velocity_noise_frequency_hz: 0.1 5 | velocity_noise: 6 | x: 7 | mean: 0.0 8 | stddev: 0.0 9 | y: 10 | mean: 0.0 11 | stddev: 0.0 12 | z: 13 | mean: 0.0 14 | stddev: 0.0 15 | yaw: 16 | mean: 0.0 17 | stddev: 0.0 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /drift_simulation/config/intensities/severe.yaml: -------------------------------------------------------------------------------- 1 | # Drift odometry simulator 2 | publish_ground_truth_pose: true 3 | ground_truth_frame_suffix: "_ground_truth" 4 | velocity_noise_frequency_hz: 0.1 5 | velocity_noise: 6 | x: 7 | mean: 0.0 8 | stddev: 0.03 9 | y: 10 | mean: 0.0 11 | stddev: 0.03 12 | z: 13 | mean: 0.0 14 | stddev: 0.01 15 | yaw: 16 | mean: 0.0 17 | stddev: 0.0 #0.007 18 | 19 | 20 | # norm of one stddev: about 0.04359 -> 45 frames 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /drift_simulation/config/intensities/strong.yaml: -------------------------------------------------------------------------------- 1 | # Drift odometry simulator 2 | publish_ground_truth_pose: true 3 | ground_truth_frame_suffix: "_ground_truth" 4 | velocity_noise_frequency_hz: 0.1 5 | velocity_noise: 6 | x: 7 | mean: 0.0 8 | stddev: 0.015 9 | y: 10 | mean: 0.0 11 | stddev: 0.015 12 | z: 13 | mean: 0.0 14 | stddev: 0.005 15 | yaw: 16 | mean: 0.0 17 | stddev: 0.0 #0.0035 18 | 19 | # norm of one stddev: about 0.0218 -> 90 frames 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /drift_simulation/include/drift_simulation/drift_reader.h: -------------------------------------------------------------------------------- 1 | #ifndef DRIFT_SIMULATION_DRIFT_READER_H_ 2 | #define DRIFT_SIMULATION_DRIFT_READER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class DriftReader { 15 | public: 16 | DriftReader(ros::NodeHandle nh, ros::NodeHandle nh_private); 17 | 18 | void cloudCallback(const sensor_msgs::PointCloud2::Ptr& pointcloud_msg); 19 | 20 | private: 21 | // ROS. 22 | ros::NodeHandle nh_; 23 | ros::NodeHandle nh_private_; 24 | ros::Subscriber pointcloud_sub_; 25 | ros::Publisher pointcloud_pub_; 26 | 27 | // Config. 28 | std::string drift_data_file_name_; 29 | std::string global_frame_name_; 30 | std::string drifted_sensor_frame_name_; 31 | 32 | // Data. 33 | size_t frame_counter_ = 0u; 34 | std::vector vector_of_transformations_; 35 | 36 | // TF transforms. 37 | tf2_ros::TransformBroadcaster tf_broadcaster_; 38 | 39 | // Variables. 40 | bool use_drift_ = true; 41 | }; 42 | 43 | #endif // DRIFT_SIMULATION_DRIFT_READER_H_ 44 | -------------------------------------------------------------------------------- /drift_simulation/include/drift_simulation/normal_distribution.h: -------------------------------------------------------------------------------- 1 | #ifndef DRIFT_SIMULATION_NORMAL_DISTRIBUTION_H_ 2 | #define DRIFT_SIMULATION_NORMAL_DISTRIBUTION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace unreal_airsim { 12 | class NormalDistribution { 13 | public: 14 | struct Config { 15 | // Initialize from ROS params 16 | static Config fromRosParams(const ros::NodeHandle& nh); 17 | 18 | // Distribution parameters 19 | double mean = 0.0; 20 | double stddev = 0.0; 21 | 22 | // Validity queries and assertions 23 | bool isValid(const std::string& error_msg_prefix = "") const; 24 | Config& checkValid() { 25 | CHECK(isValid()); 26 | return *this; 27 | } 28 | 29 | // Write config values to stream, e.g. for logging 30 | friend std::ostream& operator<<(std::ostream& os, const Config& config); 31 | }; 32 | 33 | explicit NormalDistribution(double mean = 0.0, double stddev = 1.0) 34 | : mean_(mean), stddev_(stddev) { 35 | CHECK_GE(stddev_, 0.0) << "Standard deviation must be non-negative"; 36 | } 37 | explicit NormalDistribution(const Config& config) 38 | : NormalDistribution(config.mean, config.stddev) {} 39 | 40 | double getMean() const { return mean_; } 41 | double getStddev() const { return stddev_; } 42 | 43 | // Return a sample from the normal distribution N(mean_, stddev_) 44 | double operator()() { 45 | CHECK_GE(stddev_, 0) << "The standard deviation must be non-negative."; 46 | 47 | typedef std::chrono::high_resolution_clock myclock; 48 | myclock::time_point beginning = myclock::now(); 49 | 50 | // Random engine 51 | // TODO(victorr): Add proper random seed handling (and option to provide it) 52 | // NOTE: The noise generator is static to ensure that all instances draw 53 | // subsequent (different) numbers from the same pseudo random 54 | // sequence. If the generator is instance specific, there's a risk 55 | // that multiple instances use generators with the same seed and 56 | // output the same sequence. 57 | 58 | static std::mt19937 noise_generator_; 59 | 60 | myclock::duration d = myclock::now() - beginning; 61 | unsigned seed1 = d.count(); 62 | 63 | noise_generator_.seed(seed1); 64 | 65 | // Draw a sample from the standard normal N(0,1) and 66 | // scale it using the change of variables formula 67 | return normal_distribution_(noise_generator_) * stddev_ + mean_; 68 | } 69 | 70 | private: 71 | const double mean_, stddev_; 72 | 73 | // Standard normal distribution 74 | std::normal_distribution normal_distribution_; 75 | }; 76 | } // namespace unreal_airsim 77 | 78 | #endif // DRIFT_SIMULATION_NORMAL_DISTRIBUTION_H_ 79 | -------------------------------------------------------------------------------- /drift_simulation/include/drift_simulation/odometry_drift_simulator.h: -------------------------------------------------------------------------------- 1 | #ifndef DRIFT_SIMULATION_ODOMETRY_DRIFT_SIMULATOR_H_ 2 | #define DRIFT_SIMULATION_ODOMETRY_DRIFT_SIMULATOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace unreal_airsim { 17 | class OdometryDriftSimulator { 18 | public: 19 | using NoiseDistribution = NormalDistribution; 20 | using Transformation = kindr::minimal::QuatTransformationTemplate; 21 | 22 | struct Config { 23 | // Initialize from ROS params 24 | static Config fromRosParams(const ros::NodeHandle& nh); 25 | 26 | // Whether and how to publish the ground truth pose 27 | bool publish_ground_truth_pose = false; 28 | std::string ground_truth_frame_suffix = "_ground_truth"; 29 | 30 | // Params of the distributions used to generate the pose drift and noise 31 | float velocity_noise_frequency_hz = 1; 32 | using NoiseConfigMap = std::map; 33 | NoiseConfigMap velocity_noise = { 34 | {"x", {}}, {"y", {}}, {"z", {}}, {"yaw", {}}}; 35 | NoiseConfigMap pose_noise = {{"x", {}}, {"y", {}}, {"z", {}}, 36 | {"yaw", {}}, {"pitch", {}}, {"roll", {}}}; 37 | 38 | // Validity queries and assertions 39 | bool isValid() const; 40 | Config& checkValid() { 41 | CHECK(isValid()); 42 | return *this; 43 | } 44 | 45 | // Write config values to stream, e.g. for logging 46 | friend std::ostream& operator<<(std::ostream& os, const Config& config); 47 | }; 48 | 49 | explicit OdometryDriftSimulator(Config config, ros::NodeHandle nh, 50 | const ros::NodeHandle& nh_private); 51 | ~OdometryDriftSimulator() = default; 52 | 53 | void start() { 54 | reset(); 55 | started_publishing_ = true; 56 | } 57 | void poseCallback(const sensor_msgs::PointCloud2& pointcloud_msg); 58 | void reset(); 59 | void tick(const geometry_msgs::TransformStamped& ground_truth_pose_msg); 60 | 61 | Transformation getSimulatedPose() const { return current_simulated_pose_; } 62 | Transformation getGroundTruthPose() const; 63 | geometry_msgs::TransformStamped getSimulatedPoseMsg() const; 64 | geometry_msgs::TransformStamped getGroundTruthPoseMsg() const { 65 | return last_ground_truth_pose_msg_; 66 | } 67 | 68 | Transformation convertDriftedToGroundTruthPose( 69 | const Transformation& simulated_pose) const; 70 | Transformation convertGroundTruthToDriftedPose( 71 | const Transformation& ground_truth_pose) const; 72 | geometry_msgs::TransformStamped convertDriftedToGroundTruthPoseMsg( 73 | const geometry_msgs::TransformStamped& simulated_pose_msg) const; 74 | geometry_msgs::TransformStamped convertGroundTruthToDriftedPoseMsg( 75 | const geometry_msgs::TransformStamped& ground_truth_pose_msg) const; 76 | 77 | private: 78 | const Config config_; 79 | 80 | // ROS. 81 | ros::NodeHandle nh_private_; 82 | ros::Subscriber pointcloud_sub_; 83 | 84 | // Hacked variables to store the drift. 85 | std::string output_drifted_file_name_; 86 | std::string output_gt_file_name_; 87 | std::string global_frame_name_ = "map"; 88 | std::string sensor_frame_name_ = "os1_lidar"; 89 | 90 | // TF transforms 91 | tf::TransformListener tf_listener_; 92 | 93 | // Settings 94 | bool started_publishing_; 95 | 96 | // Simulator state 97 | ros::Time last_velocity_noise_sampling_time_; 98 | const ros::Duration velocity_noise_sampling_period_; 99 | Transformation::Vector3 current_linear_velocity_noise_sample_W_; 100 | Transformation::Vector3 current_angular_velocity_noise_sample_W_; 101 | Transformation integrated_pose_drift_; 102 | Transformation current_pose_noise_; 103 | Transformation current_simulated_pose_; 104 | geometry_msgs::TransformStamped last_ground_truth_pose_msg_; 105 | 106 | // Noise distributions 107 | struct VelocityNoiseDistributions { 108 | explicit VelocityNoiseDistributions( 109 | const Config::NoiseConfigMap& velocity_noise_configs); 110 | NoiseDistribution x, y, z; 111 | NoiseDistribution yaw; 112 | } velocity_noise_; 113 | struct PoseNoiseDistributions { 114 | explicit PoseNoiseDistributions( 115 | const Config::NoiseConfigMap& pose_noise_configs); 116 | NoiseDistribution x, y, z; 117 | NoiseDistribution yaw, pitch, roll; 118 | } pose_noise_; 119 | }; 120 | } // namespace unreal_airsim 121 | 122 | #endif // DRIFT_SIMULATION_ODOMETRY_DRIFT_SIMULATOR_H_ 123 | -------------------------------------------------------------------------------- /drift_simulation/launch/generate_drift_rollout.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /drift_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | drift_simulation 4 | 0.0.0 5 | Reproducible Odometry Drift Simulation 6 | 7 | Lukas Schmid 8 | Aurelio Sulser 9 | Olov Andersson 10 | 11 | Lukas Schmid 12 | Aurelio Sulser 13 | Olov Andersson 14 | 15 | BSD 16 | 17 | catkin 18 | catkin_simple 19 | 20 | roscpp 21 | rospy 22 | std_msgs 23 | sensor_msgs 24 | geometry_msgs 25 | visualization_msgs 26 | pcl_ros 27 | pcl_conversions 28 | tf2 29 | tf2_ros 30 | tf2_geometry_msgs 31 | minkindr 32 | minkindr_conversions 33 | 34 | -------------------------------------------------------------------------------- /drift_simulation/scripts/create_drift_rollouts.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Functions. 4 | function run_experiments() { 5 | # Run all datasets 6 | for scene in "${scenes[@]}" 7 | do 8 | for seq in "${sequences[@]}" 9 | do 10 | # Make sure path exists. 11 | output=$output_path/doals/$scene/sequence_$seq 12 | if [ -d "$output" ]; then 13 | echo "Directory '$output' exists." 14 | else 15 | echo "Directory '$output' did not exist, was created." 16 | mkdir -p $output 17 | fi 18 | for int in "${drift_intensities[@]}" 19 | do 20 | local_rollouts=("${drift_rollouts[@]}") 21 | if [ "$int" == "none" ]; then 22 | local_rollouts=(1) 23 | rollout="doals/$scene/sequence_$seq/none.csv" 24 | fi 25 | 26 | for r in "${local_rollouts[@]}" 27 | do 28 | # Configure drift rollout. 29 | if [ "$int" != "none" ]; then 30 | rollout="doals/$scene/sequence_$seq/${int}_$r.csv" 31 | fi 32 | 33 | # Run experiment. 34 | echo "Started running experiment: $scene, sequence_$seq, drift: $int, rollout: $r." 35 | roslaunch drift_simulation generate_drift_rollout.launch bag_file:=$data_path/$scene/sequence_$seq/bag.bag drift_intensity:=$int output_drifted_file_name:=$output_path/$rollout player_rate:=$player_rate 36 | echo "Finished running experiment: $scene, sequence_$seq, drift: $int, rollout: $r." 37 | done 38 | done 39 | done 40 | done 41 | } 42 | 43 | 44 | 45 | # General params. 46 | data_path="/media/lukas/T7/Datasets/DOALS" 47 | player_rate="3" 48 | 49 | # Data to run. 50 | scenes=(niederdorf shopville station hauptgebaeude) 51 | sequences=(1 2) 52 | drift_intensities=(moderate) # none light moderate strong severe 53 | drift_rollouts=(1 2 3) 54 | output_path="/home/lukas/motion_ws/src/dynablox/drift_simulation/config/rollouts" 55 | 56 | # ====== Run Experiments ====== 57 | run_experiments 58 | -------------------------------------------------------------------------------- /drift_simulation/src/drift_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "drift_simulation/drift_reader.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | DriftReader::DriftReader(ros::NodeHandle nh, ros::NodeHandle nh_private) 17 | : nh_(std::move(nh)), nh_private_(std::move(nh_private)) { 18 | // Params. 19 | nh_private.param("drift_data_file_name", drift_data_file_name_, 20 | ""); 21 | nh_private.param("global_frame_name", global_frame_name_, "map"); 22 | nh_private.param("drifted_sensor_frame_name", 23 | drifted_sensor_frame_name_, "os1_drifted"); 24 | 25 | // Read drift data. 26 | if (drift_data_file_name_.empty()) { 27 | // No drift requested. 28 | LOG(WARNING) << "No drift data file was requested. No drift will be added."; 29 | use_drift_ = false; 30 | } else if (!std::filesystem::exists(drift_data_file_name_)) { 31 | // File specified but does not exist. 32 | LOG(WARNING) << "The specified drift data '" << drift_data_file_name_ 33 | << "' does not exist! No drift will be added."; 34 | use_drift_ = false; 35 | } else { 36 | // Read the drift values. 37 | std::string line; 38 | std::fstream fin; 39 | fin.open(drift_data_file_name_, std::ios::in); 40 | 41 | while (!fin.eof()) { 42 | getline(fin, line); 43 | vector_of_transformations_.push_back(line); 44 | } 45 | LOG(INFO) << "Read " << vector_of_transformations_.size() 46 | << " drifted poses from '" << drift_data_file_name_ << "'."; 47 | } 48 | 49 | // Subscribe to the undistorted pointcloud topic 50 | pointcloud_sub_ = 51 | nh_.subscribe("pointcloud", 100, &DriftReader::cloudCallback, this); 52 | pointcloud_pub_ = 53 | nh_.advertise("pointcloud_drifted", 10); 54 | } 55 | 56 | void DriftReader::cloudCallback( 57 | const sensor_msgs::PointCloud2::Ptr& pointcloud_msg) { 58 | if (!use_drift_) { 59 | // Simply forward the cloud. 60 | pointcloud_pub_.publish(pointcloud_msg); 61 | return; 62 | } 63 | 64 | // Check data available. 65 | if (frame_counter_ >= vector_of_transformations_.size()) { 66 | // Out of data. 67 | LOG(WARNING) << "No more drift values available at index " << frame_counter_ 68 | << ", no more drift will be applied."; 69 | ros::shutdown(); 70 | return; 71 | } 72 | 73 | // Read drift data. 74 | std::vector pose_data; 75 | std::string word; 76 | std::stringstream s(vector_of_transformations_[frame_counter_]); 77 | while (getline(s, word, ',')) { 78 | pose_data.push_back(std::stod(word)); 79 | } 80 | 81 | // Broadcast transform. 82 | geometry_msgs::TransformStamped transform; 83 | transform.header.stamp = pointcloud_msg->header.stamp; 84 | transform.header.frame_id = drifted_sensor_frame_name_; 85 | transform.child_frame_id = global_frame_name_; 86 | transform.transform.translation.x = pose_data[0]; 87 | transform.transform.translation.y = pose_data[1]; 88 | transform.transform.translation.z = pose_data[2]; 89 | transform.transform.rotation.x = pose_data[3]; 90 | transform.transform.rotation.y = pose_data[4]; 91 | transform.transform.rotation.z = pose_data[5]; 92 | transform.transform.rotation.w = pose_data[6]; 93 | tf_broadcaster_.sendTransform(transform); 94 | 95 | // Send pointcloud. 96 | pointcloud_msg->header.frame_id = drifted_sensor_frame_name_; 97 | pointcloud_pub_.publish(pointcloud_msg); 98 | 99 | frame_counter_ += 1; 100 | } 101 | -------------------------------------------------------------------------------- /drift_simulation/src/drift_reader_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "drift_simulation/drift_reader.h" 4 | 5 | int main(int argc, char** argv) { 6 | // Register with ROS master 7 | ros::init(argc, argv, "drift_reader"); 8 | 9 | // Create node handles 10 | ros::NodeHandle nh; 11 | ros::NodeHandle nh_private("~"); 12 | 13 | // Launch the drift simulation 14 | DriftReader drift_reader(nh, nh_private); 15 | 16 | // Spin 17 | ros::spin(); 18 | 19 | // Exit normally 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /drift_simulation/src/normal_distribution.cpp: -------------------------------------------------------------------------------- 1 | #include "drift_simulation/normal_distribution.h" 2 | 3 | #include 4 | 5 | namespace unreal_airsim { 6 | NormalDistribution::Config NormalDistribution::Config::fromRosParams( 7 | const ros::NodeHandle& nh) { 8 | Config config; 9 | nh.param("mean", config.mean, config.mean); 10 | nh.param("stddev", config.stddev, config.stddev); 11 | return config; 12 | } 13 | 14 | bool NormalDistribution::Config::isValid( 15 | const std::string& error_msg_prefix) const { 16 | if (stddev < 0.0) { 17 | LOG_IF(WARNING, !error_msg_prefix.empty()) 18 | << "The " << error_msg_prefix 19 | << "/stddev should be a non-negative float"; 20 | return false; 21 | } 22 | return true; 23 | } 24 | 25 | std::ostream& operator<<(std::ostream& os, 26 | const NormalDistribution::Config& config) { 27 | return os << "mean: " << config.mean << ", stddev: " << config.stddev; 28 | } 29 | } // namespace unreal_airsim 30 | -------------------------------------------------------------------------------- /drift_simulation/src/odometry_drift_simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "drift_simulation/odometry_drift_simulator.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace unreal_airsim { 11 | OdometryDriftSimulator::OdometryDriftSimulator( 12 | Config config, ros::NodeHandle nh, const ros::NodeHandle& nh_private) 13 | : config_(config.checkValid()), 14 | nh_private_(nh_private), 15 | started_publishing_(false), 16 | velocity_noise_sampling_period_(1.f / config.velocity_noise_frequency_hz), 17 | velocity_noise_(config.velocity_noise), 18 | pose_noise_(config.pose_noise) { 19 | // Get path to write drift values. 20 | nh_private_.param("output_drifted_file_name", 21 | output_drifted_file_name_, ""); 22 | nh_private_.param("output_gt_file_name", output_gt_file_name_, 23 | ""); 24 | nh_private_.param("global_frame_name", global_frame_name_, 25 | global_frame_name_); 26 | nh_private_.param("sensor_frame_name", sensor_frame_name_, 27 | sensor_frame_name_); 28 | 29 | // Ensure params are set. 30 | if (output_drifted_file_name_.empty()) { 31 | LOG(FATAL) << "Parameter 'output_drifted_file_name' must be set!"; 32 | } 33 | if (output_gt_file_name_.empty()) { 34 | LOG(WARNING) << "Parameter 'output_gt_file_name' is not set, not writing " 35 | "the ground truth values."; 36 | } 37 | 38 | reset(); 39 | 40 | pointcloud_sub_ = nh.subscribe("pointcloud", 100, 41 | &OdometryDriftSimulator::poseCallback, this); 42 | LOG(INFO) << "Initialized drifting odometry simulator, with config:\n" 43 | << config_; 44 | } 45 | 46 | void OdometryDriftSimulator::poseCallback( 47 | const sensor_msgs::PointCloud2& pointcloud_msg) { 48 | tf::StampedTransform transform; 49 | const int max_tries = 1000; 50 | int tries = 0; 51 | std::string except; 52 | while (tries < max_tries) { 53 | try { 54 | tf_listener_.lookupTransform(sensor_frame_name_, global_frame_name_, 55 | pointcloud_msg.header.stamp, transform); 56 | break; 57 | } catch (tf::TransformException& ex) { 58 | tries++; 59 | except = ex.what(); 60 | std::this_thread::sleep_for(std::chrono::milliseconds(5)); 61 | } 62 | } 63 | if (tries == max_tries) { 64 | LOG(FATAL) << "Failed to get transform: " << except; 65 | } 66 | geometry_msgs::TransformStamped ground_truth_pose_msg; 67 | ground_truth_pose_msg.header.stamp = transform.stamp_; 68 | tf::transformTFToMsg(transform, ground_truth_pose_msg.transform); 69 | 70 | this->tick(ground_truth_pose_msg); 71 | 72 | geometry_msgs::TransformStamped simulated_pose_msg; 73 | simulated_pose_msg = 74 | this->convertGroundTruthToDriftedPoseMsg(ground_truth_pose_msg); 75 | 76 | std::fstream fout, fout_truth; 77 | 78 | // Write the data. 79 | fout.open(output_drifted_file_name_, std::ios::out | std::ios::app); 80 | fout << simulated_pose_msg.transform.translation.x << ", " 81 | << simulated_pose_msg.transform.translation.y << ", " 82 | << simulated_pose_msg.transform.translation.z << ", " 83 | << simulated_pose_msg.transform.rotation.x << ", " 84 | << simulated_pose_msg.transform.rotation.y << ", " 85 | << simulated_pose_msg.transform.rotation.z << ", " 86 | << simulated_pose_msg.transform.rotation.w << std::endl; 87 | fout.close(); 88 | LOG_FIRST_N(INFO, 1) << "Writing drift data to '" << output_drifted_file_name_ 89 | << "'."; 90 | 91 | if (!output_gt_file_name_.empty()) { 92 | fout_truth.open(output_gt_file_name_, std::ios::out | std::ios::app); 93 | fout_truth << ground_truth_pose_msg.transform.translation.x << ", " 94 | << ground_truth_pose_msg.transform.translation.y << ", " 95 | << ground_truth_pose_msg.transform.translation.z << ", " 96 | << ground_truth_pose_msg.transform.rotation.x << ", " 97 | << ground_truth_pose_msg.transform.rotation.y << ", " 98 | << ground_truth_pose_msg.transform.rotation.z << ", " 99 | << ground_truth_pose_msg.transform.rotation.w << std::endl; 100 | fout_truth.close(); 101 | LOG_FIRST_N(INFO, 1) << "Writing gt data to '" << output_drifted_file_name_ 102 | << "'."; 103 | } 104 | } 105 | 106 | void OdometryDriftSimulator::reset() { 107 | last_velocity_noise_sampling_time_ = ros::Time(); 108 | current_linear_velocity_noise_sample_W_.setZero(); 109 | current_angular_velocity_noise_sample_W_.setZero(); 110 | integrated_pose_drift_.setIdentity(); 111 | current_pose_noise_.setIdentity(); 112 | current_simulated_pose_.setIdentity(); 113 | last_ground_truth_pose_msg_ = geometry_msgs::TransformStamped(); 114 | } 115 | 116 | void OdometryDriftSimulator::tick( 117 | const geometry_msgs::TransformStamped& ground_truth_pose_msg) { 118 | // Compute the time delta 119 | const ros::Time& current_timestamp = ground_truth_pose_msg.header.stamp; 120 | double delta_t = 0.0; 121 | if (!last_ground_truth_pose_msg_.header.stamp.isZero()) { 122 | delta_t = 123 | (current_timestamp - last_ground_truth_pose_msg_.header.stamp).toSec(); 124 | } 125 | last_ground_truth_pose_msg_ = ground_truth_pose_msg; 126 | if (delta_t < 0.0) { 127 | LOG(WARNING) << "Time difference between current and last received pose " 128 | "msg is negative. Skipping."; 129 | return; 130 | } 131 | 132 | // Get the ground truth pose 133 | Transformation ground_truth_pose; 134 | tf::transformMsgToKindr(ground_truth_pose_msg.transform, &ground_truth_pose); 135 | 136 | // Draw a random velocity noise sample, used to simulate drift 137 | if (last_velocity_noise_sampling_time_ + velocity_noise_sampling_period_ < 138 | current_timestamp) { 139 | last_velocity_noise_sampling_time_ = current_timestamp; 140 | 141 | // Sample the linear velocity noise in body frame 142 | const Transformation::Vector3 current_linear_velocity_noise_sample_B_ = { 143 | velocity_noise_.x(), velocity_noise_.y(), velocity_noise_.z()}; 144 | current_linear_velocity_noise_sample_W_ = 145 | ground_truth_pose.getRotation().rotate( 146 | current_linear_velocity_noise_sample_B_); 147 | 148 | // Sample the angular velocity noise directly in world frame, 149 | // since we only want to simulate drift on world frame yaw 150 | current_angular_velocity_noise_sample_W_ = {0.0, 0.0, 151 | velocity_noise_.yaw()}; 152 | } 153 | 154 | // Integrate the drift 155 | Transformation::Vector6 drift_delta_W_vec; 156 | drift_delta_W_vec << current_linear_velocity_noise_sample_W_, 157 | current_angular_velocity_noise_sample_W_; 158 | drift_delta_W_vec *= delta_t; 159 | const Transformation drift_delta_W = Transformation::exp(drift_delta_W_vec); 160 | integrated_pose_drift_ = drift_delta_W * integrated_pose_drift_; 161 | 162 | // Draw a random pose noise sample 163 | Transformation::Vector6 pose_noise_B_vec; 164 | pose_noise_B_vec << pose_noise_.x(), pose_noise_.y(), pose_noise_.z(), 165 | pose_noise_.roll(), pose_noise_.pitch(), pose_noise_.yaw(); 166 | current_pose_noise_ = Transformation::exp(pose_noise_B_vec); 167 | 168 | // Update the current simulated pose 169 | current_simulated_pose_ = 170 | integrated_pose_drift_ * ground_truth_pose * current_pose_noise_; 171 | } 172 | 173 | OdometryDriftSimulator::Transformation 174 | OdometryDriftSimulator::getGroundTruthPose() const { 175 | Transformation ground_truth_pose; 176 | tf::transformMsgToKindr(last_ground_truth_pose_msg_.transform, 177 | &ground_truth_pose); 178 | return ground_truth_pose; 179 | } 180 | 181 | geometry_msgs::TransformStamped OdometryDriftSimulator::getSimulatedPoseMsg() 182 | const { 183 | geometry_msgs::TransformStamped simulated_pose_msg = 184 | last_ground_truth_pose_msg_; 185 | tf::transformKindrToMsg(current_simulated_pose_, 186 | &simulated_pose_msg.transform); 187 | return simulated_pose_msg; 188 | } 189 | 190 | OdometryDriftSimulator::Transformation 191 | OdometryDriftSimulator::convertDriftedToGroundTruthPose( 192 | const OdometryDriftSimulator::Transformation& simulated_pose) const { 193 | return integrated_pose_drift_.inverse() * simulated_pose * 194 | current_pose_noise_.inverse(); 195 | } 196 | 197 | OdometryDriftSimulator::Transformation 198 | OdometryDriftSimulator::convertGroundTruthToDriftedPose( 199 | const OdometryDriftSimulator::Transformation& ground_truth_pose) const { 200 | return integrated_pose_drift_ * ground_truth_pose * current_pose_noise_; 201 | } 202 | 203 | geometry_msgs::TransformStamped 204 | OdometryDriftSimulator::convertDriftedToGroundTruthPoseMsg( 205 | const geometry_msgs::TransformStamped& simulated_pose_msg) const { 206 | Transformation simulated_pose; 207 | tf::transformMsgToKindr(simulated_pose_msg.transform, &simulated_pose); 208 | 209 | const Transformation ground_truth_pose = 210 | convertDriftedToGroundTruthPose(simulated_pose); 211 | 212 | geometry_msgs::TransformStamped ground_truth_pose_msg = simulated_pose_msg; 213 | tf::transformKindrToMsg(ground_truth_pose, &ground_truth_pose_msg.transform); 214 | return ground_truth_pose_msg; 215 | } 216 | 217 | geometry_msgs::TransformStamped 218 | OdometryDriftSimulator::convertGroundTruthToDriftedPoseMsg( 219 | const geometry_msgs::TransformStamped& ground_truth_pose_msg) const { 220 | Transformation ground_truth_pose; 221 | tf::transformMsgToKindr(ground_truth_pose_msg.transform, &ground_truth_pose); 222 | 223 | const Transformation simulated_pose = 224 | convertGroundTruthToDriftedPose(ground_truth_pose); 225 | 226 | geometry_msgs::TransformStamped simulated_pose_msg = ground_truth_pose_msg; 227 | tf::transformKindrToMsg(simulated_pose, &simulated_pose_msg.transform); 228 | return simulated_pose_msg; 229 | } 230 | 231 | OdometryDriftSimulator::VelocityNoiseDistributions::VelocityNoiseDistributions( 232 | const OdometryDriftSimulator::Config::NoiseConfigMap& 233 | velocity_noise_configs) 234 | : x(velocity_noise_configs.at("x")), 235 | y(velocity_noise_configs.at("y")), 236 | z(velocity_noise_configs.at("z")), 237 | yaw(velocity_noise_configs.at("yaw")) {} 238 | 239 | OdometryDriftSimulator::PoseNoiseDistributions::PoseNoiseDistributions( 240 | const OdometryDriftSimulator::Config::NoiseConfigMap& pose_noise_configs) 241 | : x(pose_noise_configs.at("x")), 242 | y(pose_noise_configs.at("y")), 243 | z(pose_noise_configs.at("z")), 244 | yaw(pose_noise_configs.at("yaw")), 245 | pitch(pose_noise_configs.at("pitch")), 246 | roll(pose_noise_configs.at("roll")) {} 247 | 248 | OdometryDriftSimulator::Config OdometryDriftSimulator::Config::fromRosParams( 249 | const ros::NodeHandle& nh) { 250 | Config config; 251 | 252 | nh.param("publish_ground_truth_pose", config.publish_ground_truth_pose, 253 | config.publish_ground_truth_pose); 254 | 255 | nh.param("ground_truth_frame_suffix", config.ground_truth_frame_suffix, 256 | config.ground_truth_frame_suffix); 257 | 258 | nh.param("velocity_noise_frequency_hz", config.velocity_noise_frequency_hz, 259 | config.velocity_noise_frequency_hz); 260 | 261 | for (auto& kv : config.velocity_noise) { 262 | kv.second = NormalDistribution::Config::fromRosParams( 263 | ros::NodeHandle(nh, "velocity_noise/" + kv.first)); 264 | } 265 | for (auto& kv : config.pose_noise) { 266 | kv.second = NormalDistribution::Config::fromRosParams( 267 | ros::NodeHandle(nh, "position_noise/" + kv.first)); 268 | } 269 | 270 | return config; 271 | } 272 | 273 | bool OdometryDriftSimulator::Config::isValid() const { 274 | bool is_valid = true; 275 | 276 | if (ground_truth_frame_suffix.empty()) { 277 | LOG(WARNING) 278 | << "The ground_truth_frame_suffix should be a non-empty string"; 279 | is_valid = false; 280 | } 281 | 282 | if (velocity_noise_frequency_hz <= 0.f) { 283 | LOG(WARNING) 284 | << "The velocity_noise_frequency_hz should be a positive float"; 285 | is_valid = false; 286 | } 287 | 288 | for (auto& kv : velocity_noise) { 289 | if (!kv.second.isValid("velocity_noise/" + kv.first + "/")) { 290 | is_valid = false; 291 | } 292 | } 293 | for (auto& kv : pose_noise) { 294 | if (!kv.second.isValid("position_noise/" + kv.first + "/")) { 295 | is_valid = false; 296 | } 297 | } 298 | 299 | return is_valid; 300 | } 301 | 302 | std::ostream& operator<<(std::ostream& os, 303 | const OdometryDriftSimulator::Config& config) { 304 | os << "-- publish_ground_truth_pose: " 305 | << (config.publish_ground_truth_pose ? "true" : "false") << "\n" 306 | << "-- ground_truth_frame_suffix: " << config.ground_truth_frame_suffix 307 | << "\n" 308 | << "-- velocity_noise_frequency_hz: " << config.velocity_noise_frequency_hz 309 | << "\n"; 310 | 311 | os << "-- velocity_noise/\n"; 312 | for (auto& kv : config.velocity_noise) { 313 | os << "---- " << kv.first << ": {" << kv.second << "}\n"; 314 | } 315 | os << "-- position_noise/\n"; 316 | for (auto& kv : config.pose_noise) { 317 | os << "---- " << kv.first << ": {" << kv.second << "}\n"; 318 | } 319 | 320 | return os; 321 | } 322 | } // namespace unreal_airsim 323 | -------------------------------------------------------------------------------- /drift_simulation/src/odometry_drift_simulator_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "drift_simulation/odometry_drift_simulator.h" 5 | 6 | int main(int argc, char** argv) { 7 | ros::init(argc, argv, "odometry_drift_simulator"); 8 | 9 | ros::NodeHandle nh; 10 | ros::NodeHandle nh_private("~"); 11 | 12 | unreal_airsim::OdometryDriftSimulator OdometryDriftSimulator( 13 | unreal_airsim::OdometryDriftSimulator::Config::fromRosParams(nh_private), 14 | nh, nh_private); 15 | 16 | ros::spin(); 17 | 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /dynablox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(dynablox) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | catkin_package() 7 | add_definitions(-std=c++17 -Wall -Wextra) 8 | 9 | cs_add_library(${PROJECT_NAME} 10 | src/processing/preprocessing.cpp 11 | src/processing/clustering.cpp 12 | src/processing/tracking.cpp 13 | src/processing/ever_free_integrator.cpp 14 | src/evaluation/evaluator.cpp 15 | src/evaluation/ground_truth_handler.cpp 16 | src/evaluation/io_tools.cpp 17 | ) 18 | 19 | cs_install() 20 | cs_export() 21 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/common/index_getter.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_COMMON_INDEX_GETTER_H_ 2 | #define DYNABLOX_COMMON_INDEX_GETTER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dynablox { 9 | 10 | // Thread safe index getter for parallel processing of a vector. 11 | template 12 | class IndexGetter { 13 | public: 14 | explicit IndexGetter(std::vector indices) 15 | : indices_(std::move(indices)), current_index_(0) {} 16 | bool getNextIndex(IndexT* index) { 17 | CHECK_NOTNULL(index); 18 | std::lock_guard lock(mutex_); 19 | if (current_index_ >= indices_.size()) { 20 | return false; 21 | } 22 | *index = indices_[current_index_]; 23 | current_index_++; 24 | return true; 25 | } 26 | 27 | // Resets the index getter to iterate over the same indices again. 28 | void reset() { 29 | std::lock_guard lock(mutex_); 30 | current_index_ = 0u; 31 | } 32 | 33 | private: 34 | std::mutex mutex_; 35 | std::vector indices_; 36 | size_t current_index_; 37 | }; 38 | 39 | } // namespace dynablox 40 | 41 | #endif // DYNABLOX_COMMON_INDEX_GETTER_H_ 42 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/common/neighborhood_search.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_COMMON_NEIGHBORHOOD_SEARCH_H_ 2 | #define DYNABLOX_COMMON_NEIGHBORHOOD_SEARCH_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "dynablox/common/types.h" 9 | 10 | namespace dynablox { 11 | 12 | class NeighborhoodSearch { 13 | public: 14 | explicit NeighborhoodSearch(const int connectivity) { 15 | if (connectivity == 6) { 16 | search_ = voxblox::Neighborhood< 17 | voxblox::Connectivity::kSix>::getFromBlockAndVoxelIndex; 18 | } else if (connectivity == 18) { 19 | search_ = voxblox::Neighborhood< 20 | voxblox::Connectivity::kEighteen>::getFromBlockAndVoxelIndex; 21 | } else if (connectivity == 26) { 22 | search_ = voxblox::Neighborhood< 23 | voxblox::Connectivity::kTwentySix>::getFromBlockAndVoxelIndex; 24 | } else { 25 | LOG(ERROR) << "Neighborhood Search only supports connectivities of 6, " 26 | "18, and 26 (requested: " 27 | << connectivity << ")."; 28 | } 29 | } 30 | 31 | voxblox::AlignedVector search( 32 | const BlockIndex& block_index, const VoxelIndex& voxel_index, 33 | const size_t voxels_per_side) const { 34 | voxblox::AlignedVector neighbors; 35 | search_(block_index, voxel_index, voxels_per_side, &neighbors); 36 | return neighbors; 37 | } 38 | 39 | private: 40 | std::function*)> 42 | search_; 43 | }; 44 | 45 | } // namespace dynablox 46 | 47 | #endif // DYNABLOX_COMMON_NEIGHBORHOOD_SEARCH_H_ 48 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/common/types.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_COMMON_TYPES_H_ 2 | #define DYNABLOX_COMMON_TYPES_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace dynablox { 14 | 15 | using Point = pcl::PointXYZ; 16 | using Cloud = pcl::PointCloud; 17 | 18 | using VoxelIndex = voxblox::VoxelIndex; 19 | using BlockIndex = voxblox::BlockIndex; 20 | using TsdfVoxel = voxblox::TsdfVoxel; 21 | using TsdfBlock = voxblox::Block; 22 | using TsdfLayer = voxblox::Layer; 23 | 24 | // Additional information stored for every point in the cloud. 25 | struct PointInfo { 26 | // Include this point when computing performance metrics. 27 | bool ready_for_evaluation = false; 28 | 29 | // Set to true if the point falls into a voxel labeled ever-free. 30 | bool ever_free_level_dynamic = false; 31 | 32 | // Set to true if the point belongs to a cluster labeled dynamic. 33 | bool cluster_level_dynamic = false; 34 | 35 | // Set to true if the point belongs to a tracked object. 36 | bool object_level_dynamic = false; 37 | 38 | // Distance of the point to the sensor. 39 | double distance_to_sensor = -1.0; 40 | 41 | // Ground truth label if available. 42 | bool ground_truth_dynamic = false; 43 | }; 44 | 45 | // Additional information for a point cloud. 46 | struct CloudInfo { 47 | bool has_labels = false; 48 | std::uint64_t timestamp; 49 | Point sensor_position; 50 | std::vector points; 51 | }; 52 | 53 | // Maps each voxel in a block to all point cloud indices that fall into in it. 54 | using VoxelToPointMap = voxblox::HierarchicalIndexIntMap; 55 | 56 | // Map of block indices to voxel indices and point indices of the cloud. 57 | using BlockToPointMap = voxblox::AnyIndexHashMapType::type; 58 | 59 | // Simple axis-aligned bounding box. 60 | struct BoundingBox { 61 | Point min_corner; 62 | Point max_corner; 63 | 64 | bool intersects(const BoundingBox& other, const float margin = 0.f) const { 65 | if (min_corner.x - margin > other.max_corner.x) { 66 | return false; 67 | } 68 | if (min_corner.y - margin > other.max_corner.y) { 69 | return false; 70 | } 71 | if (min_corner.z - margin > other.max_corner.x) { 72 | return false; 73 | } 74 | if (max_corner.x + margin < other.min_corner.x) { 75 | return false; 76 | } 77 | if (max_corner.y + margin < other.min_corner.y) { 78 | return false; 79 | } 80 | if (max_corner.z + margin < other.min_corner.z) { 81 | return false; 82 | } 83 | return true; 84 | } 85 | 86 | float extent() const { 87 | return (max_corner.getVector3fMap() - min_corner.getVector3fMap()).norm(); 88 | } 89 | }; 90 | 91 | // Indices of all points in the cloud belonging to this cluster. 92 | struct Cluster { 93 | int id = -1; // ID of the cluster set during tracking. 94 | int track_length = 0; // Frames this cluster has been tracked. 95 | bool valid = false; // Whether the cluster has met all cluster checks. 96 | BoundingBox aabb; // Axis-aligned bounding box of the cluster. 97 | std::vector points; // Indices of points in cloud. 98 | std::vector voxels; // Center points of voxels in this cluster. 99 | }; 100 | 101 | using Clusters = std::vector; 102 | 103 | } // namespace dynablox 104 | 105 | #endif // DYNABLOX_COMMON_TYPES_H_ 106 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/evaluation/evaluator.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_EVALUATION_EVALUATOR_H_ 2 | #define DYNABLOX_EVALUATION_EVALUATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "dynablox/common/types.h" 12 | #include "dynablox/evaluation/ground_truth_handler.h" 13 | 14 | namespace dynablox { 15 | 16 | class Evaluator { 17 | public: 18 | // Config. 19 | struct Config : public config_utilities::Config { 20 | // Where to put the evaluation. Will create a timestamped folder if 21 | // output_directory already exists, otherwise create it. 22 | std::string output_directory; 23 | 24 | // Range limiations [m]. 25 | float min_range = 0.f; 26 | float max_range = 1e6; 27 | 28 | // At which levels to evaluate. 29 | bool evaluate_point_level = true; 30 | bool evaluate_cluster_level = true; 31 | bool evaluate_object_level = true; 32 | 33 | // Save the data of all evaluated clouds. Off by default to save space. 34 | bool save_clouds = false; 35 | 36 | // If true store the parameters of all modules. 37 | bool save_config = true; 38 | 39 | // Config for the ground truth handler. 40 | GroundTruthHandler::Config ground_truth_config; 41 | 42 | Config() { setConfigName("Evaluator"); } 43 | 44 | protected: 45 | void setupParamsAndPrinting() override; 46 | void checkParams() const override; 47 | }; 48 | 49 | // Constructor. 50 | Evaluator(const Config& config_utilities); 51 | 52 | // Evaluation helper methods. 53 | /** 54 | * @brief Create the output directory. 55 | */ 56 | void setupFiles(); 57 | 58 | /** 59 | * @brief If ground truth is available, lable the cloud, compute metrics, and 60 | * write them to the output. Always update the timing infomation. 61 | * 62 | * @param cloud Point cloud to be evaluated. 63 | * @param cloud_info Cloud info to be evaluated. 64 | */ 65 | void evaluateFrame(const Cloud& cloud, CloudInfo& cloud_info, 66 | const Clusters& clusters); 67 | 68 | /** 69 | * @brief Update the timing information by overwriting the output file with 70 | * current statistics. 71 | */ 72 | void writeTimingsToFile() const; 73 | 74 | /** 75 | * @brief Compute the score for the labeled input cloud and write them to the 76 | * output file. 77 | * 78 | * @param cloud_info Labeled input pointcloud to be evaluated. 79 | */ 80 | void writeScoresToFile(CloudInfo& cloud_info); 81 | 82 | /** 83 | * @brief Save the coordinates and additional info of the evaluated cloud to 84 | * file. 85 | * 86 | * @param cloud Point cloud to be saved. 87 | * @param cloud_info Corresponding cloud info for evaluation. 88 | * @param clusters Current clustering to get cluster IDs. 89 | */ 90 | void saveCloud(const Cloud& cloud, const CloudInfo& cloud_info, 91 | const Clusters& clusters); 92 | 93 | /** 94 | * @brief Store all parameters used by the motion detector. 95 | */ 96 | void saveConfig(); 97 | 98 | /** 99 | * @brief Mark only relevant points for evaluation. 100 | * 101 | * @param cloud_info Cloud info to be marked. 102 | * @return Number of points valid for evaluation. 103 | */ 104 | int filterEvaluatedPoints(CloudInfo& cloud_info) const; 105 | 106 | /** 107 | * @brief Compute the scores for a specified level and write the results to 108 | * the output file. 109 | * 110 | * @param cloud_info Labeled cloud to evaluate. 111 | * @param level Level to evaluate [point, cluster, object] 112 | * @param output_file Filestream to write results to. 113 | */ 114 | void evaluateCloudAtLevel(const CloudInfo& cloud_info, 115 | const std::string& level, 116 | std::ofstream& output_file) const; 117 | 118 | // Computation of aggregated metrics. 119 | static float computePrecision(const uint tp, const uint fp); 120 | static float computeRecall(const uint tp, const uint fn); 121 | static float computeIntersectionOverUnion(const uint tp, const uint fp, 122 | const uint fn); 123 | 124 | int getNumberOfEvaluatedFrames() const { return gt_frame_counter_; } 125 | 126 | private: 127 | const Config config_; 128 | const GroundTruthHandler ground_truth_handler; 129 | 130 | // Variables. 131 | std::string output_directory_; 132 | std::vector evaluated_levels_; 133 | int gt_frame_counter_ = 0; 134 | bool config_saved_ = false; 135 | 136 | // Helper Functions. 137 | static std::function getCheckLevelFunction( 138 | const std::string& level); 139 | 140 | // Names of the created files. 141 | static const std::string config_file_name_; 142 | static const std::string clouds_file_name_; 143 | static const std::string scores_file_name_; 144 | static const std::string timings_file_name_; 145 | }; 146 | 147 | } // namespace dynablox 148 | 149 | #endif // DYNABLOX_EVALUATION_EVALUATOR_H_ 150 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/evaluation/ground_truth_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_EVALUATION_GROUND_TRUTH_HANDLER_H_ 2 | #define DYNABLOX_EVALUATION_GROUND_TRUTH_HANDLER_H_ 3 | 4 | #include "dynablox/3rd_party/config_utilities.hpp" 5 | #include "dynablox/common/types.h" 6 | 7 | namespace dynablox { 8 | 9 | // This ground-truth handler manages the data available in the DOALS 10 | // (https://projects.asl.ethz.ch/datasets/doku.php?id=doals) dataset. 11 | class GroundTruthHandler { 12 | public: 13 | // Config. 14 | struct Config : public config_utilities::Config { 15 | // Where to read the ground truth data. 16 | std::string file_path; 17 | 18 | Config() { setConfigName("GroundTruthHandler"); } 19 | 20 | protected: 21 | void setupParamsAndPrinting() override; 22 | void checkParams() const override; 23 | }; 24 | 25 | using TimestampVectorMap = std::map>; 26 | 27 | explicit GroundTruthHandler(const Config& config); 28 | 29 | /** 30 | * @brief Read the DOALS indices.csv file into a map. 31 | * 32 | */ 33 | void createLookupFromCSV(); 34 | 35 | /** 36 | * @brief Check whether there exist annotations for the given time stamp and 37 | * label the cloud info if available. 38 | * 39 | * @param cloud_info Cloud info to annotate. Will use the time stamp in the 40 | * cloud for matching. 41 | * @return Whether ground truth data was found. 42 | */ 43 | bool labelCloudInfoIfAvailable(CloudInfo& cloud_info) const; 44 | 45 | private: 46 | const Config config_; 47 | TimestampVectorMap ground_truth_lookup_; 48 | }; 49 | 50 | } // namespace dynablox 51 | 52 | #endif // DYNABLOX_EVALUATION_GROUND_TRUTH_HANDLER_H_ 53 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/evaluation/io_tools.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_EVALUATION_IO_TOOLS_H_ 2 | #define DYNABLOX_EVALUATION_IO_TOOLS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "dynablox/common/types.h" 10 | 11 | namespace dynablox { 12 | 13 | /** 14 | * @brief Save a cloud to a human readable csv file. Repeated calls of this 15 | * function will append clouds to the same file. 16 | * 17 | * @param file_name Full path of output file with extension. 18 | * @param cloud Point cloud to save. 19 | * @param cloud_info Associated point infos to save. 20 | * @param clusters Optionally save the clustering ids. 21 | * @param cloud_id ID of the cloud to be stored. 22 | * @returns True if the save operation was successful. 23 | */ 24 | bool saveCloudToCsv(const std::string& file_name, const Cloud& cloud, 25 | const CloudInfo& cloud_info, 26 | const Clusters& clusters = Clusters(), 27 | const int cloud_id = 0); 28 | 29 | /** 30 | * @brief Read all clouds from a given csv file. Each cloud will have a separate 31 | * entry in the vector. 32 | * 33 | * @param file_name Full path of input file with extension. 34 | * @param clouds Where to store read clouds. 35 | * @param cloud_infos Where to store read cloud infos. 36 | * @param clusters Where to store read clusters. 37 | * @return True if the load operation was successful. 38 | */ 39 | bool loadCloudFromCsv(const std::string& file_name, std::vector& clouds, 40 | std::vector& cloud_infos, 41 | std::vector& clusters); 42 | 43 | } // namespace dynablox 44 | 45 | #endif // DYNABLOX_EVALUATION_IO_TOOLS_H_ 46 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/processing/clustering.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_PROCESSING_CLUSTERING_H_ 2 | #define DYNABLOX_PROCESSING_CLUSTERING_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "dynablox/3rd_party/config_utilities.hpp" 20 | #include "dynablox/common/neighborhood_search.h" 21 | #include "dynablox/common/types.h" 22 | 23 | namespace dynablox { 24 | 25 | class Clustering { 26 | public: 27 | // Config. 28 | struct Config : public config_utilities::Config { 29 | // Filter out clusters with too few or many points. 30 | int min_cluster_size = 25; 31 | int max_cluster_size = 2500; 32 | 33 | // filter out clusters whose AABB is larger or smaller than this [m]. 34 | float min_extent = 0.25f; 35 | float max_extent = 2.5f; 36 | 37 | // Connectivity used when clustering voxels. (6, 18, 26) 38 | int neighbor_connectivity = 6; 39 | 40 | // Grow ever free detections by 1 (false) or 2 (true) voxels. 41 | bool grow_clusters_twice = true; 42 | 43 | // merge clusters whose points are closer than the minimum separation [m]. 44 | float min_cluster_separation = 0.2; 45 | 46 | // If true check separation per point, if false per voxel. 47 | bool check_cluster_separation_exact = false; 48 | 49 | Config() { setConfigName("Clustering"); } 50 | 51 | protected: 52 | void setupParamsAndPrinting() override; 53 | void checkParams() const override; 54 | }; 55 | 56 | // Constructor. 57 | Clustering(const Config& config, TsdfLayer::Ptr tsdf_layer); 58 | 59 | // Types. 60 | using ClusterIndices = std::vector; 61 | 62 | /** 63 | * @brief Cluster all currently occupied voxels that are next to an ever-free 64 | * voxel. Merge nearby clusters and apply cluster level filters. 65 | * 66 | * @param point_map Map of points to voxels. 67 | * @param occupied_ever_free_voxel_indices Occupied voxels to seed cluster 68 | * growing. 69 | * @param frame_counter Current frame number. 70 | * @param cloud_info Info to store which points are cluster-level dynamic. 71 | * @return The identified clusters. 72 | */ 73 | Clusters performClustering( 74 | const BlockToPointMap& point_map, 75 | const ClusterIndices& occupied_ever_free_voxel_indices, 76 | const int frame_counter, const Cloud& cloud, CloudInfo& cloud_info) const; 77 | 78 | /** 79 | * @brief Cluster all currently occupied voxels that are next to an ever-free 80 | * voxel. 81 | * 82 | * @param occupied_ever_free_voxel_indices Occupied ever-free voxel indices to 83 | * seed the clusters. 84 | * @param frame_counter Frame number to verify added voxels contain points 85 | * this scan. 86 | * @return Vector of all found clusters. 87 | */ 88 | std::vector voxelClustering( 89 | const ClusterIndices& occupied_ever_free_voxel_indices, 90 | const int frame_counter) const; 91 | 92 | /** 93 | * @brief Grow a single cluster from a seed voxel key. All voxels that are not 94 | * yet processed are added to the cluster and labeled as processed and 95 | * dynamic. Only ever-free voxels can further grow the cluster. 96 | * 97 | * @param seed Voxel key to start clustering from. 98 | * @param frame_counter Frame number to verify added voxels contain points 99 | * this scan. 100 | * @param result Where to store the voxel keys of all voxels of the cluster. 101 | * @return If a result cluster was found. 102 | */ 103 | bool growCluster(const voxblox::VoxelKey& seed, const int frame_counter, 104 | ClusterIndices& result) const; 105 | 106 | /** 107 | * @brief Use the voxel level clustering to assign all points to clusters. 108 | * 109 | * @param point_map Mapping of blocks to voxels and points in the cloud. 110 | * @param voxel_cluster_indices Voxel indices per cluster. 111 | * @return All clusters. 112 | */ 113 | Clusters inducePointClusters( 114 | const BlockToPointMap& point_map, 115 | const std::vector& voxel_cluster_indices) const; 116 | 117 | /** 118 | * @brief Merge clusters together whose points are clsoe together. 119 | * 120 | * @param cloud Pointcloud to lookup cluster points poses. 121 | * @param clusters Clsuters to be checked and merged. 122 | */ 123 | void mergeClusters(const Cloud& cloud, Clusters& clusters) const; 124 | 125 | /** 126 | * @brief Removes all clusters that don't meet the filtering criteria. 127 | * 128 | * @param candidates list of clusters that will be filtered. 129 | */ 130 | void applyClusterLevelFilters(Clusters& candidates) const; 131 | 132 | /** 133 | * @brief Check filters for an inidividual cluster. 134 | * 135 | * @param cluster Cluster to check. 136 | * @return True if the cluster was filtered out. 137 | */ 138 | bool filterCluster(const Cluster& cluster) const; 139 | 140 | /** 141 | * @brief Sets dynamic flag on point level (includes points belonging to 142 | * extension of high confidence detection clusters) 143 | * 144 | * @param clusters Clusters whose points will be labeled. 145 | * @param cloud_info Cloud info where the label is placed. 146 | */ 147 | void setClusterLevelDynamicFlagOfallPoints(const Clusters& clusters, 148 | CloudInfo& cloud_info) const; 149 | 150 | /** 151 | * @brief Compute the axis-aligned bounding box for a cluster. 152 | * 153 | * @param cloud Pointcloud to look up the positions. 154 | * @param cluster Clsuter to evaluate. 155 | */ 156 | void computeAABB(const Cloud& cloud, Cluster& cluster) const; 157 | 158 | private: 159 | const Config config_; 160 | const TsdfLayer::Ptr tsdf_layer_; 161 | const NeighborhoodSearch neighborhood_search_; 162 | }; 163 | 164 | } // namespace dynablox 165 | 166 | #endif // DYNABLOX_PROCESSING_CLUSTERING_H_ 167 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/processing/ever_free_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_PROCESSING_EVER_FREE_INTEGRATOR_H_ 2 | #define DYNABLOX_PROCESSING_EVER_FREE_INTEGRATOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "dynablox/3rd_party/config_utilities.hpp" 11 | #include "dynablox/common/neighborhood_search.h" 12 | #include "dynablox/common/types.h" 13 | 14 | namespace dynablox { 15 | 16 | class EverFreeIntegrator { 17 | public: 18 | // Config. 19 | struct Config : public config_utilities::Config { 20 | // Neighborhood connectivity when removing ever free. 21 | int neighbor_connectivity = 18; 22 | 23 | // After this many occupied observations, an ever-free voxel will be labeled 24 | // as occupied (thus never-free). 25 | int counter_to_reset = 50; 26 | 27 | // Number of frames a voxel can be free in between occupancy without losing 28 | // occupancy status to compensate for point sparsity. 29 | int temporal_buffer = 2; 30 | 31 | // Number of consecutive frames a voxel must be free to become ever-free. 32 | int burn_in_period = 5; 33 | 34 | // SDF distance below which a voxel is considered occupied [m]. 35 | float tsdf_occupancy_threshold = 0.3; 36 | 37 | // Number of threads to use. 38 | int num_threads = std::thread::hardware_concurrency(); 39 | 40 | Config() { setConfigName("EverFreeIntegrator"); } 41 | 42 | protected: 43 | void setupParamsAndPrinting() override; 44 | void checkParams() const override; 45 | }; 46 | 47 | EverFreeIntegrator(const Config& config, 48 | std::shared_ptr tsdf_layer); 49 | 50 | /** 51 | * @brief Update the ever-free state of all changed TSDF-voxels by checking 52 | * when they were last occupied and for how long. 53 | * 54 | * @param frame_counter Index of current lidar scan to compute age. 55 | */ 56 | void updateEverFreeVoxels(const int frame_counter) const; 57 | 58 | /** 59 | * @brief Process each block in parallel. 60 | * 61 | * @param block_index Index of block to process. 62 | * @param frame_counter Index of current lidar scan to compute age. 63 | * @return All voxels that fell outside the block and need clearing later. 64 | */ 65 | bool blockWiseUpdateEverFree( 66 | const BlockIndex& block_index, const int frame_counter, 67 | voxblox::AlignedVector& voxels_to_remove) const; 68 | 69 | /** 70 | * @brief If the voxel is currently static we leave it. If it was last static 71 | * last frame, increment the occupancy counter, else reset it. 72 | * 73 | * @param tsdf_voxel Voxel to update. 74 | * @param frame_counter Current lidar scan time index. 75 | */ 76 | void updateOccupancyCounter(TsdfVoxel& tsdf_voxel, 77 | const int frame_counter) const; 78 | 79 | /** 80 | * @brief Remove the ever-free and dynamic attributes from a given voxel and 81 | * all its neighbors (which now also don't meet the criteria anymore.) 82 | * 83 | * @param block Tsdf block containing the voxel. 84 | * @param voxel Voxel to be cleared from ever-free. 85 | * @param block_index Index of the containing block. 86 | * @param voxel_index Index of the voxel in the block. 87 | * @return All voxels that fell outside the block and need clearing later. 88 | */ 89 | voxblox::AlignedVector removeEverFree( 90 | TsdfBlock& block, TsdfVoxel& voxel, const BlockIndex& block_index, 91 | const VoxelIndex& voxel_index) const; 92 | 93 | /** 94 | * @brief Check for any occupied or unknown voxels in neighborhood, otherwise 95 | * mark voxel as ever free. Check all voxels in the block. 96 | * 97 | * @param block_index Index of block to check. 98 | * @param frame_counter Current frame to compute occupied time. 99 | */ 100 | void blockWiseMakeEverFree(const BlockIndex& block_index, 101 | const int frame_counter) const; 102 | 103 | private: 104 | const Config config_; 105 | const TsdfLayer::Ptr tsdf_layer_; 106 | const NeighborhoodSearch neighborhood_search_; 107 | 108 | // Cached frequently used values. 109 | const float voxel_size_; 110 | const size_t voxels_per_side_; 111 | const size_t voxels_per_block_; 112 | }; 113 | 114 | } // namespace dynablox 115 | 116 | #endif // DYNABLOX_PROCESSING_EVER_FREE_INTEGRATOR_H_ 117 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/processing/preprocessing.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_PROCESSING_PREPROCESSING_H_ 2 | #define DYNABLOX_PROCESSING_PREPROCESSING_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "dynablox/3rd_party/config_utilities.hpp" 10 | #include "dynablox/common/types.h" 11 | 12 | namespace dynablox { 13 | 14 | class Preprocessing { 15 | public: 16 | // Config. 17 | struct Config : public config_utilities::Config { 18 | // Maximum ray length to integrate [m]. 19 | float max_range = 20.f; 20 | 21 | // Minimum range for all points [m]. 22 | float min_range = 0.5; 23 | 24 | Config() { setConfigName("Preprocessing"); } 25 | 26 | protected: 27 | void setupParamsAndPrinting() override; 28 | void checkParams() const override; 29 | }; 30 | 31 | explicit Preprocessing(const Config& config); 32 | 33 | /** 34 | * @brief Transform the pointcloud to world frame and mark points valid for 35 | * integration and evaluation. 36 | * 37 | * @param msg Input pointcloud in sensor frame. 38 | * @param T_M_S Transform sensor (S) to map (M). 39 | * @param cloud_info Cloud info to store the data of the input cloud. 40 | * @param cloud Cloud to store the processed input point cloud. 41 | * @return Success. 42 | */ 43 | bool processPointcloud(const sensor_msgs::PointCloud2::Ptr& msg, 44 | const tf::StampedTransform T_M_S, Cloud& cloud, 45 | CloudInfo& cloud_info) const; 46 | 47 | private: 48 | // Config. 49 | const Config config_; 50 | }; 51 | 52 | } // namespace dynablox 53 | 54 | #endif // DYNABLOX_PROCESSING_PREPROCESSING_H_ 55 | -------------------------------------------------------------------------------- /dynablox/include/dynablox/processing/tracking.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_PROCESSING_TRACKING_H_ 2 | #define DYNABLOX_PROCESSING_TRACKING_H_ 3 | 4 | #include "dynablox/3rd_party/config_utilities.hpp" 5 | #include "dynablox/common/types.h" 6 | 7 | namespace dynablox { 8 | 9 | class Tracking { 10 | public: 11 | // Config. 12 | struct Config : public config_utilities::Config { 13 | // Numbers of frames a cluster needs to be tracked to be considered dynamic. 14 | int min_track_duration = 0; 15 | 16 | // Maximum distance a cluster may have moved to be considered a track [m]. 17 | float max_tracking_distance = 1.f; 18 | 19 | Config() { setConfigName("Tracking"); } 20 | 21 | protected: 22 | void setupParamsAndPrinting() override; 23 | void checkParams() const override; 24 | }; 25 | 26 | // Constructor. 27 | Tracking(const Config& config); 28 | 29 | /** 30 | * @brief Track all clusters w.r.t. the previous clsuters. Denote the object 31 | * level points dynamic in the cloud_info. 32 | * 33 | * @param cloud Current lidar point cloud. 34 | * @param clusters Current detected clusters. 35 | * @param cloud_info Cloud info to denote moving points. 36 | */ 37 | void track(const Cloud& cloud, Clusters& clusters, CloudInfo& cloud_info); 38 | 39 | private: 40 | const Config config_; 41 | 42 | // Tracking data w.r.t. previous observation. 43 | std::vector previous_centroids_; 44 | std::vector previous_ids_; 45 | std::vector previous_track_lengths_; 46 | 47 | /** 48 | * @brief Simple closest association tracking for now. 49 | * 50 | * @param cloud Lidar point cloud. 51 | * @param clusters Current clusters to be tracked. 52 | */ 53 | void trackClusterIDs(const Cloud& cloud, Clusters& clusters); 54 | }; 55 | 56 | } // namespace dynablox 57 | 58 | #endif // DYNABLOX_PROCESSING_TRACKING_H_ 59 | -------------------------------------------------------------------------------- /dynablox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynablox 4 | 0.0.0 5 | Detecting arbitrary moving objects in compelx environments 6 | 7 | Lukas Schmid 8 | Aurelio Sulser 9 | Olov Andersson 10 | 11 | Lukas Schmid 12 | Aurelio Sulser 13 | Olov Andersson 14 | 15 | BSD 16 | 17 | catkin 18 | catkin_simple 19 | 20 | 21 | pcl_conversions 22 | tf2 23 | voxblox 24 | 25 | -------------------------------------------------------------------------------- /dynablox/src/evaluation/evaluator.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/evaluation/evaluator.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "dynablox/evaluation/io_tools.h" 15 | 16 | namespace dynablox { 17 | 18 | const std::string Evaluator::config_file_name_ = "config.txt"; 19 | const std::string Evaluator::clouds_file_name_ = "clouds.csv"; 20 | const std::string Evaluator::scores_file_name_ = "scores.csv"; 21 | const std::string Evaluator::timings_file_name_ = "timings.txt"; 22 | 23 | void Evaluator::Config::checkParams() const { 24 | checkParamCond(!output_directory.empty(), "'output_directory' must be set."); 25 | checkParamGE(min_range, 0.f, "min_range"); 26 | checkParamCond(max_range > min_range, 27 | "'max_range' must be larger than 'min_range'."); 28 | checkParamConfig(ground_truth_config); 29 | } 30 | 31 | void Evaluator::Config::setupParamsAndPrinting() { 32 | setupParam("output_directory", &output_directory); 33 | setupParam("min_range", &min_range); 34 | setupParam("max_range", &max_range); 35 | setupParam("evaluate_point_level", &evaluate_point_level); 36 | setupParam("evaluate_cluster_level", &evaluate_cluster_level); 37 | setupParam("evaluate_object_level", &evaluate_object_level); 38 | setupParam("save_clouds", &save_clouds); 39 | setupParam("ground_truth", &ground_truth_config, "ground_truth"); 40 | } 41 | 42 | Evaluator::Evaluator(const Config& config) 43 | : config_(config.checkValid()), 44 | ground_truth_handler(config_.ground_truth_config) { 45 | setupFiles(); 46 | } 47 | 48 | void Evaluator::setupFiles() { 49 | output_directory_ = config_.output_directory; 50 | if (std::filesystem::exists(config_.output_directory)) { 51 | // Already exists, create a time-stamped dir instead. 52 | auto t = std::time(nullptr); 53 | auto tm = *std::localtime(&t); 54 | std::stringstream timestamp; 55 | timestamp << std::put_time(&tm, "%Y_%m_%d-%H_%M_%S"); 56 | output_directory_ = output_directory_ + "/" + timestamp.str(); 57 | } 58 | std::filesystem::create_directories(output_directory_); 59 | LOG(INFO) << "Writing evaluation to '" << output_directory_ << "."; 60 | 61 | // Setup the header of the scores file. 62 | if (config_.evaluate_point_level) { 63 | evaluated_levels_.push_back("point"); 64 | } 65 | if (config_.evaluate_cluster_level) { 66 | evaluated_levels_.push_back("cluster"); 67 | } 68 | if (config_.evaluate_object_level) { 69 | evaluated_levels_.push_back("object"); 70 | } 71 | 72 | // Setup scores file. 73 | std::ofstream writefile; 74 | writefile.open(output_directory_ + "/" + scores_file_name_, std::ios::trunc); 75 | writefile << "timestamp,"; 76 | for (const std::string& level : evaluated_levels_) { 77 | writefile << level + "_IoU," << level + "_Precision," << level + "_Recall," 78 | << level + "_TP," << level + "_TN," << level + "_FP," 79 | << level + "_FN,"; 80 | } 81 | writefile << "EvaluatedPoints,TotalPoints" << std::endl; 82 | writefile.close(); 83 | } 84 | 85 | void Evaluator::evaluateFrame(const Cloud& cloud, CloudInfo& cloud_info, 86 | const Clusters& clusters) { 87 | // Update the timings every frame. 88 | writeTimingsToFile(); 89 | saveConfig(); 90 | 91 | // If ground truth available, label the cloud and compute the metrics. 92 | if (ground_truth_handler.labelCloudInfoIfAvailable(cloud_info)) { 93 | writeScoresToFile(cloud_info); 94 | saveCloud(cloud, cloud_info, clusters); 95 | gt_frame_counter_++; 96 | LOG(INFO) << "Evaluated cloud " << gt_frame_counter_ << " with timestamp " 97 | << cloud_info.timestamp << "."; 98 | } 99 | } 100 | 101 | void Evaluator::writeTimingsToFile() const { 102 | // Overwrite the timings with the current statistics. 103 | std::ofstream writefile; 104 | writefile.open(output_directory_ + "/" + timings_file_name_, std::ios::trunc); 105 | writefile << voxblox::timing::Timing::Print() << std::endl; 106 | writefile.close(); 107 | } 108 | 109 | void Evaluator::writeScoresToFile(CloudInfo& cloud_info) { 110 | std::ofstream writefile; 111 | writefile.open(output_directory_ + "/" + scores_file_name_, std::ios::app); 112 | 113 | // Time stamp and preprocessing. 114 | writefile << cloud_info.timestamp; 115 | const int evaluated_points = filterEvaluatedPoints(cloud_info); 116 | 117 | // Evaluated levels. 118 | for (const std::string& level : evaluated_levels_) { 119 | evaluateCloudAtLevel(cloud_info, level, writefile); 120 | } 121 | 122 | // Number of evaluated points. 123 | writefile << "," << evaluated_points << "," << cloud_info.points.size() 124 | << std::endl; 125 | } 126 | 127 | void Evaluator::saveCloud(const Cloud& cloud, const CloudInfo& cloud_info, 128 | const Clusters& clusters) { 129 | if (!config_.save_clouds) { 130 | return; 131 | } 132 | 133 | const std::string file_name = output_directory_ + "/" + clouds_file_name_; 134 | saveCloudToCsv(file_name, cloud, cloud_info, clusters, gt_frame_counter_); 135 | } 136 | 137 | int Evaluator::filterEvaluatedPoints(CloudInfo& cloud_info) const { 138 | int number_of_points = 0; 139 | for (PointInfo& point : cloud_info.points) { 140 | if (point.distance_to_sensor >= config_.min_range && 141 | point.distance_to_sensor <= config_.max_range) { 142 | point.ready_for_evaluation = true; 143 | number_of_points++; 144 | } 145 | } 146 | return number_of_points; 147 | } 148 | 149 | std::function Evaluator::getCheckLevelFunction( 150 | const std::string& level) { 151 | if (level == "point") { 152 | return [](const PointInfo& point) { return point.ever_free_level_dynamic; }; 153 | } else if (level == "cluster") { 154 | return [](const PointInfo& point) { return point.cluster_level_dynamic; }; 155 | } else if (level == "object") { 156 | return [](const PointInfo& point) { return point.object_level_dynamic; }; 157 | } else { 158 | LOG(ERROR) << "Unknown evaluation level '" << level << "'!"; 159 | return [](const PointInfo& /* point */) { return false; }; 160 | } 161 | } 162 | 163 | void Evaluator::evaluateCloudAtLevel(const CloudInfo& cloud_info, 164 | const std::string& level, 165 | std::ofstream& output_file) const { 166 | // Setup. 167 | std::function check_level = 168 | getCheckLevelFunction(level); 169 | 170 | // Compute true/false positives/negatives. 171 | uint tp = 0u; 172 | uint fp = 0u; 173 | uint tn = 0u; 174 | uint fn = 0u; 175 | for (const PointInfo& point : cloud_info.points) { 176 | if (!point.ready_for_evaluation) { 177 | continue; 178 | } 179 | const bool is_dynamic = check_level(point); 180 | if (is_dynamic && point.ground_truth_dynamic) { 181 | tp++; 182 | } else if (is_dynamic && !point.ground_truth_dynamic) { 183 | fp++; 184 | } else if (!is_dynamic && !point.ground_truth_dynamic) { 185 | tn++; 186 | } else if (!is_dynamic && point.ground_truth_dynamic) { 187 | fn++; 188 | } 189 | } 190 | 191 | // Write metrics to file. 192 | output_file << "," << computeIntersectionOverUnion(tp, fp, fn) << "," 193 | << computePrecision(tp, fp) << "," << computeRecall(tp, fn) << "," 194 | << tp << "," << tn << "," << fp << "," << fn; 195 | } 196 | 197 | void Evaluator::saveConfig() { 198 | if (!config_.save_config || config_saved_) { 199 | return; 200 | } 201 | 202 | // Write config to file. 203 | std::ofstream writefile; 204 | writefile.open(output_directory_ + "/" + config_file_name_, std::ios::trunc); 205 | writefile << config_utilities::Global::printAllConfigs(); 206 | writefile.close(); 207 | config_saved_ = true; 208 | } 209 | 210 | float Evaluator::computePrecision(const uint tp, const uint fp) { 211 | if (tp + fp == 0u) { 212 | return 1.f; 213 | } 214 | return static_cast(tp) / static_cast(tp + fp); 215 | } 216 | 217 | float Evaluator::computeRecall(const uint tp, const uint fn) { 218 | if (tp + fn == 0u) { 219 | return 1.f; 220 | } 221 | return static_cast(tp) / static_cast(tp + fn); 222 | } 223 | 224 | float Evaluator::computeIntersectionOverUnion(const uint tp, const uint fp, 225 | const uint fn) { 226 | if (tp + fp + fn == 0u) { 227 | return 1.f; 228 | } 229 | return static_cast(tp) / static_cast(tp + fp + fn); 230 | } 231 | 232 | } // namespace dynablox 233 | -------------------------------------------------------------------------------- /dynablox/src/evaluation/ground_truth_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/evaluation/ground_truth_handler.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace dynablox { 14 | 15 | void GroundTruthHandler::Config::checkParams() const { 16 | checkParamCond(std::filesystem::exists(file_path), 17 | "Target file '" + file_path + "' does not exist."); 18 | } 19 | 20 | void GroundTruthHandler::Config::setupParamsAndPrinting() { 21 | setupParam("file_path", &file_path); 22 | } 23 | 24 | GroundTruthHandler::GroundTruthHandler(const Config& config) 25 | : config_(config.checkValid()) { 26 | // Setup the lookup table. 27 | createLookupFromCSV(); 28 | } 29 | 30 | void GroundTruthHandler::createLookupFromCSV() { 31 | // Open file. 32 | std::ifstream readfile; 33 | readfile.open(config_.file_path); 34 | 35 | // Read data. 36 | std::string line; 37 | size_t counter = 0; 38 | while (getline(readfile, line)) { 39 | if (line.empty()) { 40 | continue; 41 | } 42 | bool first_column = true; 43 | std::uint64_t timestamp; 44 | std::istringstream iss(line); 45 | std::string linestream; 46 | std::vector row; 47 | while (std::getline(iss, linestream, ',')) { 48 | if (first_column) { 49 | timestamp = static_cast(std::stoul(linestream)); 50 | first_column = false; 51 | } else { 52 | row.push_back(std::stoi(linestream)); 53 | } 54 | } 55 | ground_truth_lookup_[timestamp] = row; 56 | counter++; 57 | } 58 | LOG(INFO) << "Read " << counter << " entries from '" << config_.file_path 59 | << "'."; 60 | } 61 | 62 | bool GroundTruthHandler::labelCloudInfoIfAvailable( 63 | CloudInfo& cloud_info) const { 64 | // Check whether there exists a label for this timestamp. 65 | auto it = ground_truth_lookup_.find(cloud_info.timestamp); 66 | if (it == ground_truth_lookup_.end()) { 67 | return false; 68 | } 69 | 70 | // label the cloud. 71 | cloud_info.has_labels = true; 72 | for (const auto& index : it->second) { 73 | cloud_info.points[index].ground_truth_dynamic = true; 74 | } 75 | return true; 76 | } 77 | 78 | } // namespace dynablox 79 | -------------------------------------------------------------------------------- /dynablox/src/evaluation/io_tools.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/evaluation/io_tools.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace dynablox { 8 | 9 | bool saveCloudToCsv(const std::string& file_name, const Cloud& cloud, 10 | const CloudInfo& cloud_info, const Clusters& clusters, 11 | const int cloud_id) { 12 | // Initialize headers if required. 13 | std::fstream writefile; 14 | const bool file_existed = std::filesystem::exists(file_name); 15 | writefile.open(file_name, std::ios::app); 16 | if (!writefile.is_open()) { 17 | return false; 18 | } 19 | if (!file_existed) { 20 | writefile << "CloudNo,X,Y,Z,Distance,PointDynamic,ClusterDynamic," 21 | "ObjectDynamic,GTDynamic,ReadyForEvaluation,ClusterID\n"; 22 | } 23 | 24 | // Add all new data to the database. 25 | size_t i = 0; 26 | for (const Point& point : cloud) { 27 | const PointInfo& info = cloud_info.points.at(i); 28 | int cluster_id = -1; 29 | if (info.cluster_level_dynamic) { 30 | for (const Cluster& cluster : clusters) { 31 | if (std::find(cluster.points.begin(), cluster.points.end(), i) != 32 | cluster.points.end()) { 33 | cluster_id = cluster.id; 34 | break; 35 | } 36 | } 37 | } 38 | ++i; 39 | 40 | writefile << cloud_id << "," << point.x << "," << point.y << "," << point.z 41 | << "," << info.distance_to_sensor << "," 42 | << info.ever_free_level_dynamic << "," 43 | << info.cluster_level_dynamic << "," << info.object_level_dynamic 44 | << "," << info.ground_truth_dynamic << "," 45 | << info.ready_for_evaluation << "," << cluster_id << "\n"; 46 | } 47 | writefile.close(); 48 | return true; 49 | } 50 | 51 | bool loadCloudFromCsv(const std::string& file_name, std::vector& clouds, 52 | std::vector& cloud_infos, 53 | std::vector& clusters) { 54 | // Open file. 55 | std::ifstream readfile; 56 | readfile.open(file_name); 57 | if (!readfile.is_open()) { 58 | return false; 59 | } 60 | 61 | // Read data. 62 | std::string line; 63 | int cloud_counter = -2; 64 | std::unordered_map 65 | cluster_id_to_index; // [cluster_id] = position in clusters vector for 66 | // this frame. 67 | while (getline(readfile, line)) { 68 | if (line.empty()) { 69 | continue; 70 | } 71 | if (cloud_counter == -2) { 72 | // Skip headers. 73 | cloud_counter++; 74 | continue; 75 | } 76 | std::istringstream iss(line); 77 | std::string linestream; 78 | size_t row_index = 0; 79 | Point* point; 80 | PointInfo* info; 81 | while (std::getline(iss, linestream, ',')) { 82 | switch (row_index) { 83 | case 0u: { 84 | const int cloud_id = std::stoi(linestream); 85 | if (cloud_id != cloud_counter) { 86 | // Initialize new cloud. 87 | clouds.push_back(Cloud()); 88 | cloud_infos.push_back(CloudInfo()); 89 | clusters.push_back(Clusters()); 90 | cluster_id_to_index.clear(); 91 | cloud_infos.back().has_labels = true; 92 | cloud_counter++; 93 | } 94 | clouds.back().push_back(Point()); 95 | point = &clouds.back().back(); 96 | cloud_infos.back().points.push_back(PointInfo()); 97 | info = &cloud_infos.back().points.back(); 98 | break; 99 | } 100 | case 1u: { 101 | point->x = std::stof(linestream); 102 | break; 103 | } 104 | case 2u: { 105 | point->y = std::stof(linestream); 106 | break; 107 | } 108 | case 3u: { 109 | point->z = std::stof(linestream); 110 | break; 111 | } 112 | case 4u: { 113 | info->distance_to_sensor = std::stof(linestream); 114 | break; 115 | } 116 | case 5u: { 117 | info->ever_free_level_dynamic = 118 | static_cast(std::stoi(linestream)); 119 | break; 120 | } 121 | case 6u: { 122 | info->cluster_level_dynamic = 123 | static_cast(std::stoi(linestream)); 124 | break; 125 | } 126 | case 7u: { 127 | info->object_level_dynamic = static_cast(std::stoi(linestream)); 128 | break; 129 | } 130 | case 8u: { 131 | info->ground_truth_dynamic = static_cast(std::stoi(linestream)); 132 | break; 133 | } 134 | case 9u: { 135 | info->ready_for_evaluation = static_cast(std::stoi(linestream)); 136 | break; 137 | } 138 | case 10u: { 139 | if (!info->cluster_level_dynamic) { 140 | // Not part of a cluster. 141 | break; 142 | } 143 | const int cluster_id = std::stoi(linestream); 144 | Cluster* cluster; 145 | auto it = cluster_id_to_index.find(cluster_id); 146 | if (it == cluster_id_to_index.end()) { 147 | // ID does not yet exist. 148 | clusters.back().push_back(Cluster()); 149 | cluster = &clusters.back().back(); 150 | cluster->id = cluster_id; 151 | cluster->valid = true; 152 | cluster_id_to_index.insert( 153 | std::pair(cluster_id, clusters.back().size() - 1u)); 154 | } else { 155 | cluster = &clusters.back().at(it->second); 156 | } 157 | // Add the point. 158 | cluster->points.push_back(clouds.back().size() - 1u); 159 | break; 160 | } 161 | 162 | default: 163 | break; 164 | } 165 | row_index++; 166 | } 167 | } 168 | readfile.close(); 169 | return true; 170 | } 171 | 172 | } // namespace dynablox 173 | -------------------------------------------------------------------------------- /dynablox/src/processing/clustering.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/processing/clustering.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace dynablox { 10 | 11 | void Clustering::Config::checkParams() const { 12 | checkParamCond(max_cluster_size > min_cluster_size, 13 | "'max_cluster_size' must be larger than 'min_cluster_size'."); 14 | checkParamCond(neighbor_connectivity == 6 || neighbor_connectivity == 18 || 15 | neighbor_connectivity == 26, 16 | "'neighbor_connectivity' must be 6, 18, or 26."); 17 | checkParamGT(max_extent, 0.f, "max_extent"); 18 | checkParamCond(max_extent > min_extent, 19 | "'max_extent' must be larger than 'min_extent'."); 20 | } 21 | 22 | void Clustering::Config::setupParamsAndPrinting() { 23 | setupParam("min_cluster_size", &min_cluster_size); 24 | setupParam("max_cluster_size", &max_cluster_size); 25 | setupParam("min_extent", &min_extent, "m"); 26 | setupParam("max_extent", &max_extent, "m"); 27 | setupParam("grow_clusters_twice", &grow_clusters_twice); 28 | setupParam("min_cluster_separation", &min_cluster_separation, "m"); 29 | setupParam("check_cluster_separation_exact", &check_cluster_separation_exact); 30 | setupParam("neighbor_connectivity", &neighbor_connectivity); 31 | } 32 | 33 | Clustering::Clustering(const Config& config, TsdfLayer::Ptr tsdf_layer) 34 | : config_(config.checkValid()), 35 | tsdf_layer_(std::move(tsdf_layer)), 36 | neighborhood_search_(config.neighbor_connectivity) {} 37 | 38 | Clusters Clustering::performClustering( 39 | const BlockToPointMap& point_map, 40 | const ClusterIndices& occupied_ever_free_voxel_indices, 41 | const int frame_counter, const Cloud& cloud, CloudInfo& cloud_info) const { 42 | // Cluster all occupied voxels. 43 | const std::vector voxel_cluster_indices = 44 | voxelClustering(occupied_ever_free_voxel_indices, frame_counter); 45 | 46 | // Group points into clusters. 47 | Clusters clusters = inducePointClusters(point_map, voxel_cluster_indices); 48 | for (Cluster& cluster : clusters) { 49 | computeAABB(cloud, cluster); 50 | } 51 | 52 | // Merge close Clusters. 53 | mergeClusters(cloud, clusters); 54 | 55 | // Apply filters to remove spurious clusters. 56 | applyClusterLevelFilters(clusters); 57 | 58 | // Label all remaining points as dynamic. 59 | setClusterLevelDynamicFlagOfallPoints(clusters, cloud_info); 60 | return clusters; 61 | } 62 | 63 | std::vector Clustering::voxelClustering( 64 | const ClusterIndices& occupied_ever_free_voxel_indices, 65 | const int frame_counter) const { 66 | std::vector voxel_cluster_indices; 67 | 68 | // Process all newly occupied ever-free voxels as potential cluster seeds. 69 | for (const voxblox::VoxelKey& voxel_key : occupied_ever_free_voxel_indices) { 70 | ClusterIndices cluster; 71 | if (growCluster(voxel_key, frame_counter, cluster)) { 72 | voxel_cluster_indices.push_back(cluster); 73 | } 74 | } 75 | return voxel_cluster_indices; 76 | } 77 | 78 | bool Clustering::growCluster(const voxblox::VoxelKey& seed, 79 | const int frame_counter, 80 | ClusterIndices& result) const { 81 | std::vector stack = {seed}; 82 | const size_t voxels_per_side = tsdf_layer_->voxels_per_side(); 83 | 84 | while (!stack.empty()) { 85 | // Get the voxel. 86 | const voxblox::VoxelKey voxel_key = stack.back(); 87 | stack.pop_back(); 88 | TsdfBlock::Ptr tsdf_block = 89 | tsdf_layer_->getBlockPtrByIndex(voxel_key.first); 90 | if (!tsdf_block) { 91 | continue; 92 | } 93 | TsdfVoxel& tsdf_voxel = tsdf_block->getVoxelByVoxelIndex(voxel_key.second); 94 | 95 | // Process every voxel only once. 96 | if (tsdf_voxel.clustering_processed) { 97 | continue; 98 | } 99 | 100 | // Add voxel to cluster. 101 | tsdf_voxel.dynamic = true; 102 | tsdf_voxel.clustering_processed = true; 103 | result.push_back(voxel_key); 104 | 105 | // Extend cluster to neighbor voxels. 106 | const voxblox::AlignedVector neighbors = 107 | neighborhood_search_.search(voxel_key.first, voxel_key.second, 108 | voxels_per_side); 109 | 110 | for (const voxblox::VoxelKey& neighbor_key : neighbors) { 111 | TsdfBlock::Ptr neighbor_block = 112 | tsdf_layer_->getBlockPtrByIndex(neighbor_key.first); 113 | if (!neighbor_block) { 114 | continue; 115 | } 116 | TsdfVoxel& neighbor_voxel = 117 | neighbor_block->getVoxelByVoxelIndex(neighbor_key.second); 118 | 119 | // If neighbor is valid add it to the cluster, and potentially keep 120 | // growing if it is ever-free. 121 | if (!neighbor_voxel.clustering_processed && 122 | neighbor_voxel.last_lidar_occupied == frame_counter) { 123 | if (neighbor_voxel.ever_free || 124 | (tsdf_voxel.ever_free && config_.grow_clusters_twice)) { 125 | stack.push_back(neighbor_key); 126 | } else { 127 | // Add voxel to cluster. 128 | neighbor_voxel.dynamic = true; 129 | neighbor_voxel.clustering_processed = true; 130 | result.push_back(neighbor_key); 131 | } 132 | } 133 | } 134 | } 135 | return !result.empty(); 136 | } 137 | 138 | Clusters Clustering::inducePointClusters( 139 | const BlockToPointMap& point_map, 140 | const std::vector& voxel_cluster_indices) const { 141 | Clusters candidates; 142 | const int voxels_per_side = tsdf_layer_->voxels_per_side(); 143 | const float voxel_size = tsdf_layer_->voxel_size(); 144 | 145 | for (const auto& voxel_cluster : voxel_cluster_indices) { 146 | Cluster candidate_cluster; 147 | for (const voxblox::VoxelKey& voxel_key : voxel_cluster) { 148 | // Find the block. 149 | auto block_it = point_map.find(voxel_key.first); 150 | if (block_it == point_map.end()) { 151 | continue; 152 | } 153 | 154 | // Find the voxel. 155 | const VoxelToPointMap& voxel_map = block_it->second; 156 | auto voxel_it = voxel_map.find(voxel_key.second); 157 | if (voxel_it == voxel_map.end()) { 158 | continue; 159 | } 160 | 161 | // Add the voxel. 162 | const voxblox::Point center = voxblox::getCenterPointFromGridIndex( 163 | voxblox::getGlobalVoxelIndexFromBlockAndVoxelIndex( 164 | voxel_key.first, voxel_key.second, voxels_per_side), 165 | voxel_size); 166 | candidate_cluster.voxels.push_back( 167 | Point(center.x(), center.y(), center.z())); 168 | 169 | // Add all points. 170 | for (const auto& point_index : voxel_it->second) { 171 | candidate_cluster.points.push_back(point_index); 172 | } 173 | } 174 | candidates.push_back(candidate_cluster); 175 | } 176 | return candidates; 177 | } 178 | 179 | void Clustering::computeAABB(const Cloud& cloud, Cluster& cluster) const { 180 | if (cluster.points.empty()) { 181 | return; 182 | } 183 | Point& min = cluster.aabb.min_corner; 184 | Point& max = cluster.aabb.max_corner; 185 | if (config_.check_cluster_separation_exact) { 186 | // Compute the exact AABB from points. 187 | min = cloud[cluster.points[0]]; 188 | max = cloud[cluster.points[0]]; 189 | for (size_t i = 1; i < cluster.points.size(); ++i) { 190 | const Point& point = cloud[cluster.points[i]]; 191 | min.x = std::min(min.x, point.x); 192 | min.y = std::min(min.y, point.y); 193 | min.z = std::min(min.z, point.z); 194 | max.x = std::max(max.x, point.x); 195 | max.y = std::max(max.y, point.y); 196 | max.z = std::max(max.z, point.z); 197 | } 198 | } else { 199 | // Approximate the AABB from voxels. 200 | const float voxel_size = tsdf_layer_->voxel_size(); 201 | min = Point(std::numeric_limits::max(), 202 | std::numeric_limits::max(), 203 | std::numeric_limits::max()); 204 | max = Point(std::numeric_limits::lowest(), 205 | std::numeric_limits::lowest(), 206 | std::numeric_limits::lowest()); 207 | for (const Point& point : cluster.voxels) { 208 | min.x = std::min(min.x, point.x); 209 | min.y = std::min(min.y, point.y); 210 | min.z = std::min(min.z, point.z); 211 | max.x = std::max(max.x, point.x); 212 | max.y = std::max(max.y, point.y); 213 | max.z = std::max(max.z, point.z); 214 | } 215 | min.x -= 0.5f * voxel_size; 216 | min.y -= 0.5f * voxel_size; 217 | min.z -= 0.5f * voxel_size; 218 | max.x += 0.5f * voxel_size; 219 | max.y += 0.5f * voxel_size; 220 | max.z += 0.5f * voxel_size; 221 | } 222 | } 223 | 224 | void Clustering::mergeClusters(const Cloud& cloud, Clusters& clusters) const { 225 | if (config_.min_cluster_separation <= 0.f || clusters.size() < 2u) { 226 | return; 227 | } 228 | // Check all clusters versus all others. 229 | size_t first_id = 0u; 230 | while (true) { 231 | Cluster& first_cluster = clusters[first_id]; 232 | size_t second_id = first_id + 1u; 233 | while (true) { 234 | Cluster& second_cluster = clusters[second_id]; 235 | 236 | // Ignore clusters that are far apart. 237 | if (!first_cluster.aabb.intersects(second_cluster.aabb, 238 | config_.min_cluster_separation)) { 239 | second_id++; 240 | if (second_id >= clusters.size()) { 241 | break; 242 | } 243 | continue; 244 | } 245 | 246 | // Compute minimum distance between all points in both clusters. 247 | bool distance_met = false; 248 | if (config_.check_cluster_separation_exact) { 249 | // Compute distances for all points in both clusters. 250 | for (const int point_1 : first_cluster.points) { 251 | for (const int point_2 : second_cluster.points) { 252 | const float distance = (cloud[point_1].getVector3fMap() - 253 | cloud[point_2].getVector3fMap()) 254 | .norm(); 255 | if (distance <= config_.min_cluster_separation) { 256 | distance_met = true; 257 | break; 258 | } 259 | } 260 | if (distance_met) { 261 | break; 262 | } 263 | } 264 | } else { 265 | // Check approximate overlap of voxels. 266 | for (const Point& point_1 : first_cluster.voxels) { 267 | for (const Point& point_2 : second_cluster.voxels) { 268 | const float distance = 269 | (point_1.getVector3fMap() - point_2.getVector3fMap()).norm(); 270 | if (distance <= config_.min_cluster_separation) { 271 | distance_met = true; 272 | break; 273 | } 274 | } 275 | if (distance_met) { 276 | break; 277 | } 278 | } 279 | } 280 | 281 | // Merge clusters if necessary. 282 | if (distance_met) { 283 | first_cluster.points.insert(first_cluster.points.end(), 284 | second_cluster.points.begin(), 285 | second_cluster.points.end()); 286 | clusters.erase(clusters.begin() + second_id); 287 | computeAABB(cloud, first_cluster); 288 | } else { 289 | second_id++; 290 | } 291 | 292 | // Terminate if we reach the end of clusters. 293 | if (second_id == clusters.size()) { 294 | break; 295 | } 296 | } 297 | first_id++; 298 | if (first_id >= clusters.size() - 1u) { 299 | break; 300 | } 301 | } 302 | } 303 | 304 | void Clustering::applyClusterLevelFilters(Clusters& candidates) const { 305 | candidates.erase(std::remove_if(candidates.begin(), candidates.end(), 306 | [this](const Cluster& cluster) { 307 | return filterCluster(cluster); 308 | }), 309 | candidates.end()); 310 | } 311 | 312 | bool Clustering::filterCluster(const Cluster& cluster) const { 313 | // Check point count. 314 | const int cluster_size = static_cast(cluster.points.size()); 315 | if (cluster_size < config_.min_cluster_size || 316 | cluster_size > config_.max_cluster_size) { 317 | return true; 318 | } 319 | 320 | // Check extent. 321 | const float extent = cluster.aabb.extent(); 322 | if (extent < config_.min_extent || extent > config_.max_extent) { 323 | return true; 324 | } 325 | return false; 326 | } 327 | 328 | void Clustering::setClusterLevelDynamicFlagOfallPoints( 329 | const Clusters& clusters, CloudInfo& cloud_info) const { 330 | for (const Cluster& valid_cluster : clusters) { 331 | for (int idx : valid_cluster.points) { 332 | cloud_info.points[idx].cluster_level_dynamic = true; 333 | } 334 | } 335 | } 336 | 337 | } // namespace dynablox 338 | -------------------------------------------------------------------------------- /dynablox/src/processing/ever_free_integrator.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/processing/ever_free_integrator.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "dynablox/common/index_getter.h" 11 | 12 | namespace dynablox { 13 | 14 | using Timer = voxblox::timing::Timer; 15 | 16 | void EverFreeIntegrator::Config::checkParams() const { 17 | checkParamCond(neighbor_connectivity == 6 || neighbor_connectivity == 18 || 18 | neighbor_connectivity == 26, 19 | "'neighbor_connectivity' must be 6, 18, or 26."); 20 | checkParamGE(num_threads, 1, "num_threads"); 21 | checkParamGE(temporal_buffer, 0, "temporal_buffer"); 22 | } 23 | 24 | void EverFreeIntegrator::Config::setupParamsAndPrinting() { 25 | setupParam("counter_to_reset", &counter_to_reset, "frames"); 26 | setupParam("temporal_buffer", &temporal_buffer, "frames"); 27 | setupParam("burn_in_period", &burn_in_period); 28 | setupParam("tsdf_occupancy_threshold", &tsdf_occupancy_threshold, "m"); 29 | setupParam("neighbor_connectivity", &neighbor_connectivity); 30 | setupParam("num_threads", &num_threads); 31 | } 32 | 33 | EverFreeIntegrator::EverFreeIntegrator(const EverFreeIntegrator::Config& config, 34 | TsdfLayer::Ptr tsdf_layer) 35 | : config_(config.checkValid()), 36 | tsdf_layer_(std::move(tsdf_layer)), 37 | neighborhood_search_(config_.neighbor_connectivity), 38 | voxel_size_(tsdf_layer_->voxel_size()), 39 | voxels_per_side_(tsdf_layer_->voxels_per_side()), 40 | voxels_per_block_(voxels_per_side_ * voxels_per_side_ * 41 | voxels_per_side_) {} 42 | 43 | void EverFreeIntegrator::updateEverFreeVoxels(const int frame_counter) const { 44 | // Get all updated blocks. NOTE: we highjack the kESDF flag here for ever-free 45 | // tracking. 46 | voxblox::BlockIndexList updated_blocks; 47 | tsdf_layer_->getAllUpdatedBlocks(voxblox::Update::kEsdf, &updated_blocks); 48 | std::vector indices(updated_blocks.size()); 49 | for (size_t i = 0; i < indices.size(); ++i) { 50 | indices[i] = updated_blocks[i]; 51 | } 52 | 53 | // Update occupancy counter and calls removeEverFree if warranted in parallel 54 | // by block. 55 | voxblox::AlignedVector voxels_to_remove; 56 | std::mutex result_aggregation_mutex; 57 | IndexGetter index_getter(indices); 58 | std::vector> threads; 59 | Timer remove_timer("update_ever_free/remove_occupied"); 60 | for (int i = 0; i < config_.num_threads; ++i) { 61 | threads.emplace_back(std::async(std::launch::async, [&]() { 62 | BlockIndex index; 63 | voxblox::AlignedVector local_voxels_to_remove; 64 | 65 | // Process all blocks. 66 | while (index_getter.getNextIndex(&index)) { 67 | voxblox::AlignedVector voxels; 68 | if (blockWiseUpdateEverFree(index, frame_counter, voxels)) { 69 | local_voxels_to_remove.insert(local_voxels_to_remove.end(), 70 | voxels.begin(), voxels.end()); 71 | } 72 | } 73 | 74 | // Aggregate results. 75 | std::lock_guard lock(result_aggregation_mutex); 76 | voxels_to_remove.insert(voxels_to_remove.end(), 77 | local_voxels_to_remove.begin(), 78 | local_voxels_to_remove.end()); 79 | })); 80 | } 81 | for (auto& thread : threads) { 82 | thread.get(); 83 | } 84 | 85 | // Remove the remaining voxels single threaded. 86 | for (const auto& voxel_key : voxels_to_remove) { 87 | TsdfBlock::Ptr tsdf_block = 88 | tsdf_layer_->getBlockPtrByIndex(voxel_key.first); 89 | if (!tsdf_block) { 90 | continue; 91 | } 92 | TsdfVoxel& tsdf_voxel = tsdf_block->getVoxelByVoxelIndex(voxel_key.second); 93 | tsdf_voxel.ever_free = false; 94 | tsdf_voxel.dynamic = false; 95 | } 96 | remove_timer.Stop(); 97 | 98 | // Labels tsdf-updated voxels as ever-free if they satisfy the criteria. 99 | // Performed blockwise in parallel. 100 | index_getter.reset(); 101 | threads.clear(); 102 | Timer label_timer("update_ever_free/label_free"); 103 | for (int i = 0; i < config_.num_threads; ++i) { 104 | threads.emplace_back(std::async(std::launch::async, [&]() { 105 | BlockIndex index; 106 | while (index_getter.getNextIndex(&index)) { 107 | blockWiseMakeEverFree(index, frame_counter); 108 | } 109 | })); 110 | } 111 | for (auto& thread : threads) { 112 | thread.get(); 113 | } 114 | } 115 | 116 | bool EverFreeIntegrator::blockWiseUpdateEverFree( 117 | const BlockIndex& block_index, const int frame_counter, 118 | voxblox::AlignedVector& voxels_to_remove) const { 119 | TsdfBlock::Ptr tsdf_block = tsdf_layer_->getBlockPtrByIndex(block_index); 120 | if (!tsdf_block) { 121 | return false; 122 | } 123 | 124 | for (size_t index = 0; index < voxels_per_block_; ++index) { 125 | TsdfVoxel& tsdf_voxel = tsdf_block->getVoxelByLinearIndex(index); 126 | 127 | // Updating the occupancy counter. 128 | if (tsdf_voxel.distance < config_.tsdf_occupancy_threshold || 129 | tsdf_voxel.last_lidar_occupied == frame_counter) { 130 | updateOccupancyCounter(tsdf_voxel, frame_counter); 131 | } 132 | if (tsdf_voxel.last_lidar_occupied < 133 | frame_counter - config_.temporal_buffer) { 134 | tsdf_voxel.dynamic = false; 135 | } 136 | 137 | // Call to remove ever-free if warranted. 138 | if (tsdf_voxel.occ_counter >= config_.counter_to_reset && 139 | tsdf_voxel.ever_free) { 140 | const VoxelIndex voxel_index = 141 | tsdf_block->computeVoxelIndexFromLinearIndex(index); 142 | voxblox::AlignedVector voxels = 143 | removeEverFree(*tsdf_block, tsdf_voxel, block_index, voxel_index); 144 | voxels_to_remove.insert(voxels_to_remove.end(), voxels.begin(), 145 | voxels.end()); 146 | } 147 | } 148 | 149 | return !voxels_to_remove.empty(); 150 | } 151 | 152 | void EverFreeIntegrator::blockWiseMakeEverFree(const BlockIndex& block_index, 153 | const int frame_counter) const { 154 | TsdfBlock::Ptr tsdf_block = tsdf_layer_->getBlockPtrByIndex(block_index); 155 | if (!tsdf_block) { 156 | return; 157 | } 158 | 159 | // Check all voxels. 160 | for (size_t index = 0; index < voxels_per_block_; ++index) { 161 | TsdfVoxel& tsdf_voxel = tsdf_block->getVoxelByLinearIndex(index); 162 | 163 | // If already ever-free we can save the cost of checking the neighbourhood. 164 | // Only observed voxels (with weight) can be set to ever free. 165 | // Voxel must be unoccupied for the last burn_in_period frames and 166 | // TSDF-value must be larger than 3/2 voxel_size 167 | if (tsdf_voxel.ever_free || tsdf_voxel.weight <= 1e-6 || 168 | tsdf_voxel.last_occupied > frame_counter - config_.burn_in_period) { 169 | continue; 170 | } 171 | 172 | // Check the neighbourhood for unobserved or occupied voxels. 173 | const VoxelIndex voxel_index = 174 | tsdf_block->computeVoxelIndexFromLinearIndex(index); 175 | 176 | voxblox::AlignedVector neighbors = 177 | neighborhood_search_.search(block_index, voxel_index, voxels_per_side_); 178 | 179 | bool neighbor_occupied_or_unobserved = false; 180 | 181 | for (const voxblox::VoxelKey& neighbor_key : neighbors) { 182 | const TsdfBlock* neighbor_block; 183 | if (neighbor_key.first == block_index) { 184 | // Often will be the same block. 185 | neighbor_block = tsdf_block.get(); 186 | } else { 187 | neighbor_block = 188 | tsdf_layer_->getBlockPtrByIndex(neighbor_key.first).get(); 189 | if (neighbor_block == nullptr) { 190 | // Block does not exist. 191 | neighbor_occupied_or_unobserved = true; 192 | break; 193 | } 194 | } 195 | 196 | // Check the voxel if it is unobserved or static. 197 | const TsdfVoxel& neighbor_voxel = 198 | neighbor_block->getVoxelByVoxelIndex(neighbor_key.second); 199 | if (neighbor_voxel.weight < 1e-6 || 200 | neighbor_voxel.last_occupied > 201 | frame_counter - config_.burn_in_period) { 202 | neighbor_occupied_or_unobserved = true; 203 | break; 204 | } 205 | } 206 | 207 | // Only observed free space, can be labeled as ever-free. 208 | if (!neighbor_occupied_or_unobserved) { 209 | tsdf_voxel.ever_free = true; 210 | } 211 | } 212 | tsdf_block->updated().reset(voxblox::Update::kEsdf); 213 | } 214 | 215 | voxblox::AlignedVector EverFreeIntegrator::removeEverFree( 216 | TsdfBlock& block, TsdfVoxel& voxel, const BlockIndex& block_index, 217 | const VoxelIndex& voxel_index) const { 218 | // Remove ever-free attributes. 219 | voxel.ever_free = false; 220 | voxel.dynamic = false; 221 | 222 | // Remove ever-free attribute also from neighbouring voxels. 223 | voxblox::AlignedVector neighbors = 224 | neighborhood_search_.search(block_index, voxel_index, voxels_per_side_); 225 | voxblox::AlignedVector voxels_to_remove; 226 | 227 | for (const voxblox::VoxelKey& neighbor_key : neighbors) { 228 | if (neighbor_key.first == block_index) { 229 | // Since this is executed in parallel only modify this block. 230 | TsdfVoxel& neighbor_voxel = 231 | block.getVoxelByVoxelIndex(neighbor_key.second); 232 | neighbor_voxel.ever_free = false; 233 | neighbor_voxel.dynamic = false; 234 | } else { 235 | // Otherwise mark the voxel for later clean-up. 236 | voxels_to_remove.push_back(neighbor_key); 237 | } 238 | } 239 | 240 | return voxels_to_remove; 241 | } 242 | 243 | void EverFreeIntegrator::updateOccupancyCounter(TsdfVoxel& voxel, 244 | const int frame_counter) const { 245 | // Allow for breaks of temporal_buffer between occupied observations to 246 | // compensate for lidar sparsity. 247 | if (voxel.last_occupied >= frame_counter - config_.temporal_buffer) { 248 | voxel.occ_counter++; 249 | } else { 250 | voxel.occ_counter = 1; 251 | } 252 | voxel.last_occupied = frame_counter; 253 | } 254 | 255 | } // namespace dynablox 256 | -------------------------------------------------------------------------------- /dynablox/src/processing/preprocessing.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/processing/preprocessing.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace dynablox { 8 | 9 | void Preprocessing::Config::checkParams() const { 10 | checkParamGT(min_range, 0.f, "min_range"); 11 | checkParamCond(max_range > min_range, 12 | "'max_range' must be larger than 'min_range'."); 13 | } 14 | 15 | void Preprocessing::Config::setupParamsAndPrinting() { 16 | setupParam("min_range", &min_range, "m"); 17 | setupParam("max_range", &max_range, "m"); 18 | } 19 | 20 | Preprocessing::Preprocessing(const Config& config) 21 | : config_(config.checkValid()) {} 22 | 23 | bool Preprocessing::processPointcloud(const sensor_msgs::PointCloud2::Ptr& msg, 24 | const tf::StampedTransform T_M_S, 25 | Cloud& cloud, 26 | CloudInfo& cloud_info) const { 27 | // Convert to ROS msg to pcl cloud. 28 | pcl::fromROSMsg(*msg, cloud); 29 | 30 | // Populate the cloud information with data for all points. 31 | cloud_info.timestamp = msg->header.stamp.toNSec(); 32 | cloud_info.sensor_position.x = T_M_S.getOrigin().x(); 33 | cloud_info.sensor_position.y = T_M_S.getOrigin().y(); 34 | cloud_info.sensor_position.z = T_M_S.getOrigin().z(); 35 | 36 | cloud_info.points = std::vector(cloud.size()); 37 | size_t i = 0; 38 | for (const auto& point : cloud) { 39 | const float norm = 40 | std::sqrt(point.x * point.x + point.y * point.y + point.z * point.z); 41 | PointInfo& info = cloud_info.points.at(i); 42 | info.distance_to_sensor = norm; 43 | i++; 44 | } 45 | 46 | // Transform the cloud to world frame. 47 | pcl_ros::transformPointCloud(cloud, cloud, T_M_S); 48 | return true; 49 | } 50 | 51 | } // namespace dynablox 52 | -------------------------------------------------------------------------------- /dynablox/src/processing/tracking.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox/processing/tracking.h" 2 | 3 | namespace dynablox { 4 | 5 | void Tracking::Config::checkParams() const {} 6 | 7 | void Tracking::Config::setupParamsAndPrinting() { 8 | setupParam("min_track_duration", &min_track_duration, "frames"); 9 | setupParam("max_tracking_distance", &max_tracking_distance, "m"); 10 | } 11 | 12 | Tracking::Tracking(const Config& config) : config_(config.checkValid()) {} 13 | 14 | void Tracking::track(const Cloud& cloud, Clusters& clusters, 15 | CloudInfo& cloud_info) { 16 | // Associate current to previous cluster ids. 17 | trackClusterIDs(cloud, clusters); 18 | 19 | // Label the cloud info. 20 | for (Cluster& cluster : clusters) { 21 | if (cluster.track_length >= config_.min_track_duration) { 22 | cluster.valid = true; 23 | for (int idx : cluster.points) { 24 | cloud_info.points[idx].object_level_dynamic = true; 25 | } 26 | } 27 | } 28 | } 29 | 30 | void Tracking::trackClusterIDs(const Cloud& cloud, Clusters& clusters) { 31 | // Compute the centroids of all clusters. 32 | std::vector centroids(clusters.size()); 33 | size_t i = 0; 34 | for (const Cluster& cluster : clusters) { 35 | voxblox::Point centroid = {0, 0, 0}; 36 | for (int index : cluster.points) { 37 | const Point& point = cloud[index]; 38 | centroid = centroid + voxblox::Point(point.x, point.y, point.z); 39 | } 40 | centroids[i] = centroid / cluster.points.size(); 41 | ++i; 42 | } 43 | 44 | // Compute the distances of all clusters. [previous][current]->dist 45 | struct Association { 46 | float distance; 47 | int previous_id; 48 | int current_id; 49 | }; 50 | 51 | std::vector> distances(previous_centroids_.size()); 52 | for (size_t i = 0; i < previous_centroids_.size(); ++i) { 53 | std::vector& d = distances[i]; 54 | d.reserve(centroids.size()); 55 | for (size_t j = 0; j < centroids.size(); ++j) { 56 | Association association; 57 | association.distance = (previous_centroids_[i] - centroids[j]).norm(); 58 | association.previous_id = i; 59 | association.current_id = j; 60 | d.push_back(association); 61 | } 62 | } 63 | 64 | // Associate all previous ids until no more minimum distances exist. 65 | std::unordered_set reused_ids; 66 | while (true) { 67 | // Find the minimum distance and IDs (exhaustively). 68 | float min = std::numeric_limits::max(); 69 | int prev_id = 0; 70 | int curr_id = 0; 71 | int erase_i = 0; 72 | int erase_j = 0; 73 | for (size_t i = 0u; i < distances.size(); ++i) { 74 | for (size_t j = 0u; j < distances[i].size(); ++j) { 75 | const Association& association = distances[i][j]; 76 | if (association.distance < min) { 77 | min = association.distance; 78 | curr_id = association.current_id; 79 | prev_id = association.previous_id; 80 | erase_i = i; 81 | erase_j = j; 82 | } 83 | } 84 | } 85 | 86 | if (min > config_.max_tracking_distance) { 87 | // no more good fits. 88 | break; 89 | } 90 | 91 | // Update traked cluster and remove that match to search for next best. 92 | clusters[curr_id].id = previous_ids_[prev_id]; 93 | clusters[curr_id].track_length = previous_track_lengths_[prev_id] + 1; 94 | reused_ids.insert(previous_ids_[prev_id]); 95 | distances.erase(distances.begin() + erase_i); 96 | for (auto& vec : distances) { 97 | vec.erase(vec.begin() + erase_j); 98 | } 99 | } 100 | 101 | // Fill in all remaining ids and track data. 102 | previous_centroids_ = centroids; 103 | previous_ids_.clear(); 104 | previous_ids_.reserve(clusters.size()); 105 | previous_track_lengths_.clear(); 106 | previous_ids_.reserve(clusters.size()); 107 | 108 | int id_counter = 0; 109 | for (Cluster& cluster : clusters) { 110 | if (cluster.id == -1) { 111 | // We need to replace it. 112 | while (reused_ids.find(id_counter) != reused_ids.end()) { 113 | id_counter++; 114 | } 115 | cluster.id = id_counter; 116 | id_counter++; 117 | } 118 | previous_ids_.push_back(cluster.id); 119 | previous_track_lengths_.push_back(cluster.track_length); 120 | } 121 | } 122 | 123 | } // namespace dynablox 124 | -------------------------------------------------------------------------------- /dynablox_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(dynablox_ros) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | catkin_package() 7 | add_definitions(-std=c++17 -Wall -Wextra) 8 | 9 | cs_add_library(${PROJECT_NAME} 10 | src/visualization/motion_visualizer.cpp 11 | src/visualization/cloud_visualizer.cpp 12 | src/motion_detector.cpp 13 | ) 14 | 15 | cs_add_executable(motion_detector 16 | src/motion_detector_node.cpp 17 | ) 18 | target_link_libraries(motion_detector ${PROJECT_NAME}) 19 | 20 | cs_add_executable(cloud_visualizer 21 | src/cloud_visualizer_node.cpp 22 | ) 23 | target_link_libraries(cloud_visualizer ${PROJECT_NAME}) 24 | 25 | cs_install() 26 | cs_export() 27 | -------------------------------------------------------------------------------- /dynablox_ros/config/motion_detector/default.yaml: -------------------------------------------------------------------------------- 1 | # ----------- Motion Detector Parameters -------- 2 | # General. 3 | #evaluate: true # Set by launch file 4 | #visualize: true # Set by launch file 5 | #num_threads: 1 # uses hardware concurrency if left empty. 6 | queue_size: 20 7 | shutdown_after: 10 # number evaluations. 8 | 9 | # Preprocessing. 10 | preprocessing: 11 | min_range: &min_range 0.5 # m 12 | max_range: &max_range 20 # m 13 | 14 | # Ever-Free Integration. 15 | ever_free_integrator: 16 | counter_to_reset: 150 # Observations to un-free an ever-free voxel [frames] 17 | temporal_buffer: 2 # To compensate sparsity [frames]. 18 | burn_in_period: 5 # Burn in before becoming ever-free [frames]. 19 | tsdf_occupancy_threshold: 0.3 # 1.5 voxel sizes. 20 | neighbor_connectivity: 26 21 | 22 | # Clustering. 23 | clustering: 24 | min_cluster_size: 20 25 | max_cluster_size: 200000 26 | min_extent: 0 # m 27 | max_extent: 200000 # m 28 | neighbor_connectivity: 6 29 | grow_clusters_twice: false 30 | min_cluster_separation: 0.2 31 | 32 | # Tracking. 33 | tracking: 34 | min_track_duration: 0 # frames 35 | max_tracking_distance: 1 # m 36 | 37 | # Evaluation. 38 | evaluation: 39 | min_range: *min_range # m 40 | max_range: *max_range # m 41 | evaluate_point_level: true 42 | evaluate_cluster_level: true 43 | evaluate_object_level: true 44 | save_clouds: true # For detailed inspection of results. 45 | 46 | # Visualization. 47 | visualization: 48 | static_point_color: [0,0,0,1] 49 | dynamic_point_color: [1,0,0.5,1] 50 | out_of_bounds_color: [0,0,0,0.1] 51 | ever_free_color: [1, 0, 1, 0.5] 52 | never_free_color: [0, 1, 1, 0.5] 53 | static_point_scale: 0.03 54 | dynamic_point_scale: 0.08 55 | sensor_scale: 0.5 56 | color_clusters: true 57 | color_wheel_num_colors: 20 58 | slice_height: -1 59 | visualization_max_z: 100 60 | 61 | # Voxblox parameters. 62 | voxblox: 63 | # SDF. 64 | tsdf_voxel_size: 0.2 65 | truncation_distance: 0.4 66 | tsdf_voxels_per_side: 16 67 | max_weight: 1000 68 | 69 | # Integration. 70 | method: projective 71 | sensor_horizontal_resolution: 2048 72 | sensor_vertical_resolution: 64 73 | sensor_vertical_field_of_view_degrees: 33.22222 74 | min_ray_length_m: *min_range 75 | max_ray_length_m: *max_range 76 | use_const_weight: true 77 | verbose: false 78 | -------------------------------------------------------------------------------- /dynablox_ros/config/rviz/cloud_visualizer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Object1 8 | - /Object1/Namespaces1 9 | - /MarkerArray1 10 | - /MarkerArray1/Namespaces1 11 | Splitter Ratio: 0.5382353067398071 12 | Tree Height: 631 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 1 39 | Class: rviz/Axes 40 | Enabled: true 41 | Length: 1 42 | Name: Map Origin 43 | Radius: 0.10000000149011612 44 | Reference Frame: 45 | Show Trail: false 46 | Value: true 47 | - Class: rviz/Marker 48 | Enabled: false 49 | Marker Topic: /cloud_visualizer/ground_truth/point 50 | Name: Point 51 | Namespaces: 52 | {} 53 | Queue Size: 100 54 | Value: false 55 | - Class: rviz/Marker 56 | Enabled: false 57 | Marker Topic: /cloud_visualizer/ground_truth/cluster 58 | Name: Cluster 59 | Namespaces: 60 | {} 61 | Queue Size: 100 62 | Value: false 63 | - Class: rviz/Marker 64 | Enabled: true 65 | Marker Topic: /cloud_visualizer/ground_truth/object 66 | Name: Object 67 | Namespaces: 68 | cloud_0: true 69 | cloud_1: false 70 | cloud_2: false 71 | cloud_3: false 72 | cloud_4: false 73 | cloud_5: false 74 | cloud_6: false 75 | cloud_7: false 76 | cloud_8: false 77 | cloud_9: false 78 | Queue Size: 100 79 | Value: true 80 | - Class: rviz/MarkerArray 81 | Enabled: true 82 | Marker Topic: /cloud_visualizer/clusters 83 | Name: MarkerArray 84 | Namespaces: 85 | cloud_0: true 86 | cloud_1: false 87 | cloud_2: false 88 | cloud_3: false 89 | cloud_4: false 90 | cloud_5: false 91 | cloud_6: false 92 | cloud_7: false 93 | cloud_8: false 94 | cloud_9: false 95 | Queue Size: 100 96 | Value: true 97 | Enabled: true 98 | Global Options: 99 | Background Color: 255; 255; 255 100 | Default Light: true 101 | Fixed Frame: map 102 | Frame Rate: 30 103 | Name: root 104 | Tools: 105 | - Class: rviz/Interact 106 | Hide Inactive Objects: true 107 | - Class: rviz/MoveCamera 108 | - Class: rviz/Select 109 | - Class: rviz/FocusCamera 110 | - Class: rviz/Measure 111 | - Class: rviz/SetInitialPose 112 | Theta std deviation: 0.2617993950843811 113 | Topic: /initialpose 114 | X std deviation: 0.5 115 | Y std deviation: 0.5 116 | - Class: rviz/SetGoal 117 | Topic: /move_base_simple/goal 118 | - Class: rviz/PublishPoint 119 | Single click: true 120 | Topic: /clicked_point 121 | Value: true 122 | Views: 123 | Current: 124 | Class: rviz/Orbit 125 | Distance: 57.858673095703125 126 | Enable Stereo Rendering: 127 | Stereo Eye Separation: 0.05999999865889549 128 | Stereo Focal Distance: 1 129 | Swap Stereo Eyes: false 130 | Value: false 131 | Field of View: 0.7853981852531433 132 | Focal Point: 133 | X: 4.652673244476318 134 | Y: 3.1562368869781494 135 | Z: -18.15682029724121 136 | Focal Shape Fixed Size: true 137 | Focal Shape Size: 0.05000000074505806 138 | Invert Z Axis: false 139 | Name: Current View 140 | Near Clip Distance: 0.009999999776482582 141 | Pitch: 1.0047969818115234 142 | Target Frame: map 143 | Yaw: 3.876859426498413 144 | Saved: ~ 145 | Window Geometry: 146 | Displays: 147 | collapsed: false 148 | Height: 1016 149 | Hide Left Dock: false 150 | Hide Right Dock: true 151 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000033efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001d9000002d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000002d8000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007400000005afc0100000002fb0000000800540069006d0065010000000000000740000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e40000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 152 | Selection: 153 | collapsed: false 154 | Time: 155 | collapsed: false 156 | Tool Properties: 157 | collapsed: false 158 | Views: 159 | collapsed: true 160 | Width: 1856 161 | X: 64 162 | Y: 27 163 | -------------------------------------------------------------------------------- /dynablox_ros/config/rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Input Data1 8 | - /Visualization1 9 | - /Visualization1/Sensor1 10 | - /Visualization1/Slice1 11 | - /Visualization1/Volume1 12 | - /Detection1 13 | - /Ground Truth1 14 | Splitter Ratio: 0.9548872113227844 15 | Tree Height: 980 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 1 42 | Autocompute Intensity Bounds: true 43 | Autocompute Value Bounds: 44 | Max Value: 10 45 | Min Value: -10 46 | Value: true 47 | Axis: Z 48 | Channel Name: intensity 49 | Class: rviz/PointCloud2 50 | Color: 255; 255; 255 51 | Color Transformer: RGB8 52 | Decay Time: 0 53 | Enabled: false 54 | Invert Rainbow: false 55 | Max Color: 0; 0; 0 56 | Min Color: 0; 0; 0 57 | Name: static lidar_points 58 | Position Transformer: XYZ 59 | Queue Size: 10 60 | Selectable: true 61 | Size (Pixels): 1 62 | Size (m): 0.009999999776482582 63 | Style: Points 64 | Topic: /dynablox/point_cloud_without_dynamic_points 65 | Unreliable: false 66 | Use Fixed Frame: true 67 | Use rainbow: false 68 | Value: false 69 | - Class: rviz/Group 70 | Displays: 71 | - Alpha: 1 72 | Autocompute Intensity Bounds: true 73 | Autocompute Value Bounds: 74 | Max Value: 10 75 | Min Value: -10 76 | Value: true 77 | Axis: Z 78 | Channel Name: intensity 79 | Class: rviz/PointCloud2 80 | Color: 0; 0; 0 81 | Color Transformer: FlatColor 82 | Decay Time: 0 83 | Enabled: false 84 | Invert Rainbow: false 85 | Max Color: 255; 255; 255 86 | Min Color: 0; 0; 0 87 | Name: Lidar Output Points 88 | Position Transformer: XYZ 89 | Queue Size: 10 90 | Selectable: true 91 | Size (Pixels): 1 92 | Size (m): 0.009999999776482582 93 | Style: Points 94 | Topic: /lidar_undistortion/pointcloud_corrected 95 | Unreliable: false 96 | Use Fixed Frame: true 97 | Use rainbow: true 98 | Value: false 99 | - Alpha: 1 100 | Class: rviz/Axes 101 | Enabled: false 102 | Length: 1 103 | Name: Map Origin 104 | Radius: 0.10000000149011612 105 | Reference Frame: 106 | Show Trail: false 107 | Value: false 108 | - Alpha: 1 109 | Class: rviz/Axes 110 | Enabled: false 111 | Length: 0.6000000238418579 112 | Name: Sensor GT 113 | Radius: 0.05000000074505806 114 | Reference Frame: os_lidar 115 | Show Trail: false 116 | Value: false 117 | Enabled: true 118 | Name: Input Data 119 | - Class: rviz/Group 120 | Displays: 121 | - Class: rviz/Group 122 | Displays: 123 | - Class: rviz/Marker 124 | Enabled: true 125 | Marker Topic: /motion_detector/visualization/lidar_pose 126 | Name: Lidar Pose 127 | Namespaces: 128 | "": true 129 | Queue Size: 100 130 | Value: true 131 | - Class: rviz/Marker 132 | Enabled: false 133 | Marker Topic: /motion_detector/visualization/lidar_points 134 | Name: Lidar Points 135 | Namespaces: 136 | {} 137 | Queue Size: 100 138 | Value: false 139 | Enabled: true 140 | Name: Sensor 141 | - Class: rviz/Group 142 | Displays: 143 | - Class: rviz/Marker 144 | Enabled: true 145 | Marker Topic: /motion_detector/visualization/slice/ever_free 146 | Name: Ever Free 147 | Namespaces: 148 | {} 149 | Queue Size: 100 150 | Value: true 151 | - Class: rviz/Marker 152 | Enabled: true 153 | Marker Topic: /motion_detector/visualization/slice/never_free 154 | Name: Never Free 155 | Namespaces: 156 | {} 157 | Queue Size: 100 158 | Value: true 159 | - Class: rviz/Marker 160 | Enabled: false 161 | Marker Topic: /motion_detector/visualization/slice/points 162 | Name: Points 163 | Namespaces: 164 | {} 165 | Queue Size: 100 166 | Value: false 167 | - Alpha: 0.5 168 | Autocompute Intensity Bounds: true 169 | Autocompute Value Bounds: 170 | Max Value: 10 171 | Min Value: -10 172 | Value: true 173 | Axis: Z 174 | Channel Name: intensity 175 | Class: rviz/PointCloud2 176 | Color: 255; 255; 255 177 | Color Transformer: Intensity 178 | Decay Time: 0 179 | Enabled: false 180 | Invert Rainbow: false 181 | Max Color: 255; 255; 255 182 | Min Color: 0; 0; 0 183 | Name: TSDF 184 | Position Transformer: XYZ 185 | Queue Size: 10 186 | Selectable: true 187 | Size (Pixels): 3 188 | Size (m): 0.20000000298023224 189 | Style: Boxes 190 | Topic: /motion_detector/visualization/slice/tsdf 191 | Unreliable: false 192 | Use Fixed Frame: true 193 | Use rainbow: true 194 | Value: false 195 | Enabled: false 196 | Name: Slice 197 | - Class: rviz/Group 198 | Displays: 199 | - Class: rviz/Marker 200 | Enabled: true 201 | Marker Topic: /motion_detector/visualization/ever_free 202 | Name: Ever Free 203 | Namespaces: 204 | {} 205 | Queue Size: 100 206 | Value: true 207 | - Class: rviz/Marker 208 | Enabled: true 209 | Marker Topic: /motion_detector/visualization/never_free 210 | Name: Never Free 211 | Namespaces: 212 | {} 213 | Queue Size: 100 214 | Value: true 215 | Enabled: false 216 | Name: Volume 217 | - Class: voxblox_rviz_plugin/VoxbloxMesh 218 | Enabled: false 219 | Name: Mesh 220 | Queue Size: 10 221 | Topic: /motion_detector/visualization/mesh 222 | Unreliable: false 223 | Value: false 224 | Enabled: true 225 | Name: Visualization 226 | - Class: rviz/Group 227 | Displays: 228 | - Class: rviz/Group 229 | Displays: 230 | - Class: rviz/Marker 231 | Enabled: true 232 | Marker Topic: /motion_detector/visualization/detections/point/dynamic 233 | Name: Dynamic 234 | Namespaces: 235 | {} 236 | Queue Size: 100 237 | Value: true 238 | - Class: rviz/Marker 239 | Enabled: true 240 | Marker Topic: /motion_detector/visualization/detections/point/static 241 | Name: Static 242 | Namespaces: 243 | {} 244 | Queue Size: 100 245 | Value: true 246 | Enabled: false 247 | Name: Points 248 | - Class: rviz/Group 249 | Displays: 250 | - Class: rviz/Marker 251 | Enabled: true 252 | Marker Topic: /motion_detector/visualization/detections/cluster/dynamic 253 | Name: Dynamic 254 | Namespaces: 255 | {} 256 | Queue Size: 100 257 | Value: true 258 | - Class: rviz/Marker 259 | Enabled: true 260 | Marker Topic: /motion_detector/visualization/detections/cluster/static 261 | Name: Static 262 | Namespaces: 263 | {} 264 | Queue Size: 100 265 | Value: true 266 | Enabled: false 267 | Name: Clusters 268 | - Class: rviz/Group 269 | Displays: 270 | - Class: rviz/Marker 271 | Enabled: true 272 | Marker Topic: /motion_detector/visualization/detections/object/dynamic 273 | Name: Dynamic 274 | Namespaces: 275 | "": true 276 | Queue Size: 100 277 | Value: true 278 | - Class: rviz/Marker 279 | Enabled: true 280 | Marker Topic: /motion_detector/visualization/detections/object/static 281 | Name: Static 282 | Namespaces: 283 | "": true 284 | Queue Size: 100 285 | Value: true 286 | Enabled: true 287 | Name: Objects 288 | Enabled: true 289 | Name: Detection 290 | - Class: rviz/Group 291 | Displays: 292 | - Class: rviz/Marker 293 | Enabled: true 294 | Marker Topic: /motion_detector/visualization/lidar_points 295 | Name: Ground Truth 296 | Namespaces: 297 | {} 298 | Queue Size: 100 299 | Value: true 300 | Enabled: false 301 | Name: Ground Truth 302 | Enabled: true 303 | Global Options: 304 | Background Color: 255; 255; 255 305 | Default Light: true 306 | Fixed Frame: map 307 | Frame Rate: 30 308 | Name: root 309 | Tools: 310 | - Class: rviz/Interact 311 | Hide Inactive Objects: true 312 | - Class: rviz/MoveCamera 313 | - Class: rviz/Select 314 | - Class: rviz/FocusCamera 315 | - Class: rviz/Measure 316 | - Class: rviz/SetInitialPose 317 | Theta std deviation: 0.2617993950843811 318 | Topic: /initialpose 319 | X std deviation: 0.5 320 | Y std deviation: 0.5 321 | - Class: rviz/SetGoal 322 | Topic: /move_base_simple/goal 323 | - Class: rviz/PublishPoint 324 | Single click: true 325 | Topic: /clicked_point 326 | Value: true 327 | Views: 328 | Current: 329 | Class: rviz/Orbit 330 | Distance: 49.86338806152344 331 | Enable Stereo Rendering: 332 | Stereo Eye Separation: 0.05999999865889549 333 | Stereo Focal Distance: 1 334 | Swap Stereo Eyes: false 335 | Value: false 336 | Field of View: 0.7853981852531433 337 | Focal Point: 338 | X: -12.735674858093262 339 | Y: -3.735276460647583 340 | Z: -16.775846481323242 341 | Focal Shape Fixed Size: true 342 | Focal Shape Size: 0.05000000074505806 343 | Invert Z Axis: false 344 | Name: Current View 345 | Near Clip Distance: 0.009999999776482582 346 | Pitch: 1.2147971391677856 347 | Target Frame: map 348 | Yaw: 0.7368373870849609 349 | Saved: ~ 350 | Window Geometry: 351 | Displays: 352 | collapsed: false 353 | Height: 1359 354 | Hide Left Dock: false 355 | Hide Right Dock: true 356 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000499fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000780000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000499000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001d9000002d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000002d8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009f40000005afc0100000002fb0000000800540069006d00650100000000000009f40000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000008980000049900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 357 | Selection: 358 | collapsed: false 359 | Time: 360 | collapsed: false 361 | Tool Properties: 362 | collapsed: false 363 | Views: 364 | collapsed: true 365 | Width: 2548 366 | X: -32 367 | Y: -32 368 | -------------------------------------------------------------------------------- /dynablox_ros/include/dynablox_ros/motion_detector.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_ROS_MOTION_DETECTOR_H_ 2 | #define DYNABLOX_ROS_MOTION_DETECTOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "dynablox/3rd_party/config_utilities.hpp" 19 | #include "dynablox/common/index_getter.h" 20 | #include "dynablox/common/types.h" 21 | #include "dynablox/evaluation/evaluator.h" 22 | #include "dynablox/evaluation/ground_truth_handler.h" 23 | #include "dynablox/processing/clustering.h" 24 | #include "dynablox/processing/ever_free_integrator.h" 25 | #include "dynablox/processing/preprocessing.h" 26 | #include "dynablox/processing/tracking.h" 27 | #include "dynablox_ros/visualization/motion_visualizer.h" 28 | 29 | namespace dynablox { 30 | 31 | class MotionDetector { 32 | public: 33 | // Config. 34 | struct Config : public config_utilities::Config { 35 | // If true evaluate the performance against GT. 36 | bool evaluate = false; 37 | 38 | // Enable helper and debug visualizations. 39 | bool visualize = true; 40 | 41 | // Print additional information when running. 42 | bool verbose = true; 43 | 44 | // Frame names. 45 | std::string global_frame_name = "map"; 46 | std::string sensor_frame_name = 47 | ""; // Takes msg header if empty, overrides msg header if set. 48 | 49 | // Subscriber queue size. 50 | int queue_size = 1; 51 | 52 | // Number of threads to use. 53 | int num_threads = std::thread::hardware_concurrency(); 54 | 55 | // If >0, shutdown after this many evaluated frames. 56 | int shutdown_after = 0; 57 | 58 | Config() { setConfigName("MotionDetector"); } 59 | 60 | protected: 61 | void setupParamsAndPrinting() override; 62 | void checkParams() const override; 63 | }; 64 | 65 | // Constructor. 66 | MotionDetector(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 67 | 68 | // Setup. 69 | void setupMembers(); 70 | void setupRos(); 71 | 72 | // Callbacks. 73 | void pointcloudCallback(const sensor_msgs::PointCloud2::Ptr& msg); 74 | 75 | // Motion detection pipeline. 76 | bool lookupTransform(const std::string& target_frame, 77 | const std::string& source_frame, uint64_t timestamp, 78 | tf::StampedTransform& result) const; 79 | 80 | /** 81 | * @brief Create a mapping of each voxel index to the points it contains. Each 82 | * point will be checked whether it falls into an ever-free voxel and updates 83 | * voxel occupancy, since we go through voxels anyways already. 84 | * 85 | * @param cloud Complete point cloud to look up positions. 86 | * @param point_map Resulting map. 87 | * @param occupied_ever_free_voxel_indices Indices of voxels containing 88 | * ever-free points. 89 | * @param cloud_info Cloud info to store ever-free flags of checked points. 90 | */ 91 | void setUpPointMap( 92 | const Cloud& cloud, BlockToPointMap& point_map, 93 | std::vector& occupied_ever_free_voxel_indices, 94 | CloudInfo& cloud_info) const; 95 | 96 | /** 97 | * @brief Create a mapping of each block to ids of points that fall into it. 98 | * 99 | * @param cloud Points to process. 100 | * @return Mapping of block to point ids in cloud. 101 | */ 102 | voxblox::HierarchicalIndexIntMap buildBlockToPointsMap( 103 | const Cloud& cloud) const; 104 | 105 | /** 106 | * @brief Create a mapping of each voxel index to the points it contains. Each 107 | * point will be checked whether it falls into an ever-free voxel and updates 108 | * voxel occupancy, since we go through voxels anyways already. This function 109 | * operates on a single block for data parallelism. 110 | * 111 | * @param cloud Complete point cloud to look up positions. 112 | * @param block_index Index of the block to be processed. 113 | * @param points_in_block Indices of all points in the block. 114 | * @param point_map Where to store the resulting point map for this block. 115 | * @param occupied_ever_free_voxel_indices Where to store the indices of ever 116 | * free voxels in this block. 117 | * @param cloud_info Cloud info to store ever-free flags of checked points. 118 | */ 119 | void blockwiseBuildPointMap( 120 | const Cloud& cloud, const BlockIndex& block_index, 121 | const voxblox::AlignedVector& points_in_block, 122 | VoxelToPointMap& point_map, 123 | std::vector& occupied_ever_free_voxel_indices, 124 | CloudInfo& cloud_info) const; 125 | 126 | private: 127 | const Config config_; 128 | 129 | // ROS. 130 | ros::NodeHandle nh_; 131 | ros::NodeHandle nh_private_; 132 | ros::Subscriber lidar_pcl_sub_; 133 | tf::TransformListener tf_listener_; 134 | 135 | // Voxblox map. 136 | std::shared_ptr tsdf_server_; 137 | std::shared_ptr tsdf_layer_; 138 | 139 | // Processing. 140 | std::shared_ptr preprocessing_; 141 | std::shared_ptr ever_free_integrator_; 142 | std::shared_ptr clustering_; 143 | std::shared_ptr tracking_; 144 | std::shared_ptr evaluator_; 145 | std::shared_ptr visualizer_; 146 | 147 | // Cached data. 148 | size_t voxels_per_side_; 149 | size_t voxels_per_block_; 150 | 151 | // Variables. 152 | int frame_counter_ = 0; 153 | }; 154 | 155 | } // namespace dynablox 156 | 157 | #endif // DYNABLOX_ROS_MOTION_DETECTOR_H_ 158 | -------------------------------------------------------------------------------- /dynablox_ros/include/dynablox_ros/visualization/cloud_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_ROS_VISUALIZATION_CLOUD_VISUALIZER_H_ 2 | #define DYNABLOX_ROS_VISUALIZATION_CLOUD_VISUALIZER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "dynablox/3rd_party/config_utilities.hpp" 8 | #include "dynablox/common/types.h" 9 | #include "dynablox_ros/visualization/motion_visualizer.h" 10 | 11 | namespace dynablox { 12 | 13 | class CloudVisualizer { 14 | public: 15 | // Config. 16 | struct Config : public config_utilities::Config { 17 | // File to load cloud data from. 18 | std::string file_path; 19 | 20 | // How frequently visualizations should be republished [s]. 21 | float refresh_rate = 0.25; 22 | 23 | Config() { setConfigName("CloudVisualizer"); } 24 | 25 | protected: 26 | void setupParamsAndPrinting() override; 27 | void checkParams() const override; 28 | }; 29 | 30 | // Setup. 31 | CloudVisualizer(ros::NodeHandle nh); 32 | 33 | void timerCalback(const ros::TimerEvent& /** e */); 34 | 35 | void readClouds(); 36 | 37 | void visualizeClouds(); 38 | 39 | private: 40 | const Config config_; 41 | MotionVisualizer visualizer_; 42 | 43 | // ROS. 44 | ros::NodeHandle nh_; 45 | ros::Timer timer_; 46 | 47 | // Data to visualize. 48 | std::vector clouds_; 49 | std::vector cloud_infos_; 50 | std::vector clusters_; 51 | }; 52 | 53 | } // namespace dynablox 54 | 55 | #endif // DYNABLOX_ROS_VISUALIZATION_CLOUD_VISUALIZER_H_ 56 | -------------------------------------------------------------------------------- /dynablox_ros/include/dynablox_ros/visualization/motion_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNABLOX_ROS_VISUALIZATION_MOTION_VISUALIZER_H_ 2 | #define DYNABLOX_ROS_VISUALIZATION_MOTION_VISUALIZER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "dynablox/3rd_party/config_utilities.hpp" 19 | #include "dynablox/common/types.h" 20 | 21 | namespace dynablox { 22 | 23 | class MotionVisualizer { 24 | public: 25 | // Config. 26 | struct Config : public config_utilities::Config { 27 | std::string global_frame_name = "map"; 28 | 29 | // Set RGBA colors in [0, 1] if wanted. 30 | std::vector static_point_color = {0.f, 0.f, 0.f, 1.f}; 31 | std::vector dynamic_point_color = {1.f, 0.f, 0.5f, 1.f}; 32 | std::vector sensor_color = {1.f, 0.f, 0.f, 1.f}; 33 | std::vector true_positive_color = {0.f, 1.f, 0.f, 1.f}; 34 | std::vector false_positive_color = {1.f, 0.f, 0.f, 1.f}; 35 | std::vector true_negative_color = {0.f, 0.f, 0.f, 1.f}; 36 | std::vector false_negative_color = {0.f, 0.f, 1.f, 1.f}; 37 | std::vector out_of_bounds_color = {.7f, .7f, .7f, 1.f}; 38 | std::vector ever_free_color = {1.f, 0.f, 1.f, .5f}; 39 | std::vector never_free_color = {0.f, 1.f, 1.f, .5f}; 40 | std::vector point_level_slice_color = {1.f, 0.f, 1.f, 1.f}; 41 | std::vector cluster_level_slice_color = {0.f, 1.f, 1.f, 1.f}; 42 | 43 | // Scales of visualizations [m]. 44 | float static_point_scale = 0.1f; 45 | float dynamic_point_scale = 0.1f; 46 | float sensor_scale = 0.3f; 47 | float cluster_line_width = 0.05f; 48 | 49 | // Number of colors for the a full color wheel revolution. 50 | int color_wheel_num_colors = 20; 51 | 52 | // True: every cluster and object has a color, False: just mark them as 53 | // dynamic. 54 | bool color_clusters = true; 55 | 56 | // Height in map frame of the slice being visualized [m]. 57 | float slice_height = 0; 58 | 59 | // True: slice height is relative to the sensor, False: slice in world frame 60 | bool slice_relative_to_sensor = true; 61 | 62 | // Crop all visualizations at this height for better visibility. 63 | float visualization_max_z = 10000.f; 64 | 65 | Config() { setConfigName("MotionVisualizer"); } 66 | 67 | protected: 68 | void setupParamsAndPrinting() override; 69 | void checkParams() const override; 70 | void checkColor(const std::vector& color, 71 | const std::string& name) const; 72 | }; 73 | 74 | // Setup. 75 | MotionVisualizer(ros::NodeHandle nh, std::shared_ptr tsdf_layer); 76 | 77 | void setupRos(); 78 | 79 | // Visualization. 80 | void visualizeAll(const Cloud& cloud, const CloudInfo& cloud_info, 81 | const Clusters& clusters); 82 | void visualizeLidarPose(const CloudInfo& cloud_info) const; 83 | void visualizeLidarPoints(const Cloud& cloud) const; 84 | void visualizePointDetections(const Cloud& cloud, 85 | const CloudInfo& cloud_info) const; 86 | void visualizeClusterDetections(const Cloud& cloud, 87 | const CloudInfo& cloud_info, 88 | const Clusters& clusters) const; 89 | void visualizeObjectDetections(const Cloud& cloud, 90 | const CloudInfo& cloud_info, 91 | const Clusters& clusters) const; 92 | void visualizeGroundTruth(const Cloud& cloud, const CloudInfo& cloud_info, 93 | const std::string& ns = "") const; 94 | void visualizeMesh() const; 95 | void visualizeEverFree() const; 96 | void visualizeEverFreeSlice(const float slice_height) const; 97 | void visualizeTsdfSlice(const float slice_height) const; 98 | void visualizeSlicePoints(const Cloud& cloud, 99 | const CloudInfo& cloud_info) const; 100 | void visualizeClusters(const Clusters& clusters, 101 | const std::string& ns = "") const; 102 | 103 | // ROS msg helper tools. 104 | static geometry_msgs::Vector3 setScale(const float scale); 105 | static std_msgs::ColorRGBA setColor(const std::vector& color); 106 | static std_msgs::ColorRGBA setColor(const voxblox::Color& color); 107 | static geometry_msgs::Point setPoint(const Point& point); 108 | static geometry_msgs::Point setPoint(const voxblox::Point& point); 109 | 110 | private: 111 | const Config config_; 112 | voxblox::ExponentialOffsetIdColorMap color_map_; 113 | ros::NodeHandle nh_; 114 | std::shared_ptr tsdf_layer_; 115 | std::shared_ptr> mesh_integrator_; 116 | std::shared_ptr mesh_layer_; 117 | 118 | // Publishers. 119 | ros::Publisher sensor_pose_pub_; 120 | ros::Publisher sensor_points_pub_; 121 | ros::Publisher detection_points_pub_; 122 | ros::Publisher detection_points_comp_pub_; 123 | ros::Publisher detection_cluster_pub_; 124 | ros::Publisher detection_cluster_comp_pub_; 125 | ros::Publisher detection_object_pub_; 126 | ros::Publisher detection_object_comp_pub_; 127 | ros::Publisher gt_point_pub_; 128 | ros::Publisher gt_cluster_pub_; 129 | ros::Publisher gt_object_pub_; 130 | ros::Publisher ever_free_pub_; 131 | ros::Publisher never_free_pub_; 132 | ros::Publisher ever_free_slice_pub_; 133 | ros::Publisher never_free_slice_pub_; 134 | ros::Publisher tsdf_slice_pub_; 135 | ros::Publisher point_slice_pub_; 136 | ros::Publisher mesh_pub_; 137 | ros::Publisher cluster_vis_pub_; 138 | 139 | // Variables. 140 | ros::Time current_stamp_; 141 | bool time_stamp_set_ = false; 142 | 143 | // Helper functions. 144 | void visualizeGroundTruthAtLevel( 145 | const Cloud& cloud, const CloudInfo& cloud_info, 146 | const std::function& check_level, 147 | const ros::Publisher& pub, const std::string& ns) const; 148 | 149 | ros::Time getStamp() const; 150 | }; 151 | 152 | } // namespace dynablox 153 | 154 | #endif // DYNABLOX_ROS_VISUALIZATION_MOTION_VISUALIZER_H_ 155 | -------------------------------------------------------------------------------- /dynablox_ros/launch/cloud_visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | [0.8,0.8,0.8,1] 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /dynablox_ros/launch/play_doals_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /dynablox_ros/launch/play_dynablox_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /dynablox_ros/launch/run_experiment.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 | -------------------------------------------------------------------------------- /dynablox_ros/launch/visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /dynablox_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynablox_ros 4 | 0.0.0 5 | Detecting diverse dynamic objects in compelx environments 6 | 7 | Lukas Schmid 8 | Aurelio Sulser 9 | Olov Andersson 10 | 11 | Lukas Schmid 12 | Aurelio Sulser 13 | Olov Andersson 14 | 15 | BSD 16 | 17 | catkin 18 | catkin_simple 19 | 20 | 21 | roscpp 22 | rospy 23 | std_msgs 24 | sensor_msgs 25 | geometry_msgs 26 | visualization_msgs 27 | tf2_geometry_msgs 28 | voxblox_msgs 29 | pcl_ros 30 | pcl_conversions 31 | tf2 32 | tf2_ros 33 | voxblox 34 | voxblox_ros 35 | voxblox_rviz_plugin 36 | dynablox 37 | 38 | 39 | drift_simulation 40 | ouster_ros 41 | lidar_undistortion 42 | 43 | 44 | -------------------------------------------------------------------------------- /dynablox_ros/scripts/download_doals_data.sh: -------------------------------------------------------------------------------- 1 | # Check target dir. 2 | if [ -z $1 ]; 3 | then 4 | echo "No target directory specified."; 5 | echo "Usage: ./download_doals_data.sh "; 6 | exit 1 7 | else 8 | echo "Download the DOALS dataset to '$1'?"; 9 | read -p "[Y/N]? " -n 1 -r 10 | if [[ ! $REPLY =~ ^[Yy]$ ]] 11 | then 12 | echo "Exiting."; 13 | exit 1 14 | fi 15 | fi 16 | 17 | # Download. 18 | mkdir -p $1 19 | echo "" 20 | echo "Downloading and processing scenes. This may take a few minutes ..."; 21 | 22 | # Hauptgebaeude. 23 | wget http://robotics.ethz.ch/~asl-datasets/2021_ICRA_dynamic_object_lidar_dataset/scenes/hauptgebaeude.zip -P $1 24 | unzip $1/hauptgebaeude.zip -d $1 25 | rm $1/hauptgebaeude.zip 26 | mv $1/hauptgebaeude/sequence_1/2020-02-20-11-58-45.bag $1/hauptgebaeude/sequence_1/bag.bag 27 | mv $1/hauptgebaeude/sequence_2/2020-02-20-12-04-48.bag $1/hauptgebaeude/sequence_2/bag.bag 28 | 29 | # Niederdorf. 30 | wget http://robotics.ethz.ch/~asl-datasets/2021_ICRA_dynamic_object_lidar_dataset/scenes/niederdorf.zip -P $1 31 | unzip $1/niederdorf.zip -d $1 32 | rm $1/niederdorf.zip 33 | mv $1/niederdorf/sequence_1/2020-02-19-16-12-14.bag $1/niederdorf/sequence_1/bag.bag 34 | mv $1/niederdorf/sequence_2/2020-02-19-16-20-17.bag $1/niederdorf/sequence_2/bag.bag 35 | 36 | # Shopville. 37 | wget http://robotics.ethz.ch/~asl-datasets/2021_ICRA_dynamic_object_lidar_dataset/scenes/shopville.zip -P $1 38 | unzip $1/shopville.zip -d $1 39 | rm $1/shopville.zip 40 | mv $1/shopville/sequence_1/2020-02-20-16-39-51.bag $1/shopville/sequence_1/bag.bag 41 | mv $1/shopville/sequence_2/2020-02-20-16-43-52.bag $1/shopville/sequence_2/bag.bag 42 | 43 | # Station. 44 | wget http://robotics.ethz.ch/~asl-datasets/2021_ICRA_dynamic_object_lidar_dataset/scenes/station.zip -P $1 45 | unzip $1/station.zip -d $1 46 | rm $1/station.zip 47 | mv $1/station/sequence_1/2020-02-20-17-28-39.bag $1/station/sequence_1/bag.bag 48 | mv $1/station/sequence_2/2020-02-20-17-35-27.bag $1/station/sequence_2/bag.bag 49 | 50 | # Finished. 51 | echo "Successfully downloaded and processed the DOALS dataset to '$1'."; 52 | exit 0 -------------------------------------------------------------------------------- /dynablox_ros/scripts/download_dynablox_data.sh: -------------------------------------------------------------------------------- 1 | # Check target dir. 2 | if [ -z $1 ]; 3 | then 4 | echo "No target directory specified."; 5 | echo "Usage: ./download_dynablox_data.sh "; 6 | exit 1 7 | else 8 | echo "Download the Dynablox dataset to '$1'?"; 9 | read -p "[Y/N]? " -n 1 -r 10 | if [[ ! $REPLY =~ ^[Yy]$ ]] 11 | then 12 | echo "Exiting."; 13 | exit 1 14 | fi 15 | fi 16 | 17 | # Download. 18 | mkdir -p $1 19 | echo ""; 20 | echo "Downloading and processing scenes. This may take a few minutes ..."; 21 | 22 | # Get data. 23 | wget http://robotics.ethz.ch/~asl-datasets/2023_RAL_Dynablox/processed.zip -P $1 24 | unzip $1/processed.zip -d $1 25 | rm $1/processed.zip 26 | 27 | # Finished. 28 | echo "Successfully downloaded and processed the Dynablox dataset to '$1'."; 29 | exit 0 -------------------------------------------------------------------------------- /dynablox_ros/src/cloud_visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "dynablox_ros/visualization/cloud_visualizer.h" 5 | 6 | int main(int argc, char** argv) { 7 | ros::init(argc, argv, "cloud_visualizer"); 8 | 9 | // Always add these arguments for proper logging. 10 | config_utilities::RequiredArguments ra( 11 | &argc, &argv, {"--logtostderr", "--colorlogtostderr"}); 12 | 13 | // Setup logging. 14 | google::InitGoogleLogging(argv[0]); 15 | google::InstallFailureSignalHandler(); 16 | google::ParseCommandLineFlags(&argc, &argv, false); 17 | 18 | // Setup node. 19 | ros::NodeHandle nh("~"); 20 | dynablox::CloudVisualizer visualizer(nh); 21 | 22 | ros::spin(); 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /dynablox_ros/src/evaluation/data_tools.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import os 3 | import numpy as np 4 | 5 | 6 | def read_score_data(csv_file): 7 | """ 8 | Read a single CSV file of a run (tsdf or ssc) and turn it into a dictionary. 9 | """ 10 | data = {} 11 | if not os.path.isfile(csv_file): 12 | print(f"File '{csv_file}' does not exist!") 13 | return data 14 | with open(csv_file, newline='\n') as csvfile: 15 | reader = csv.reader(csvfile, delimiter=',') 16 | header = next(reader) 17 | for key in header: 18 | data[key] = [] 19 | 20 | for row in reader: 21 | for i, r in enumerate(row): 22 | data[header[i]].append(float(r)) 23 | return data 24 | 25 | 26 | def verify_data(data, names, expected_num_entries=10): 27 | num_incomplete = 0 28 | for i, d in enumerate(data): 29 | if not "timestamp" in d: 30 | print(f"Warning: failed to read data for '{names[i]}'.") 31 | num_incomplete = num_incomplete + 1 32 | continue 33 | num_entries = len(d["timestamp"]) 34 | if num_entries < expected_num_entries: 35 | print( 36 | f"Warning: Incomplete data for '{names[i]}' ({num_entries}/{expected_num_entries})." 37 | ) 38 | num_incomplete = num_incomplete + 1 39 | num_samples = len(data) 40 | print( 41 | f"{num_samples-num_incomplete}/{num_samples} data entries are complete." 42 | ) 43 | 44 | 45 | def get_grid(data, field): 46 | if field not in data: 47 | print(f"Warning: Did not fiend field '{field}' to create grid from.") 48 | return np.array([np.nan]) 49 | return np.array(data[field]) 50 | 51 | 52 | def read_cloud_data(csv_file): 53 | """ Returns data[field_name][point_index] = value. """ 54 | data = {} 55 | if not os.path.isfile(csv_file): 56 | print(f"File '{csv_file}' does not exist!") 57 | return data 58 | with open(csv_file, newline='\n') as csvfile: 59 | reader = csv.reader(csvfile, delimiter=',') 60 | header = None 61 | for row in reader: 62 | if header is None: 63 | header = row 64 | for key in header: 65 | data[key] = [] 66 | else: 67 | for i, r in enumerate(row): 68 | data[header[i]].append(float(r)) 69 | return data 70 | 71 | 72 | def read_time_data(file_name): 73 | data = {} 74 | file = open(file_name, 'r') 75 | lines = file.readlines() 76 | for l in lines[2:-1]: 77 | entries = l.split("\t") 78 | key = entries[0].strip(" ") 79 | calls = int(entries[1]) 80 | total = float(entries[2]) 81 | mean = float(entries[3].partition(" +- ")[0][1:]) 82 | std = float(entries[3].partition(" +- ")[2][:-1]) 83 | min = float(entries[4].partition(",")[0][1:]) 84 | max = float(entries[4].partition(",")[2][:-2]) 85 | data[key] = { 86 | 'calls': calls, 87 | 'total': total, 88 | 'mean': mean, 89 | 'std': std, 90 | 'min': min, 91 | 'max': max 92 | } 93 | return data -------------------------------------------------------------------------------- /dynablox_ros/src/evaluation/evaluate_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import os 4 | import numpy as np 5 | import argparse 6 | from data_tools import read_score_data, verify_data, get_grid 7 | 8 | 9 | SCENES = ["hauptgebaeude", "niederdorf", "station", "shopville"] 10 | SEQUENCES = [1, 2] 11 | 12 | 13 | def main(args): 14 | # Metrics 15 | # timestamp point_IoU point_Precision point_Recall point_TP point_TN point_FP point_FN cluster_IoU cluster_Precision cluster_Recall cluster_TP cluster_TN cluster_FP cluster_FN object_IoU object_Precision object_Recall object_TP object_TN object_FP object_FN EvaluatedPoints TotalPoints 16 | metrics = ['object_IoU', 'object_Precision', 'object_Recall'] 17 | 18 | # Print configuration 19 | print_names = True 20 | print_std = True 21 | print_nan = True 22 | print_mode = 'read' # 'read', 'csv', 'latex' 23 | print_overall = True 24 | 25 | # Run. 26 | table(args.data_path, metrics, print_names, print_std, print_nan, print_mode, 27 | print_overall) 28 | 29 | 30 | def read_data(data_path): 31 | data = [] 32 | names = [] 33 | success, name, datum = read_single_dir(data_path) 34 | if success: 35 | data.append(datum) 36 | names.append(name) 37 | for d in os.listdir(data_path): 38 | if not os.path.isdir(os.path.join(data_path, d)) or d == '..': 39 | continue 40 | success, name, datum = read_single_dir(os.path.join(data_path, d)) 41 | if success: 42 | data.append(datum) 43 | names.append(name) 44 | return data, names 45 | 46 | 47 | def read_single_dir(path): 48 | print(path) 49 | config_file = os.path.join(path, "config.txt") 50 | scores_file = os.path.join(path, "scores.csv") 51 | if not os.path.isfile(config_file) or not os.path.isfile(scores_file): 52 | return False, None, None 53 | with open(config_file, 'r') as file: 54 | lines = file.readlines() 55 | string = "".join(lines).replace('\n', '').replace(' ', '') 56 | for s in SCENES: 57 | for seq in SEQUENCES: 58 | if string.find(f"{s}/sequence_{seq}/indices.csv") != -1: 59 | return True, f"{s}_{seq}", read_score_data(scores_file) 60 | return False, None, None 61 | 62 | 63 | def table(data_path, metrics, 64 | print_names=True, 65 | print_std=True, 66 | print_nan=True, 67 | print_mode=False, 68 | print_overall=True): 69 | data, names = read_data(data_path) 70 | verify_data(data, names) 71 | 72 | def print_row(entries): 73 | if print_mode == 'latex': 74 | print(("".join('%-15s & ' % x for x in entries))[:-2] + "\\\\") 75 | elif print_mode == 'csv': 76 | print(("".join('%s,' % x for x in entries))[:-1]) 77 | else: 78 | print("".join('%-40s' % x for x in entries)) 79 | 80 | def evaluate_row(results): 81 | nans = np.sum(np.isnan(results)) 82 | msg = (f"{np.nanmean(results):.1f}" if nans == 0 else "-") 83 | if print_std and nans == 0: 84 | if print_mode == 'latex': 85 | msg = msg + f" $\pm$ {np.nanstd(results):.1f}" 86 | elif print_mode == 'csv': 87 | msg = msg + f",{np.nanstd(results):.1f}" 88 | else: 89 | msg = msg + f" +- {np.nanstd(results):.1f}" 90 | 91 | if print_nan and nans > 0: 92 | msg = msg + f" ({nans})" 93 | return msg 94 | 95 | all_data = {} 96 | for m in metrics: 97 | all_data[m] = np.array([]) 98 | 99 | print_row((["Data"] + metrics) if print_names else metrics) 100 | for i, n in enumerate(names): 101 | entries = [n] if print_names else [] 102 | for m in metrics: 103 | results = get_grid(data[i], m) * 100 104 | all_data[m] = np.append(all_data[m], results) 105 | entries.append(evaluate_row(results)) 106 | print_row(entries) 107 | 108 | if print_overall: 109 | entries = ["All"] if print_names else [] 110 | for m in metrics: 111 | entries.append(evaluate_row(all_data[m])) 112 | print_row(entries) 113 | 114 | 115 | if __name__ == "__main__": 116 | parser = argparse.ArgumentParser( 117 | prog='evaluate_data', 118 | description='Present raw data as a human readable table.') 119 | parser.add_argument('data_path') 120 | main(parser.parse_args()) 121 | -------------------------------------------------------------------------------- /dynablox_ros/src/motion_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox_ros/motion_detector.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace dynablox { 23 | 24 | using Timer = voxblox::timing::Timer; 25 | 26 | void MotionDetector::Config::checkParams() const { 27 | checkParamCond(!global_frame_name.empty(), 28 | "'global_frame_name' may not be empty."); 29 | checkParamGE(num_threads, 1, "num_threads"); 30 | checkParamGE(queue_size, 0, "queue_size"); 31 | } 32 | 33 | void MotionDetector::Config::setupParamsAndPrinting() { 34 | setupParam("global_frame_name", &global_frame_name); 35 | setupParam("sensor_frame_name", &sensor_frame_name); 36 | setupParam("queue_size", &queue_size); 37 | setupParam("evaluate", &evaluate); 38 | setupParam("visualize", &visualize); 39 | setupParam("verbose", &verbose); 40 | setupParam("num_threads", &num_threads); 41 | setupParam("shutdown_after", &shutdown_after); 42 | } 43 | 44 | MotionDetector::MotionDetector(const ros::NodeHandle& nh, 45 | const ros::NodeHandle& nh_private) 46 | : config_( 47 | config_utilities::getConfigFromRos(nh_private) 48 | .checkValid()), 49 | nh_(nh), 50 | nh_private_(nh_private) { 51 | setupMembers(); 52 | 53 | // Cache frequently used constants. 54 | voxels_per_side_ = tsdf_layer_->voxels_per_side(); 55 | voxels_per_block_ = voxels_per_side_ * voxels_per_side_ * voxels_per_side_; 56 | 57 | // Advertise and subscribe to topics. 58 | setupRos(); 59 | 60 | // Print current configuration of all components. 61 | LOG_IF(INFO, config_.verbose) << "Configuration:\n" 62 | << config_utilities::Global::printAllConfigs(); 63 | } 64 | 65 | void MotionDetector::setupMembers() { 66 | // Voxblox. Overwrite dependent config parts. Note that this TSDF layer is 67 | // shared with all other processing components and is mutable for processing. 68 | ros::NodeHandle nh_voxblox(nh_private_, "voxblox"); 69 | nh_voxblox.setParam("world_frame", config_.global_frame_name); 70 | nh_voxblox.setParam("update_mesh_every_n_sec", 0); 71 | nh_voxblox.setParam("voxel_carving_enabled", 72 | true); // Integrate whole ray not only truncation band. 73 | nh_voxblox.setParam("allow_clear", 74 | true); // Integrate rays up to max_ray_length. 75 | nh_voxblox.setParam("integrator_threads", config_.num_threads); 76 | 77 | tsdf_server_ = std::make_shared(nh_voxblox, nh_voxblox); 78 | tsdf_layer_.reset(tsdf_server_->getTsdfMapPtr()->getTsdfLayerPtr()); 79 | 80 | // Preprocessing. 81 | preprocessing_ = std::make_shared( 82 | config_utilities::getConfigFromRos( 83 | ros::NodeHandle(nh_private_, "preprocessing"))); 84 | 85 | // Clustering. 86 | clustering_ = std::make_shared( 87 | config_utilities::getConfigFromRos( 88 | ros::NodeHandle(nh_private_, "clustering")), 89 | tsdf_layer_); 90 | 91 | // Tracking. 92 | tracking_ = std::make_shared( 93 | config_utilities::getConfigFromRos( 94 | ros::NodeHandle(nh_private_, "tracking"))); 95 | 96 | // Ever-Free Integrator. 97 | ros::NodeHandle nh_ever_free(nh_private_, "ever_free_integrator"); 98 | nh_ever_free.setParam("num_threads", config_.num_threads); 99 | ever_free_integrator_ = std::make_shared( 100 | config_utilities::getConfigFromRos( 101 | nh_ever_free), 102 | tsdf_layer_); 103 | 104 | // Evaluation. 105 | if (config_.evaluate) { 106 | // NOTE(schmluk): These will be uninitialized if not requested, but then no 107 | // config files need to be set. 108 | evaluator_ = std::make_shared( 109 | config_utilities::getConfigFromRos( 110 | ros::NodeHandle(nh_private_, "evaluation"))); 111 | } 112 | 113 | // Visualization. 114 | visualizer_ = std::make_shared( 115 | ros::NodeHandle(nh_private_, "visualization"), tsdf_layer_); 116 | } 117 | 118 | void MotionDetector::setupRos() { 119 | lidar_pcl_sub_ = nh_.subscribe("pointcloud", config_.queue_size, 120 | &MotionDetector::pointcloudCallback, this); 121 | } 122 | 123 | void MotionDetector::pointcloudCallback( 124 | const sensor_msgs::PointCloud2::Ptr& msg) { 125 | Timer frame_timer("frame"); 126 | Timer detection_timer("motion_detection"); 127 | 128 | // Lookup cloud transform T_M_S of sensor (S) to map (M). 129 | // If different sensor frame is required, update the message. 130 | Timer tf_lookup_timer("motion_detection/tf_lookup"); 131 | const std::string sensor_frame_name = config_.sensor_frame_name.empty() 132 | ? msg->header.frame_id 133 | : config_.sensor_frame_name; 134 | 135 | tf::StampedTransform T_M_S; 136 | if (!lookupTransform(config_.global_frame_name, sensor_frame_name, 137 | msg->header.stamp.toNSec(), T_M_S)) { 138 | // Getting transform failed, need to skip. 139 | return; 140 | } 141 | tf_lookup_timer.Stop(); 142 | 143 | // Preprocessing. 144 | Timer preprocessing_timer("motion_detection/preprocessing"); 145 | frame_counter_++; 146 | CloudInfo cloud_info; 147 | Cloud cloud; 148 | preprocessing_->processPointcloud(msg, T_M_S, cloud, cloud_info); 149 | preprocessing_timer.Stop(); 150 | 151 | // Build a mapping of all blocks to voxels to points for the scan. 152 | Timer setup_timer("motion_detection/indexing_setup"); 153 | BlockToPointMap point_map; 154 | std::vector occupied_ever_free_voxel_indices; 155 | setUpPointMap(cloud, point_map, occupied_ever_free_voxel_indices, cloud_info); 156 | setup_timer.Stop(); 157 | 158 | // Clustering. 159 | Timer clustering_timer("motion_detection/clustering"); 160 | Clusters clusters = clustering_->performClustering( 161 | point_map, occupied_ever_free_voxel_indices, frame_counter_, cloud, 162 | cloud_info); 163 | clustering_timer.Stop(); 164 | 165 | // Tracking. 166 | Timer tracking_timer("motion_detection/tracking"); 167 | tracking_->track(cloud, clusters, cloud_info); 168 | tracking_timer.Stop(); 169 | 170 | // Integrate ever-free information. 171 | Timer update_ever_free_timer("motion_detection/update_ever_free"); 172 | ever_free_integrator_->updateEverFreeVoxels(frame_counter_); 173 | update_ever_free_timer.Stop(); 174 | 175 | // Integrate the pointcloud into the voxblox TSDF map. 176 | Timer tsdf_timer("motion_detection/tsdf_integration"); 177 | voxblox::Transformation T_G_C; 178 | tf::transformTFToKindr(T_M_S, &T_G_C); 179 | tsdf_server_->processPointCloudMessageAndInsert(msg, T_G_C, false); 180 | tsdf_timer.Stop(); 181 | detection_timer.Stop(); 182 | 183 | // Evaluation if requested. 184 | if (config_.evaluate) { 185 | Timer eval_timer("evaluation"); 186 | evaluator_->evaluateFrame(cloud, cloud_info, clusters); 187 | eval_timer.Stop(); 188 | if (config_.shutdown_after > 0 && 189 | evaluator_->getNumberOfEvaluatedFrames() >= config_.shutdown_after) { 190 | LOG(INFO) << "Evaluated " << config_.shutdown_after 191 | << " frames, shutting down"; 192 | ros::shutdown(); 193 | } 194 | } 195 | 196 | // Visualization if requested. 197 | if (config_.visualize) { 198 | Timer vis_timer("visualizations"); 199 | visualizer_->visualizeAll(cloud, cloud_info, clusters); 200 | vis_timer.Stop(); 201 | } 202 | } 203 | 204 | bool MotionDetector::lookupTransform(const std::string& target_frame, 205 | const std::string& source_frame, 206 | uint64_t timestamp, 207 | tf::StampedTransform& result) const { 208 | ros::Time timestamp_ros; 209 | timestamp_ros.fromNSec(timestamp); 210 | 211 | // Note(schmluk): We could also wait for transforms here but this is easier 212 | // and faster atm. 213 | try { 214 | tf_listener_.lookupTransform(target_frame, source_frame, timestamp_ros, 215 | result); 216 | } catch (tf::TransformException& ex) { 217 | LOG(WARNING) << "Could not get sensor transform, skipping pointcloud: " 218 | << ex.what(); 219 | return false; 220 | } 221 | return true; 222 | } 223 | 224 | void MotionDetector::setUpPointMap( 225 | const Cloud& cloud, BlockToPointMap& point_map, 226 | std::vector& occupied_ever_free_voxel_indices, 227 | CloudInfo& cloud_info) const { 228 | // Identifies for any LiDAR point the block it falls in and constructs the 229 | // hash-map block2points_map mapping each block to the LiDAR points that 230 | // fall into the block. 231 | const voxblox::HierarchicalIndexIntMap block2points_map = 232 | buildBlockToPointsMap(cloud); 233 | 234 | // Builds the voxel2point-map in parallel blockwise. 235 | std::vector block_indices(block2points_map.size()); 236 | size_t i = 0; 237 | for (const auto& block : block2points_map) { 238 | block_indices[i] = block.first; 239 | ++i; 240 | } 241 | IndexGetter index_getter(block_indices); 242 | std::vector> threads; 243 | std::mutex aggregate_results_mutex; 244 | for (int i = 0; i < config_.num_threads; ++i) { 245 | threads.emplace_back(std::async(std::launch::async, [&]() { 246 | // Data to store results. 247 | BlockIndex block_index; 248 | std::vector local_occupied_indices; 249 | BlockToPointMap local_point_map; 250 | 251 | // Process until no more blocks. 252 | while (index_getter.getNextIndex(&block_index)) { 253 | VoxelToPointMap result; 254 | this->blockwiseBuildPointMap(cloud, block_index, 255 | block2points_map.at(block_index), result, 256 | local_occupied_indices, cloud_info); 257 | local_point_map.insert(std::pair(block_index, result)); 258 | } 259 | 260 | // After processing is done add data to the output map. 261 | std::lock_guard lock(aggregate_results_mutex); 262 | occupied_ever_free_voxel_indices.insert( 263 | occupied_ever_free_voxel_indices.end(), 264 | local_occupied_indices.begin(), local_occupied_indices.end()); 265 | point_map.merge(local_point_map); 266 | })); 267 | } 268 | 269 | for (auto& thread : threads) { 270 | thread.get(); 271 | } 272 | } 273 | 274 | voxblox::HierarchicalIndexIntMap MotionDetector::buildBlockToPointsMap( 275 | const Cloud& cloud) const { 276 | voxblox::HierarchicalIndexIntMap result; 277 | 278 | int i = 0; 279 | for (const Point& point : cloud) { 280 | voxblox::Point coord(point.x, point.y, point.z); 281 | const BlockIndex blockindex = 282 | tsdf_layer_->computeBlockIndexFromCoordinates(coord); 283 | result[blockindex].push_back(i); 284 | i++; 285 | } 286 | return result; 287 | } 288 | 289 | void MotionDetector::blockwiseBuildPointMap( 290 | const Cloud& cloud, const BlockIndex& block_index, 291 | const voxblox::AlignedVector& points_in_block, 292 | VoxelToPointMap& voxel_map, 293 | std::vector& occupied_ever_free_voxel_indices, 294 | CloudInfo& cloud_info) const { 295 | // Get the block. 296 | TsdfBlock::Ptr tsdf_block = tsdf_layer_->getBlockPtrByIndex(block_index); 297 | if (!tsdf_block) { 298 | return; 299 | } 300 | 301 | // Create a mapping of each voxel index to the points it contains. 302 | for (size_t i : points_in_block) { 303 | const Point& point = cloud[i]; 304 | const voxblox::Point coords(point.x, point.y, point.z); 305 | const VoxelIndex voxel_index = 306 | tsdf_block->computeVoxelIndexFromCoordinates(coords); 307 | if (!tsdf_block->isValidVoxelIndex(voxel_index)) { 308 | continue; 309 | } 310 | voxel_map[voxel_index].push_back(i); 311 | 312 | // EverFree detection flag at the same time, since we anyways lookup 313 | // voxels. 314 | if (tsdf_block->getVoxelByVoxelIndex(voxel_index).ever_free) { 315 | cloud_info.points.at(i).ever_free_level_dynamic = true; 316 | } 317 | } 318 | 319 | // Update the voxel status of the currently occupied voxels. 320 | for (const auto& voxel_points_pair : voxel_map) { 321 | TsdfVoxel& tsdf_voxel = 322 | tsdf_block->getVoxelByVoxelIndex(voxel_points_pair.first); 323 | tsdf_voxel.last_lidar_occupied = frame_counter_; 324 | 325 | // This voxel attribute is used in the voxel clustering method: it 326 | // signalizes that a currently occupied voxel has not yet been clustered 327 | tsdf_voxel.clustering_processed = false; 328 | 329 | // The set of occupied_ever_free_voxel_indices allows for fast access of 330 | // the seed voxels in the voxel clustering 331 | if (tsdf_voxel.ever_free) { 332 | occupied_ever_free_voxel_indices.push_back( 333 | std::make_pair(block_index, voxel_points_pair.first)); 334 | } 335 | } 336 | } 337 | 338 | } // namespace dynablox 339 | -------------------------------------------------------------------------------- /dynablox_ros/src/motion_detector_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "dynablox_ros/motion_detector.h" 5 | 6 | int main(int argc, char** argv) { 7 | ros::init(argc, argv, "motion_detector"); 8 | 9 | // Always add these arguments for proper logging. 10 | config_utilities::RequiredArguments ra( 11 | &argc, &argv, {"--logtostderr", "--colorlogtostderr"}); 12 | 13 | // Setup logging. 14 | google::InitGoogleLogging(argv[0]); 15 | google::InstallFailureSignalHandler(); 16 | google::ParseCommandLineFlags(&argc, &argv, false); 17 | 18 | // Setup node. 19 | ros::NodeHandle nh; 20 | ros::NodeHandle nh_private("~"); 21 | dynablox::MotionDetector motion_detector(nh, nh_private); 22 | 23 | ros::spin(); 24 | return 0; 25 | } 26 | -------------------------------------------------------------------------------- /dynablox_ros/src/visualization/cloud_visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "dynablox_ros/visualization/cloud_visualizer.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "dynablox/evaluation/io_tools.h" 10 | #include "dynablox/processing/clustering.h" 11 | 12 | namespace dynablox { 13 | 14 | void CloudVisualizer::Config::checkParams() const { 15 | checkParamCond(std::filesystem::exists(file_path), 16 | "Target file '" + file_path + "' does not exist."); 17 | checkParamGT(refresh_rate, 0.f, "refresh_rate"); 18 | } 19 | 20 | void CloudVisualizer::Config::setupParamsAndPrinting() { 21 | setupParam("file_path", &file_path); 22 | setupParam("refresh_rate", &refresh_rate, "s"); 23 | } 24 | 25 | CloudVisualizer::CloudVisualizer(ros::NodeHandle nh) 26 | : config_(config_utilities::getConfigFromRos(nh) 27 | .checkValid()), 28 | visualizer_(nh, std::make_shared(0.2, 16)), 29 | nh_(nh) { 30 | // NOTE(schmluk): The Tsdf Layer is a dummy and is not going to be used. 31 | LOG(INFO) << "Configuration:\n" 32 | << config_utilities::Global::printAllConfigs(); 33 | 34 | // Load the data. 35 | if (!loadCloudFromCsv(config_.file_path, clouds_, cloud_infos_, clusters_)) { 36 | LOG(FATAL) << "Failed to read clouds from '" << config_.file_path << "'."; 37 | } 38 | LOG(INFO) << "Read " << clouds_.size() << " clouds from '" 39 | << config_.file_path << "'."; 40 | 41 | // Recompute the cluster aabbs. 42 | for (size_t i = 0; i < clusters_.size(); ++i) { 43 | const Cloud& cloud = clouds_[i]; 44 | for (Cluster& cluster : clusters_[i]) { 45 | Point& min = cluster.aabb.min_corner; 46 | Point& max = cluster.aabb.max_corner; 47 | min = cloud[cluster.points[0]]; 48 | max = cloud[cluster.points[0]]; 49 | for (size_t i = 1; i < cluster.points.size(); ++i) { 50 | const Point& point = cloud[cluster.points[i]]; 51 | min.x = std::min(min.x, point.x); 52 | min.y = std::min(min.y, point.y); 53 | min.z = std::min(min.z, point.z); 54 | max.x = std::max(max.x, point.x); 55 | max.y = std::max(max.y, point.y); 56 | max.z = std::max(max.z, point.z); 57 | } 58 | } 59 | } 60 | 61 | // Visualize periodically just in case. 62 | timer_ = nh_.createTimer(ros::Duration(config_.refresh_rate), 63 | &CloudVisualizer::timerCalback, this); 64 | } 65 | 66 | void CloudVisualizer::timerCalback(const ros::TimerEvent& /** e */) { 67 | visualizeClouds(); 68 | } 69 | 70 | void CloudVisualizer::visualizeClouds() { 71 | for (size_t i = 0; i < clouds_.size(); ++i) { 72 | const std::string ns = "cloud_" + std::to_string(i); 73 | visualizer_.visualizeGroundTruth(clouds_[i], cloud_infos_[i], ns); 74 | visualizer_.visualizeClusters(clusters_[i], ns); 75 | } 76 | } 77 | 78 | } // namespace dynablox 79 | -------------------------------------------------------------------------------- /https.rosinstall: -------------------------------------------------------------------------------- 1 | # Dependencies 2 | - git: 3 | local-name: voxblox 4 | uri: https://github.com/ethz-asl/voxblox.git 5 | version: dynablox/release 6 | - git: 7 | local-name: catkin_simple 8 | uri: https://github.com/catkin/catkin_simple.git 9 | - git: 10 | local-name: eigen_catkin 11 | uri: https://github.com/ethz-asl/eigen_catkin.git 12 | - git: 13 | local-name: eigen_checks 14 | uri: https://github.com/ethz-asl/eigen_checks.git 15 | - git: 16 | local-name: gflags_catkin 17 | uri: https://github.com/ethz-asl/gflags_catkin.git 18 | - git: 19 | local-name: glog_catkin 20 | uri: https://github.com/ethz-asl/glog_catkin.git 21 | - git: 22 | local-name: minkindr 23 | uri: https://github.com/ethz-asl/minkindr.git 24 | - git: 25 | local-name: minkindr_ros 26 | uri: https://github.com/ethz-asl/minkindr_ros.git 27 | - git: 28 | local-name: protobuf_catkin 29 | uri: https://github.com/ethz-asl/protobuf_catkin.git 30 | - git: 31 | local-name: numpy_eigen 32 | uri: https://github.com/ethz-asl/numpy_eigen.git 33 | - git: 34 | local-name: catkin_boost_python_buildtool 35 | uri: https://github.com/ethz-asl/catkin_boost_python_buildtool.git 36 | - git: 37 | local-name: catkin_grpc 38 | uri: https://github.com/CogRob/catkin_grpc.git 39 | 40 | # Dependencies to run demo 41 | - git: 42 | local-name: lidar_undistortion 43 | uri: https://github.com/ethz-asl/lidar_undistortion.git 44 | version: dynablox/release 45 | - git: 46 | local-name: ouster-ros 47 | uri: https://github.com/ouster-lidar/ouster-ros.git -------------------------------------------------------------------------------- /ssh.rosinstall: -------------------------------------------------------------------------------- 1 | # Dependencies 2 | - git: 3 | local-name: voxblox 4 | uri: git@github.com:ethz-asl/voxblox.git 5 | version: dynablox/release 6 | - git: 7 | local-name: catkin_simple 8 | uri: git@github.com:catkin/catkin_simple.git 9 | - git: 10 | local-name: eigen_catkin 11 | uri: git@github.com:ethz-asl/eigen_catkin.git 12 | - git: 13 | local-name: eigen_checks 14 | uri: git@github.com:ethz-asl/eigen_checks.git 15 | - git: 16 | local-name: gflags_catkin 17 | uri: git@github.com:ethz-asl/gflags_catkin.git 18 | - git: 19 | local-name: glog_catkin 20 | uri: git@github.com:ethz-asl/glog_catkin.git 21 | - git: 22 | local-name: minkindr 23 | uri: git@github.com:ethz-asl/minkindr.git 24 | - git: 25 | local-name: minkindr_ros 26 | uri: git@github.com:ethz-asl/minkindr_ros.git 27 | - git: 28 | local-name: protobuf_catkin 29 | uri: git@github.com:ethz-asl/protobuf_catkin.git 30 | - git: 31 | local-name: numpy_eigen 32 | uri: git@github.com:ethz-asl/numpy_eigen.git 33 | - git: 34 | local-name: catkin_boost_python_buildtool 35 | uri: git@github.com:ethz-asl/catkin_boost_python_buildtool.git 36 | - git: 37 | local-name: catkin_grpc 38 | uri: git@github.com:CogRob/catkin_grpc.git 39 | 40 | # Dependencies to run demo 41 | - git: 42 | local-name: lidar_undistortion 43 | uri: git@github.com:ethz-asl/lidar_undistortion.git 44 | version: dynablox/release 45 | - git: 46 | local-name: ouster-ros 47 | uri: git@github.com:ouster-lidar/ouster-ros.git --------------------------------------------------------------------------------