├── .gitignore ├── .gitmodules ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── MCL.cfg └── RING.cfg ├── doc └── tsdf_loc_teaser.gif ├── include └── tsdf_localization │ ├── cuda │ ├── cuda_data.h │ ├── cuda_eval_particles.h │ ├── cuda_evaluator.h │ ├── cuda_sub_voxel_map.h │ ├── cuda_sub_voxel_map.tcc │ ├── cuda_sum.h │ └── cuda_util.h │ ├── evaluation │ ├── model │ │ ├── evaluation_model.h │ │ ├── likelihood_evaluation.h │ │ ├── naiv_evaluation.h │ │ └── omp_likelihood_evaluation.h │ ├── particle_evaluator.h │ └── tsdf_evaluator.h │ ├── int_mcl │ └── int_constant.h │ ├── map │ ├── device_map.h │ ├── distance_function.hpp │ ├── grid_map.h │ ├── groot.hpp │ ├── hash_grid_map.h │ ├── hash_grid_map.tcc │ ├── map_util.h │ ├── sub_voxel_map.h │ └── sub_voxel_map.tcc │ ├── particle.h │ ├── particle_cloud.h │ ├── resampling │ ├── novel_resampling.h │ ├── resampler.h │ └── wheel_resampler.h │ ├── ring_buffer │ ├── ring_buffer.h │ └── ring_buffer.tcc │ └── util │ ├── constant.h │ ├── filter.h │ ├── global_map.h │ ├── imu_accumulator.h │ ├── mcl_file.h │ ├── runtime_evaluator.h │ ├── time_util.h │ ├── tsdf.h │ └── util.h ├── launch ├── experiments │ ├── cache_eval.launch │ ├── deskew_ouster.launch │ ├── ekf.launch │ ├── hilti_imu_filter.launch │ └── num_particles_eval.launch ├── int_mcl_2d.launch ├── int_mcl_3d.launch ├── mcl_2d.launch ├── mcl_3d_hilti.launch └── mcl_3d_sim.launch ├── package.xml └── src ├── cuda ├── cuda_evaluator.cu ├── cuda_sum.cu ├── cuda_test_eval.cu └── cuda_util.cu ├── evaluation ├── model │ ├── likelihood_evaluation.cpp │ ├── naiv_evaluation.cpp │ └── omp_likelihood_evaluation.cpp └── tsdf_evaluator.cpp ├── mcl_3d.cpp ├── num_particles_eval.cpp ├── particle_cloud.cpp ├── resampling └── wheel_resampler.cpp ├── snap_shot_node.cpp ├── snap_vis_node.cpp ├── tsdf_vis.cpp └── util ├── global_map.cpp ├── imu_accumulator.cpp ├── mcl_file.cpp ├── runtime_evaluator.cpp └── util.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ext/HighFive"] 2 | path = ext/HighFive 3 | url = https://github.com/BlueBrain/HighFive.git 4 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | preferred-citation: 3 | title: "Towards 6D MCL for LiDARs in 3D TSDF Maps on Embedded Systems with GPUs" 4 | doi: "10.1109/IRC59093.2023.00035" 5 | year: "2023" 6 | type: conference-paper 7 | collection-title: "2023 Seventh IEEE International Conference on Robotic Computing (IRC)" 8 | url: https://ieeexplore.ieee.org/document/10473560 9 | codeurl: https://github.com/uos/tsdf_localization 10 | authors: 11 | - family-names: Eisoldt 12 | given-names: Marc 13 | - family-names: Mock 14 | given-names: Alexander 15 | orcid: 0009-0004-7800-9774 16 | - family-names: Porrmann 17 | given-names: Mario 18 | - family-names: Wiemann 19 | given-names: Thomas 20 | orcid: 0000-0003-0710-872X 21 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(tsdf_localization 3 | VERSION 1.0.0) 4 | 5 | # RELEASE PER DEFAULT 6 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 7 | if (NOT CMAKE_BUILD_TYPE) 8 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 9 | endif() 10 | endif() 11 | 12 | ## Compile as C++11, supported in ROS Kinetic and newer 13 | add_compile_options(-std=c++17) 14 | set(CMAKE_CXX_STANDARD 17) 15 | 16 | ## Find catkin macros and libraries 17 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 18 | ## is used, also find other catkin packages 19 | find_package(catkin REQUIRED 20 | COMPONENTS 21 | diagnostic_updater 22 | dynamic_reconfigure 23 | geometry_msgs 24 | roscpp 25 | sensor_msgs 26 | visualization_msgs 27 | nav_msgs 28 | tf2 29 | tf2_sensor_msgs 30 | message_runtime 31 | message_generation 32 | ) 33 | 34 | find_package(CUDA) 35 | 36 | # let cmake find OpenMP and set some variables 37 | find_package(OpenMP REQUIRED) 38 | if(OPENMP_FOUND) 39 | message(STATUS "OPENMP FOUND") 40 | set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) # or if you use C: ${OpenMP_C_FLAGS} 41 | set(OpenMP_LIBS gomp) 42 | endif() 43 | 44 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 45 | 46 | find_package(Boost REQUIRED COMPONENTS filesystem) 47 | 48 | find_package(FLANN) 49 | 50 | # dynamic reconfigure 51 | generate_dynamic_reconfigure_options( 52 | cfg/MCL.cfg 53 | cfg/RING.cfg 54 | ) 55 | 56 | catkin_package( 57 | CATKIN_DEPENDS 58 | diagnostic_updater 59 | dynamic_reconfigure 60 | geometry_msgs 61 | roscpp 62 | sensor_msgs 63 | visualization_msgs 64 | nav_msgs 65 | tf2 66 | tf2_sensor_msgs 67 | message_runtime 68 | message_generation 69 | INCLUDE_DIRS include 70 | DEPENDS OpenMP 71 | ) 72 | 73 | ########### 74 | ## Build ## 75 | ########### 76 | 77 | ## Specify additional locations of header files 78 | ## Your package locations should be listed before other locations 79 | include_directories( 80 | include 81 | ${catkin_INCLUDE_DIRS} 82 | ) 83 | 84 | set(TSDF_LOC_SRC 85 | src/particle_cloud.cpp 86 | src/util/util.cpp 87 | src/evaluation/model/naiv_evaluation.cpp 88 | src/evaluation/model/likelihood_evaluation.cpp 89 | src/evaluation/model/omp_likelihood_evaluation.cpp 90 | src/resampling/wheel_resampler.cpp 91 | src/evaluation/tsdf_evaluator.cpp 92 | src/util/runtime_evaluator.cpp 93 | src/util/imu_accumulator.cpp 94 | ) 95 | 96 | # External interface for HDF5 (libhdf5) HighFive (https://github.com/BlueBrain/HighFive). 97 | set(HIGHFIVE_EXAMPLES FALSE CACHE INTERNAL "") 98 | set(HIGHFIVE_UNIT_TESTS FALSE CACHE INTERNAL "") 99 | add_subdirectory(ext/HighFive) 100 | include_directories("ext/HighFive/include") 101 | find_package(HDF5 REQUIRED COMPONENTS CXX C HL) 102 | include_directories(${HDF5_INCLUDE_DIRS}) 103 | 104 | function(MAKE_NODE NAME CPP) 105 | add_executable(${NAME} ${CPP} src/util/runtime_evaluator.cpp) 106 | 107 | add_dependencies(${NAME} ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) 108 | 109 | target_compile_options(${NAME} PRIVATE ${OpenMP_FLAGS}) 110 | 111 | target_link_libraries(${NAME} 112 | ${catkin_LIBRARIES} 113 | ${OpenMP_LIBS} 114 | ${HDF5_LIBRARIES} 115 | ${HDF5_HL_LIBRARIES} 116 | Eigen3::Eigen 117 | ) 118 | endfunction() 119 | 120 | ### CUDA libraries 121 | 122 | if (CUDA_FOUND) 123 | 124 | set(CUDA_NVCC_FLAGS "--std=c++14" CACHE STRING "nvcc flags" FORCE) 125 | set(LIB_TYPE STATIC) 126 | cuda_add_library(CUDAEvaluator ${LIB_TYPE} src/cuda/cuda_evaluator.cu src/cuda/cuda_sum.cu src/cuda/cuda_util.cu src/util/runtime_evaluator.cpp) 127 | target_link_libraries(CUDAEvaluator ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} ${Boost_LIBRARIES}) 128 | 129 | cuda_add_executable(cuda_test_eval src/cuda/cuda_test_eval.cu src/cuda/cuda_evaluator.cu src/cuda/cuda_sum.cu src/cuda/cuda_util.cu src/util/mcl_file.cpp src/util/runtime_evaluator.cpp) 130 | target_link_libraries(cuda_test_eval ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} ${Boost_LIBRARIES}) 131 | 132 | endif(CUDA_FOUND) 133 | 134 | ### ROS nodes 135 | 136 | add_executable(mcl_3d src/mcl_3d.cpp ${TSDF_LOC_SRC}) 137 | 138 | if (CUDA_FOUND) 139 | 140 | add_dependencies(mcl_3d ${PROJECT_NAME}_gencfg CUDAEvaluator) 141 | target_link_libraries(mcl_3d ${catkin_LIBRARIES} ${Boost_LIBRARIES} CUDAEvaluator) 142 | 143 | else (CUDA_FOUND) 144 | 145 | add_dependencies(mcl_3d ${PROJECT_NAME}_gencfg) 146 | target_link_libraries(mcl_3d ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 147 | 148 | endif (CUDA_FOUND) 149 | 150 | target_compile_options(mcl_3d PRIVATE ${OpenMP_FLAGS}) 151 | 152 | target_link_libraries(mcl_3d 153 | ${catkin_LIBRARIES} 154 | ${OpenMP_LIBS} 155 | ${HDF5_LIBRARIES} 156 | ${HDF5_HL_LIBRARIES} 157 | Eigen3::Eigen 158 | ) 159 | 160 | add_executable(num_particles_eval src/num_particles_eval.cpp src/util/mcl_file.cpp ${TSDF_LOC_SRC}) 161 | 162 | if (CUDA_FOUND) 163 | 164 | add_dependencies(num_particles_eval ${PROJECT_NAME}_gencfg CUDAEvaluator) 165 | target_link_libraries(num_particles_eval ${catkin_LIBRARIES} ${Boost_LIBRARIES} CUDAEvaluator) 166 | 167 | else (CUDA_FOUND) 168 | 169 | add_dependencies(num_particles_eval ${PROJECT_NAME}_gencfg) 170 | target_link_libraries(num_particles_eval ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 171 | 172 | endif (CUDA_FOUND) 173 | 174 | target_compile_options(num_particles_eval PRIVATE ${OpenMP_FLAGS}) 175 | 176 | target_link_libraries(num_particles_eval 177 | ${catkin_LIBRARIES} 178 | ${OpenMP_LIBS} 179 | ${HDF5_LIBRARIES} 180 | ${HDF5_HL_LIBRARIES} 181 | Eigen3::Eigen 182 | ) 183 | 184 | if (CUDA_FOUND) 185 | 186 | 187 | find_package(PkgConfig REQUIRED) 188 | pkg_check_modules(LZ4 REQUIRED liblz4) 189 | 190 | endif (CUDA_FOUND) 191 | 192 | MAKE_NODE(tsdf_vis src/tsdf_vis.cpp) 193 | 194 | if (CUDA_FOUND) 195 | 196 | add_executable(snap_shot_node src/snap_shot_node.cpp src/util/mcl_file.cpp ${TSDF_LOC_SRC}) 197 | add_dependencies(snap_shot_node ${PROJECT_NAME}_gencfg CUDAEvaluator) 198 | target_link_libraries(snap_shot_node ${catkin_LIBRARIES} ${Boost_LIBRARIES} CUDAEvaluator) 199 | 200 | target_compile_options(snap_shot_node PRIVATE ${OpenMP_FLAGS}) 201 | 202 | target_link_libraries(snap_shot_node 203 | ${catkin_LIBRARIES} 204 | ${OpenMP_LIBS} 205 | ${HDF5_LIBRARIES} 206 | ${HDF5_HL_LIBRARIES} 207 | Eigen3::Eigen 208 | ) 209 | 210 | add_executable(snap_vis_node src/snap_vis_node.cpp src/util/mcl_file.cpp ${TSDF_LOC_SRC}) 211 | add_dependencies(snap_vis_node ${PROJECT_NAME}_gencfg CUDAEvaluator) 212 | target_link_libraries(snap_vis_node ${catkin_LIBRARIES} ${Boost_LIBRARIES} CUDAEvaluator) 213 | 214 | target_compile_options(snap_vis_node PRIVATE ${OpenMP_FLAGS}) 215 | 216 | target_link_libraries(snap_vis_node 217 | ${catkin_LIBRARIES} 218 | ${OpenMP_LIBS} 219 | ${HDF5_LIBRARIES} 220 | ${HDF5_HL_LIBRARIES} 221 | Eigen3::Eigen 222 | ) 223 | 224 | endif (CUDA_FOUND) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Osnabrück University, Knowledge Based Systems Group 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TSDF-based 3D Monte Carlo Localization ROS Package 2 | 3 | This package consists of several nodes and tools to perform a 6D Monte Carlo Localization of robots equipped with a 3D LiDAR in 3D TSDF maps. 4 | The sensor update is massively accelerated by a GPU-based implementation, but can also be executed on the CPU. 5 | 6 | [![Demo GIF](doc/tsdf_loc_teaser.gif)](https://github.com/uos/tsdf_localization_demo.git) 7 | 8 | ## Prerequisites 9 | * ROS Noetic (*ros-noetic-desktop-full*) 10 | * ROS packages: See `package.xml` 11 | * OpenMP (for CPU acceleration) 12 | * CUDA (optional, recommended for acceleration) 13 | 14 | ## Installation 15 | 16 | 1. Clone this repository into your ROS workspace 17 | ```console 18 | $ git clone --recursive https://github.com/uos/tsdf_localization.git 19 | ``` 20 | 2. Make sure have also installed the required external packages or also cloned them into the local ROS workspace 21 | 22 | 3. Build the ROS workspace 23 | ```console 24 | $ catkin build 25 | ``` 26 | 27 | ## Demo 28 | 29 | A quick startup including how to use tsdf_localization within your package is shown here: https://github.com/uos/tsdf_localization_demo.git 30 | 31 | ## Citation 32 | 33 | Please reference the following papers when using `tsdf_localization` in your scientific work. 34 | 35 | ```bib 36 | @inproceedings{eisoldt2023tsdfmcl, 37 | author={Eisoldt, Marc and Mock, Alexander and Porrmann, Mario and Wiemann, Thomas}, 38 | booktitle={2023 Seventh IEEE International Conference on Robotic Computing (IRC)}, 39 | title={{Towards 6D MCL for LiDARs in 3D TSDF Maps on Embedded Systems with GPUs}}, 40 | year={2023}, 41 | pages={158-165}, 42 | doi={10.1109/IRC59093.2023.00035} 43 | } 44 | ``` 45 | 46 | The paper is available on [IEEE Xplore](https://ieeexplore.ieee.org/document/10473560) and as preprint on [arXiv](https://arxiv.org/abs/2310.04172). 47 | 48 | 49 | ```bib 50 | @article{eisoldt2025tsdfmcl, 51 | author={Eisoldt, Marc and Mock, Alexander and Wiemann, Thomas and Porrmann, Mario}, 52 | title={Efficient Global 6D Localization in 3D TSDF Maps Using Point-wise and Scan-wise Reduction Methods on Embedded GPUs}, 53 | journal={International Journal of Semantic Computing}, 54 | doi={10.1142/S1793351X25410053} 55 | } 56 | ``` 57 | 58 | The paper is available on [World Scientific](https://www.worldscientific.com/doi/abs/10.1142/S1793351X25410053). 59 | 60 | 61 | ## Nodes 62 | 63 | ### mcl_3d 64 | 65 | Starts MCL in a given TSDF map. 66 | 67 | #### Subscribed Topics: 68 | 69 | `initialpose (geometry_msgs/PoseWithCovarianceStamped)` 70 | 71 | Initial pose guess can be provided using RViz. 72 | 73 | `/cloud (sensor_msgs/PointCloud2)` 74 | 75 | PointCloud topic for sensor update. 76 | 77 | `/odom (nav_msgs/Odometry)` 78 | 79 | Odometry message for motion update. 80 | 81 | (optional) `/imu_data (sensor_msgs/Imu)` 82 | 83 | #### Services 84 | 85 | Start global localization: 86 | 87 | `/global_localization` 88 | 89 | 90 | ## Contributions 91 | 92 | We are happy about issues and pull requests or other feedback. Please let us know if something did not work out as expected. 93 | 94 | -------------------------------------------------------------------------------- /cfg/MCL.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "tsdf_localization" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("number_of_particles", int_t, 0, "Defines the number of particles created per step", 100000, 0, 1000000) 9 | 10 | gen.add("init_sigma_x", double_t, 0, "The sigma for the normal distribution used for initializing the x coordinate of the particles", 0.5, 0, 10) 11 | gen.add("init_sigma_y", double_t, 0, "The sigma for the normal distribution used for initializing the y coordinate of the particles", 0.5, 0, 10) 12 | gen.add("init_sigma_z", double_t, 0, "The sigma for the normal distribution used for initializing the z coordinate of the particles", 0.5, 0, 10) 13 | 14 | gen.add("init_sigma_roll", double_t, 0, "The sigma for the normal distribution used for initializing the roll of the particles (in degrees)", 0, 0, 360) 15 | gen.add("init_sigma_pitch", double_t, 0, "The sigma for the normal distribution used for initializing the pitch of the particles (in degrees)", 0, 0, 360) 16 | gen.add("init_sigma_yaw", double_t, 0, "The sigma for the normal distribution used for initializing the yaw of the particles (in degrees)", 30, 0, 360) 17 | 18 | gen.add("delta_update_dist", double_t, 0, "Moved distance at which a resampling step should executed", 0.03, 0, 10) 19 | gen.add("delta_update_angle", double_t, 0, "rotated angle at which a resampling step should executed (in degrees)", 5, 0, 360) 20 | 21 | 22 | gen.add("use_cuda", bool_t, 0, "Using cuda for the sensor-update", True) 23 | 24 | gen.add("a_1", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0) 25 | gen.add("a_2", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0) 26 | gen.add("a_3", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0) 27 | gen.add("a_4", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0) 28 | gen.add("a_5", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0) 29 | gen.add("a_6", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0) 30 | gen.add("a_7", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0) 31 | gen.add("a_8", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0) 32 | gen.add("a_9", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0) 33 | gen.add("a_10", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0) 34 | gen.add("a_11", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0) 35 | gen.add("a_12", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0) 36 | 37 | gen.add("lin_scale", double_t, 0, "Linear scale of the motion update that only applies noise to the particles", 0.1, 0.0, 10.0) 38 | gen.add("ang_scale", double_t, 0, "Angular scale of the motion update that only applies noise to the particles", 0.1, 0.0, 10.0) 39 | 40 | evaluation_enum = gen.enum([ gen.const("Naiv", int_t, 0, "Naiv range evaluation"), 41 | gen.const("Likelihood", int_t, 1, "Likelihood range evaluation")], 42 | "An enum to set range evaluation model") 43 | 44 | resampling_enum = gen.enum([ gen.const("Wheel", int_t, 0, "Wheel Resampler"), 45 | gen.const("Residual", int_t, 1, "Residual Resampler"), 46 | gen.const("Systematic", int_t, 2, "Systematic Resampler"), 47 | gen.const("ResidualSystematic", int_t, 3, "Residual Systematic Resampler"), 48 | gen.const("Metropolis", int_t, 4, "Metropolis Resampler"), 49 | gen.const("RejectionSystematic", int_t, 5, "Rejection Resampler")], 50 | "An enum to set the resampling method") 51 | 52 | gen.add("evaluation_model", int_t, 0, "Evaluation model for evaluation the difference of an observed and a simulated scan", 1, 0, 1, edit_method=evaluation_enum) 53 | 54 | gen.add("resampling_method", int_t, 0, "Method for the resampling step in the particle filter", 1, 0, 5, edit_method=resampling_enum) 55 | 56 | exit(gen.generate(PACKAGE, "tsdf_localization", "MCL")) -------------------------------------------------------------------------------- /cfg/RING.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "mcl" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("ring_number", int_t, 0, "Number of the extracted scan ring", 10, 0, 14) 9 | gen.add("ring_2d", bool_t, 0, "Set z axis to zero?", True) 10 | 11 | exit(gen.generate(PACKAGE, "ring_extractor", "RING")) 12 | -------------------------------------------------------------------------------- /doc/tsdf_loc_teaser.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/tsdf_localization/b0945830b25569804c13fcd8dd23626d0e98f6a2/doc/tsdf_loc_teaser.gif -------------------------------------------------------------------------------- /include/tsdf_localization/cuda/cuda_data.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDA_DATA_H 2 | #define CUDA_DATA_H 3 | 4 | namespace tsdf_localization 5 | { 6 | 7 | #ifdef __CUDACC__ 8 | 9 | constexpr FLOAT_T SIGMAR = 0.1f; 10 | constexpr FLOAT_T SIGMAR_QUAD = SIGMAR * SIGMAR; 11 | 12 | //using OCC_T = int; 13 | 14 | __constant__ FLOAT_T const_tf_matrix[16]; 15 | //FLOAT_T const_tf_matrix[16]; 16 | 17 | CudaSubVoxelMap::MapCoef* g_map_coef_ = nullptr; 18 | __constant__ CudaSubVoxelMap::MapCoef const_map_coef_; 19 | 20 | cudaTextureObject_t grid_occ_tex_; 21 | 22 | cudaTextureObject_t* d_tex_grid_ = nullptr; 23 | std::vector d_arrays_; 24 | 25 | OCC_T* g_grid_occ_ = nullptr; 26 | FLOAT_T* g_data_ = nullptr; 27 | 28 | __constant__ FLOAT_T const_a_hit_; 29 | __constant__ FLOAT_T const_a_range_; 30 | __constant__ FLOAT_T const_a_max_; 31 | 32 | __constant__ FLOAT_T const_max_range_; 33 | __constant__ FLOAT_T const_max_range_squared_; 34 | __constant__ FLOAT_T const_inv_max_range_; 35 | 36 | 37 | #endif 38 | 39 | } // namespace tsdf_localization 40 | 41 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/cuda/cuda_evaluator.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDA_EVALUATOR_H 2 | #define CUDA_EVALUATOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | namespace tsdf_localization 20 | { 21 | 22 | struct CudaPoint 23 | { 24 | FLOAT_T x; 25 | FLOAT_T y; 26 | FLOAT_T z; 27 | 28 | CudaPoint(FLOAT_T x = 0, FLOAT_T y = 0, FLOAT_T z = 0) : x(x), y(y), z(z) {} 29 | 30 | bool operator==(const CudaPoint& other) const 31 | { 32 | return (this->x == other.x && this->y == other.y && this->z == other.z); 33 | } 34 | 35 | bool operator!=(const CudaPoint& other) const 36 | { 37 | return !(*this == other); 38 | } 39 | }; 40 | 41 | struct SortClass 42 | { 43 | SortClass() 44 | { 45 | 46 | } 47 | 48 | SortClass(int ring, int index, const CudaPoint& id_point, const std::shared_ptr& original = nullptr) : ring_(ring), index_(index), point_(id_point), original_(original) 49 | { 50 | 51 | } 52 | 53 | bool operator==(const SortClass& other) const 54 | { 55 | return ring_ == other.ring_ && point_ == other.point_; 56 | } 57 | 58 | int ring_; 59 | int index_; 60 | CudaPoint point_; 61 | std::shared_ptr original_; 62 | }; 63 | 64 | struct hash 65 | { 66 | std::size_t operator()(const CudaPoint& p) const noexcept 67 | { 68 | long long v = ((long long)(p.x * 1000) << 32) ^ ((long long)(p.y * 1000) << 16) ^ (long long)(p.z * 1000); 69 | return std::hash()(v); 70 | } 71 | 72 | std::size_t operator()(const std::pair& p) const noexcept 73 | { 74 | long long v = ((long long)(p.second.x * 1000) << 32) ^ ((long long)(p.second.y * 1000) << 16) ^ (long long)(p.second.z * 1000); 75 | return std::hash()(v); 76 | } 77 | 78 | std::size_t operator()(const std::pair>& p) const noexcept 79 | { 80 | long long v = ((long long)(p.second.second.x * 1000) << 32) ^ ((long long)(p.second.second.y * 1000) << 16) ^ (long long)(p.second.second.z * 1000); 81 | return std::hash()(v); 82 | } 83 | 84 | std::size_t operator()(const SortClass& p) const noexcept 85 | { 86 | long long v = ((long long)(p.point_.x * 1000) << 32) ^ ((long long)(p.point_.y * 1000) << 16) ^ (long long)(p.point_.z * 1000); 87 | return std::hash()(v); 88 | } 89 | 90 | }; 91 | 92 | constexpr size_t TRANSFORM_BYTES = sizeof(FLOAT_T) * 16; 93 | 94 | class Evaluator 95 | { 96 | public: 97 | virtual geometry_msgs::PoseWithCovariance evaluate(std::vector& particles, const sensor_msgs::PointCloud2& real_cloud, FLOAT_T tf_matrix[16]) 98 | { 99 | throw std::runtime_error("CUDA acceleration is not supported. Please install CUDA!"); 100 | } 101 | 102 | virtual geometry_msgs::PoseWithCovariance evaluate(std::vector& particles, const std::vector& points, FLOAT_T tf_matrix[16]) 103 | { 104 | throw std::runtime_error("CUDA acceleration is not supported. Please install CUDA!"); 105 | } 106 | 107 | }; 108 | 109 | class CudaEvaluator : public Evaluator 110 | { 111 | public: 112 | CudaEvaluator(CudaSubVoxelMap& map, bool per_point = false, FLOAT_T a_hit = 0.9, FLOAT_T a_range = 0.1, FLOAT_T a_max = 0.0, FLOAT_T max_range = 100.0); 113 | 114 | CudaEvaluator(const CudaEvaluator&) = delete; 115 | CudaEvaluator& operator=(const CudaEvaluator&) = delete; 116 | 117 | virtual geometry_msgs::PoseWithCovariance evaluate(std::vector& particles, const sensor_msgs::PointCloud2& real_cloud, FLOAT_T tf_matrix[16]); 118 | 119 | virtual geometry_msgs::PoseWithCovariance evaluate(std::vector& particles, const std::vector& points, FLOAT_T tf_matrix[16]); 120 | 121 | virtual ~CudaEvaluator(); 122 | 123 | private: 124 | CudaSubVoxelMap* d_map_; 125 | bool per_point_; 126 | 127 | OCC_T* d_grid_occ_; 128 | 129 | FLOAT_T* d_data_; 130 | 131 | Particle* d_particles_; 132 | FLOAT_T* d_particles_ordered_; 133 | size_t particles_reserved_; 134 | 135 | CudaPoint* d_points_; 136 | FLOAT_T* d_points_ordered_; 137 | size_t points_reserved_; 138 | 139 | FLOAT_T* d_transform_; 140 | 141 | std::vector new_weights_; 142 | FLOAT_T* d_new_weights_; 143 | 144 | FLOAT_T* d_point_weights_; 145 | size_t point_weights_size_; 146 | 147 | FLOAT_T* p_x_; 148 | FLOAT_T* p_y_; 149 | FLOAT_T* p_z_; 150 | 151 | FLOAT_T* sin_a_; 152 | FLOAT_T* cos_a_; 153 | 154 | FLOAT_T* sin_b_; 155 | FLOAT_T* cos_b_; 156 | 157 | FLOAT_T* sin_c_; 158 | FLOAT_T* cos_c_; 159 | 160 | FLOAT_T a_hit_; 161 | FLOAT_T a_range_; 162 | FLOAT_T a_max_; 163 | 164 | FLOAT_T max_range_; 165 | FLOAT_T inv_max_range_; 166 | FLOAT_T max_range_squared_; 167 | 168 | //curandState* dev_random_; 169 | }; 170 | 171 | 172 | } // namespace tsdf_localization 173 | 174 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/cuda/cuda_sub_voxel_map.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDA_SUB_VOXEL_MAP_H 2 | #define CUDA_SUB_VOXEL_MAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | 10 | namespace tsdf_localization 11 | { 12 | 13 | using OCC_T = int; 14 | 15 | constexpr FLOAT_T sub_voxel_size = 1.0; 16 | 17 | template 18 | class CudaSubVoxelMap 19 | { 20 | public: 21 | 22 | struct MapCoef 23 | { 24 | size_t dim_x_; 25 | size_t dim_y_; 26 | size_t dim_z_; 27 | 28 | IndexT min_x_; 29 | IndexT min_y_; 30 | IndexT min_z_; 31 | 32 | IndexT max_x_; 33 | IndexT max_y_; 34 | IndexT max_z_; 35 | 36 | IndexT resolution_; 37 | 38 | DataT init_value_; 39 | 40 | size_t up_dim_x_; 41 | size_t up_dim_y_; 42 | size_t up_dim_z_; 43 | size_t up_dim_2_; 44 | 45 | size_t sub_dim_; 46 | size_t sub_dim_2_; 47 | 48 | size_t grid_occ_size_; 49 | size_t data_size_; 50 | }; 51 | 52 | HOST_CALLABLE_MEMBER CudaSubVoxelMap(IndexT min_x , IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value = 0); 53 | 54 | HOST_CALLABLE_MEMBER void setTrackReuse(bool track_reuse) 55 | { 56 | track_reuse_ = track_reuse; 57 | } 58 | 59 | HOST_CALLABLE_MEMBER bool trackReuse() 60 | { 61 | return track_reuse_; 62 | } 63 | 64 | HOST_CALLABLE_MEMBER bool isNewTrack() 65 | { 66 | return new_track_; 67 | } 68 | 69 | HOST_CALLABLE_MEMBER int getTrackCount() 70 | { 71 | return track_count_; 72 | } 73 | 74 | HOST_CALLABLE_MEMBER int getLastTrack() 75 | { 76 | return last_track_; 77 | } 78 | 79 | HOST_CALLABLE_MEMBER int getInvalidCount() 80 | { 81 | return invalid_count_; 82 | } 83 | 84 | HOST_CALLABLE_MEMBER void resetInvalidCount() 85 | { 86 | invalid_count_ = 0; 87 | } 88 | 89 | HOST_CALLABLE_MEMBER void resetOccReuse() 90 | { 91 | for (auto index = 0; index < map_coef.grid_occ_size_; ++index) 92 | { 93 | occ_reuse_[index] = 0; 94 | } 95 | } 96 | 97 | CUDA_CALLABLE_MEMBER DataT getEntry(IndexT x, IndexT y, IndexT z); 98 | CUDA_CALLABLE_MEMBER void setEntry(IndexT x, IndexT y, IndexT z, DataT value); 99 | 100 | CUDA_CALLABLE_MEMBER size_t size() 101 | { 102 | return map_coef.data_size_ * sizeof(DataT) + map_coef.grid_occ_size_ * sizeof(long); 103 | } 104 | 105 | HOST_CALLABLE_MEMBER void setData(const std::vector>& data); 106 | 107 | HOST_CALLABLE_MEMBER ~CudaSubVoxelMap() 108 | { 109 | if (grid_occ_ != nullptr) 110 | { 111 | delete[] grid_occ_; 112 | grid_occ_ = nullptr; 113 | } 114 | 115 | if (data_ != nullptr) 116 | { 117 | delete[] data_; 118 | data_ = nullptr; 119 | } 120 | } 121 | 122 | CUDA_CALLABLE_MEMBER CudaSubVoxelMap(const CudaSubVoxelMap&) = delete; 123 | CUDA_CALLABLE_MEMBER CudaSubVoxelMap& operator=(const CudaSubVoxelMap&) = delete; 124 | 125 | HOST_CALLABLE_MEMBER CudaSubVoxelMap(CudaSubVoxelMap&& other) 126 | { 127 | move(other); 128 | } 129 | 130 | HOST_CALLABLE_MEMBER CudaSubVoxelMap& operator=(CudaSubVoxelMap&& other) 131 | { 132 | if (this == &other) 133 | { 134 | return *this; 135 | } 136 | 137 | move(other); 138 | return *this; 139 | } 140 | 141 | CUDA_CALLABLE_MEMBER OCC_T* rawGridOcc() 142 | { 143 | return grid_occ_; 144 | } 145 | 146 | CUDA_CALLABLE_MEMBER OCC_T** rawGridOccPtr() 147 | { 148 | return &grid_occ_; 149 | } 150 | 151 | HOST_CALLABLE_MEMBER long* occReuse() 152 | { 153 | return occ_reuse_; 154 | } 155 | 156 | CUDA_CALLABLE_MEMBER size_t gridOccSize() const 157 | { 158 | return map_coef.grid_occ_size_; 159 | } 160 | 161 | CUDA_CALLABLE_MEMBER size_t gridOccBytes() const 162 | { 163 | return map_coef.grid_occ_size_ * sizeof(OCC_T); 164 | } 165 | 166 | CUDA_CALLABLE_MEMBER DataT* rawData() 167 | { 168 | return data_; 169 | } 170 | 171 | CUDA_CALLABLE_MEMBER DataT** rawDataPtr() 172 | { 173 | return &data_; 174 | } 175 | 176 | CUDA_CALLABLE_MEMBER size_t subGridSize() const 177 | { 178 | return map_coef.sub_dim_ * map_coef.sub_dim_ * map_coef.sub_dim_; 179 | } 180 | 181 | CUDA_CALLABLE_MEMBER size_t subGridBytes() const 182 | { 183 | return subGridSize() * sizeof(DataT); 184 | } 185 | 186 | CUDA_CALLABLE_MEMBER size_t dataSize() const 187 | { 188 | return map_coef.data_size_; 189 | } 190 | 191 | CUDA_CALLABLE_MEMBER size_t dataBytes() const 192 | { 193 | return map_coef.data_size_ * sizeof(DataT); 194 | } 195 | 196 | CUDA_CALLABLE_MEMBER size_t getDimX() 197 | { 198 | return map_coef.dim_x_; 199 | } 200 | 201 | CUDA_CALLABLE_MEMBER size_t getDimY() 202 | { 203 | return map_coef.dim_y_; 204 | } 205 | 206 | CUDA_CALLABLE_MEMBER size_t getDimZ() 207 | { 208 | return map_coef.dim_z_; 209 | } 210 | 211 | CUDA_CALLABLE_MEMBER size_t getUpDimX() 212 | { 213 | return map_coef.up_dim_x_; 214 | } 215 | 216 | CUDA_CALLABLE_MEMBER size_t getUpDimY() 217 | { 218 | return map_coef.up_dim_y_; 219 | } 220 | 221 | CUDA_CALLABLE_MEMBER size_t getUpDimZ() 222 | { 223 | return map_coef.up_dim_z_; 224 | } 225 | 226 | CUDA_CALLABLE_MEMBER size_t getSubDim() 227 | { 228 | return map_coef.sub_dim_; 229 | } 230 | 231 | CUDA_CALLABLE_MEMBER IndexT getMinX() 232 | { 233 | return map_coef.min_x_; 234 | } 235 | 236 | CUDA_CALLABLE_MEMBER IndexT getMinY() 237 | { 238 | return map_coef.min_y_; 239 | } 240 | 241 | CUDA_CALLABLE_MEMBER IndexT getMinZ() 242 | { 243 | return map_coef.min_z_; 244 | } 245 | 246 | CUDA_CALLABLE_MEMBER IndexT getMaxX() 247 | { 248 | return map_coef.max_x_; 249 | } 250 | 251 | CUDA_CALLABLE_MEMBER IndexT getMaxY() 252 | { 253 | return map_coef.max_y_; 254 | } 255 | 256 | CUDA_CALLABLE_MEMBER IndexT getMaxZ() 257 | { 258 | return map_coef.max_z_; 259 | } 260 | 261 | CUDA_CALLABLE_MEMBER IndexT getResolution() 262 | { 263 | return map_coef.resolution_; 264 | } 265 | 266 | CUDA_CALLABLE_MEMBER DataT getInitValue() 267 | { 268 | return map_coef.init_value_; 269 | } 270 | 271 | CUDA_CALLABLE_MEMBER MapCoef& coef() 272 | { 273 | return map_coef; 274 | } 275 | 276 | private: 277 | void move(CudaSubVoxelMap& other) 278 | { 279 | grid_occ_ = other.grid_occ_; 280 | data_ = other.data_; 281 | occ_reuse_ = other.occ_reuse_; 282 | 283 | map_coef.grid_occ_size_ = other.map_coef.grid_occ_size_; 284 | map_coef.data_size_ = other.map_coef.data_size_; 285 | 286 | map_coef.up_dim_x_ = other.map_coef.up_dim_x_; 287 | map_coef.up_dim_y_ = other.map_coef.up_dim_y_; 288 | map_coef.up_dim_z_ = other.map_coef.up_dim_z_; 289 | map_coef.up_dim_2_ = other.map_coef.up_dim_2_; 290 | 291 | map_coef.sub_dim_ = other.map_coef.sub_dim_; 292 | map_coef.sub_dim_2_ = other.map_coef.sub_dim_2_; 293 | 294 | map_coef.grid_occ_size_ = other.map_coef.grid_occ_size_; 295 | map_coef.data_size_ = other.map_coef.data_size_; 296 | 297 | other.grid_occ_ = nullptr; 298 | other.data_ = nullptr; 299 | other.occ_reuse_ = nullptr; 300 | 301 | other.map_coef.grid_occ_size_ = 0; 302 | other.map_coef.data_size_ = 0; 303 | 304 | map_coef.dim_x_ = other.map_coef.dim_x_; 305 | map_coef.dim_y_ = other.map_coef.dim_y_; 306 | map_coef.dim_z_ = other.map_coef.dim_z_; 307 | 308 | map_coef.min_x_ = other.map_coef.min_x_; 309 | map_coef.min_y_ = other.map_coef.min_y_; 310 | map_coef.min_z_ = other.map_coef.min_z_; 311 | 312 | map_coef.max_x_ = other.map_coef.max_x_; 313 | map_coef.max_y_ = other.map_coef.max_y_; 314 | map_coef.max_z_ = other.map_coef.max_z_; 315 | 316 | map_coef.resolution_ = other.map_coef.resolution_; 317 | map_coef.init_value_ = other.map_coef.init_value_; 318 | 319 | track_reuse_ = other.track_reuse_; 320 | new_track_ = other.new_track_; 321 | track_count_ = other.track_count_; 322 | last_track_ = other.last_track_; 323 | last_index_ = other.last_index_; 324 | invalid_count_ = other.invalid_count_; 325 | } 326 | 327 | CUDA_CALLABLE_MEMBER size_t getIndex(IndexT x, IndexT y, IndexT z); 328 | 329 | OCC_T* grid_occ_; 330 | DataT* data_; 331 | 332 | MapCoef map_coef; 333 | 334 | bool track_reuse_; 335 | bool new_track_; 336 | int track_count_; 337 | int last_track_; 338 | int last_index_; 339 | int invalid_count_; 340 | 341 | long* occ_reuse_; 342 | }; 343 | 344 | } // namespace tsdf_localization 345 | 346 | #include 347 | 348 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/cuda/cuda_sub_voxel_map.tcc: -------------------------------------------------------------------------------- 1 | namespace tsdf_localization 2 | { 3 | 4 | template 5 | CudaSubVoxelMap::CudaSubVoxelMap(IndexT min_x , IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value) : 6 | grid_occ_(nullptr), 7 | data_(nullptr), 8 | track_reuse_(false), 9 | new_track_(false), 10 | track_count_(0), 11 | last_track_(0), 12 | last_index_(-1), 13 | invalid_count_(0), 14 | occ_reuse_(nullptr) 15 | { 16 | map_coef.dim_x_ = std::ceil(std::abs(max_x - min_x) / resolution); 17 | map_coef.dim_y_ = std::ceil(std::abs(max_y - min_y) / resolution); 18 | map_coef.dim_z_ = std::ceil(std::abs(max_z - min_z) / resolution); 19 | map_coef.min_x_ = min_x; 20 | map_coef.min_y_ = min_y; 21 | map_coef.min_z_ = min_z; 22 | map_coef.max_x_ = max_x; 23 | map_coef.max_y_ = max_y; 24 | map_coef.max_z_ = max_z; 25 | map_coef.resolution_ = resolution; 26 | map_coef.init_value_ = init_value; 27 | map_coef.up_dim_x_ = std::ceil(static_cast(std::abs(max_x - min_x)) / sub_voxel_size); 28 | map_coef.up_dim_y_ = std::ceil(static_cast(std::abs(max_y - min_y)) / sub_voxel_size); 29 | map_coef.up_dim_z_ = std::ceil(static_cast(std::abs(max_z - min_z)) / sub_voxel_size); 30 | map_coef.up_dim_2_ = map_coef.up_dim_x_ * map_coef.up_dim_y_; 31 | map_coef.sub_dim_ = std::ceil(static_cast(sub_voxel_size) / resolution); 32 | map_coef.sub_dim_2_ = map_coef.sub_dim_ * map_coef.sub_dim_; 33 | map_coef.grid_occ_size_ = map_coef.up_dim_x_ * map_coef.up_dim_y_ * map_coef.up_dim_z_; 34 | map_coef.data_size_ = 0; 35 | 36 | //this->grid_occ_.resize(up_dim_x_ * up_dim_y_ * up_dim_z_, -1); 37 | 38 | grid_occ_ = new OCC_T[map_coef.grid_occ_size_]; 39 | occ_reuse_ = new long[map_coef.grid_occ_size_]; 40 | 41 | for (auto index = 0; index < map_coef.grid_occ_size_; ++index) 42 | { 43 | grid_occ_[index] = -1; 44 | occ_reuse_[index] = 0; 45 | } 46 | 47 | //data_.resize(dim_x_ * dim_y_ * dim_z_, init_value); 48 | } 49 | 50 | template 51 | size_t CudaSubVoxelMap::getIndex(IndexT x, IndexT y, IndexT z) 52 | { 53 | size_t grid_x = x - map_coef.min_x_; 54 | size_t grid_y = y - map_coef.min_y_; 55 | size_t grid_z = z - map_coef.min_z_; 56 | 57 | 58 | grid_x /= map_coef.resolution_; 59 | grid_y /= map_coef.resolution_; 60 | grid_z /= map_coef.resolution_; 61 | 62 | if (grid_x >= map_coef.dim_x_ || grid_y >= map_coef.dim_y_ || grid_z >= map_coef.dim_z_) 63 | { 64 | return map_coef.data_size_; 65 | } 66 | 67 | auto x_offset = x - map_coef.min_x_; 68 | auto y_offset = y - map_coef.min_y_; 69 | auto z_offset = z - map_coef.min_z_; 70 | 71 | size_t up_x, up_y, up_z; 72 | 73 | 74 | up_x = (x_offset / sub_voxel_size); 75 | up_y = (y_offset / sub_voxel_size); 76 | up_z = (z_offset / sub_voxel_size); 77 | 78 | IndexT sub_pos_x, sub_pos_y, sub_pos_z; 79 | 80 | 81 | 82 | sub_pos_x = x_offset - up_x * sub_voxel_size; 83 | sub_pos_y = y_offset - up_y * sub_voxel_size; 84 | sub_pos_z = z_offset - up_z * sub_voxel_size; 85 | 86 | size_t up_index = up_x + up_y * map_coef.up_dim_x_ + up_z * map_coef.up_dim_2_; 87 | 88 | if (track_reuse_) 89 | { 90 | if (last_index_ == up_index) 91 | { 92 | new_track_ = false; 93 | ++track_count_; 94 | } 95 | else 96 | { 97 | if (last_index_ != -1) 98 | { 99 | new_track_ = true; 100 | last_track_ = track_count_; 101 | } 102 | 103 | last_index_ = up_index; 104 | track_count_ = 1; 105 | } 106 | } 107 | 108 | if (up_index >= map_coef.grid_occ_size_) 109 | { 110 | return map_coef.data_size_; 111 | } 112 | 113 | auto sub_index = grid_occ_[up_index]; 114 | 115 | if (track_reuse_) 116 | { 117 | ++occ_reuse_[up_index]; 118 | } 119 | 120 | if (sub_index < 0) 121 | { 122 | if (track_reuse_) 123 | { 124 | ++invalid_count_; 125 | } 126 | 127 | return map_coef.data_size_; 128 | } 129 | 130 | size_t sub_x, sub_y, sub_z; 131 | 132 | sub_x = sub_pos_x / map_coef.resolution_; 133 | sub_y = sub_pos_y / map_coef.resolution_; 134 | sub_z = sub_pos_z / map_coef.resolution_; 135 | 136 | return sub_index + sub_x + sub_y * map_coef.sub_dim_ + sub_z * map_coef.sub_dim_2_ ; 137 | } 138 | 139 | template 140 | DataT CudaSubVoxelMap::getEntry(IndexT x, IndexT y, IndexT z) 141 | { 142 | if (this->data_ == nullptr || this->grid_occ_ == nullptr) 143 | { 144 | return map_coef.init_value_; 145 | } 146 | 147 | auto index = this->getIndex(x, y, z); 148 | 149 | if (index < map_coef.data_size_) 150 | { 151 | return this->data_[index]; 152 | } 153 | else 154 | { 155 | return map_coef.init_value_; 156 | } 157 | } 158 | 159 | template 160 | void CudaSubVoxelMap::setEntry(IndexT x, IndexT y, IndexT z, DataT value) 161 | { 162 | if (this->data_ == nullptr || this->grid_occ_ == nullptr) 163 | { 164 | return; 165 | } 166 | 167 | this->data_[this->getIndex(x, y, z)] = value; 168 | } 169 | 170 | template 171 | void CudaSubVoxelMap::setData(const std::vector>& data) 172 | { 173 | if (data.size() == 0) 174 | { 175 | return; 176 | } 177 | 178 | for (const auto& cell : data) 179 | { 180 | auto x_offset = std::get<0>(cell) - map_coef.min_x_; 181 | auto y_offset = std::get<1>(cell) - map_coef.min_y_; 182 | auto z_offset = std::get<2>(cell) - map_coef.min_z_; 183 | 184 | size_t up_x = (x_offset / sub_voxel_size); 185 | size_t up_y = (y_offset / sub_voxel_size); 186 | size_t up_z = (z_offset / sub_voxel_size); 187 | 188 | auto up_index = up_x + up_y * map_coef.up_dim_x_ + up_z * map_coef.up_dim_x_ * map_coef.up_dim_y_; 189 | 190 | if (up_index >= map_coef.grid_occ_size_) 191 | { 192 | throw std::runtime_error("Upper voxel index overflow!"); 193 | } 194 | 195 | grid_occ_[up_index] = 0; 196 | } 197 | 198 | OCC_T current_offset = 0; 199 | 200 | auto sub_size = map_coef.sub_dim_ * map_coef.sub_dim_ * map_coef.sub_dim_; 201 | 202 | for (auto occ_index = 0u; occ_index < map_coef.grid_occ_size_; ++occ_index) 203 | { 204 | auto& up_cell = grid_occ_[occ_index]; 205 | 206 | if (up_cell >= 0) 207 | { 208 | up_cell = current_offset; 209 | current_offset += sub_size; 210 | } 211 | } 212 | 213 | if (data_ != nullptr) 214 | { 215 | delete[] data_; 216 | } 217 | 218 | map_coef.data_size_ = current_offset; 219 | data_ = new DataT[map_coef.data_size_]; 220 | 221 | for (auto index = 0; index < map_coef.data_size_; ++index) 222 | { 223 | data_[index] = map_coef.init_value_; 224 | } 225 | 226 | for (const auto& cell : data) 227 | { 228 | setEntry(std::get<0>(cell), std::get<1>(cell), std::get<2>(cell), std::get<3>(cell)); 229 | } 230 | } 231 | 232 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /include/tsdf_localization/cuda/cuda_sum.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDA_SUM_H 2 | #define CUDA_SUM_H 3 | 4 | #include 5 | #include 6 | 7 | namespace tsdf_localization 8 | { 9 | 10 | #ifdef __CUDACC__ 11 | 12 | __device__ void warpReduce(volatile FLOAT_T* sdata, unsigned int tid); 13 | 14 | __global__ void chunk_sums_kernel(const FLOAT_T* data, unsigned int data_size, unsigned int chunkSize, FLOAT_T* res); 15 | 16 | void chunk_sums(const FLOAT_T* data_d, unsigned int data_size, FLOAT_T* res_d, unsigned int Nchunks, unsigned int chunkSize); 17 | 18 | FLOAT_T* sumBatched(const FLOAT_T* data, size_t data_size, size_t batchSize); 19 | 20 | FLOAT_T* sumBatchedRecursive(const FLOAT_T* data, size_t data_size); 21 | 22 | FLOAT_T weightSum(const FLOAT_T* data, size_t data_size); 23 | 24 | } // namespace tsdf_localization 25 | 26 | #endif 27 | 28 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/cuda/cuda_util.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDA_UTIL_H 2 | #define CUDA_UTIL_H 3 | 4 | #ifdef __CUDACC__ 5 | 6 | #define CUDA_CALLABLE_MEMBER __host__ __device__ 7 | #define HOST_CALLABLE_MEMBER __host__ 8 | 9 | #include 10 | #include 11 | 12 | void cudaCheck(); 13 | 14 | #else 15 | 16 | #define CUDA_CALLABLE_MEMBER 17 | #define HOST_CALLABLE_MEMBER 18 | 19 | #endif 20 | 21 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/evaluation/model/evaluation_model.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file evaluation_model.h 3 | * @author Marc Eiosldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Interface to represent a class that can implement an evaluation model in the Monte Carlo Localization 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef EVALUATION_MODEL_H 14 | #define EVALUATION_MODEL_H 15 | 16 | #include 17 | 18 | namespace tsdf_localization 19 | { 20 | 21 | /** 22 | * @class EvaluationModel 23 | * 24 | * @brief Interface for a model for comparing two scans 25 | */ 26 | class EvaluationModel 27 | { 28 | public: 29 | /** 30 | * @brief Is called for every observed scan range with the corresponding expected scan range 31 | */ 32 | virtual void insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range) = 0; 33 | 34 | /** 35 | * @brief Is called for every observed scan range with the corresponding expected scan range 36 | */ 37 | virtual void insertMeasurement(FLOAT_T measurement) = 0; 38 | 39 | /** 40 | * @brief Is called after comparing every scan range for getting a probability, 41 | * which represents how good an observed scan matches a simulated scan 42 | */ 43 | virtual FLOAT_T evaluate() = 0; 44 | 45 | /** 46 | * @brief Is called before comparing two new scans 47 | */ 48 | virtual void reset() = 0; 49 | }; 50 | 51 | } // namespace tsdf_localization 52 | 53 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/evaluation/model/likelihood_evaluation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file likelihood_evaluation.h 3 | * @author Marc Eiosldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Class that implements a likelihood model 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef LIKELIHOODEVALUATION_H 14 | #define LIKELIHOODEVALUATION_H 15 | 16 | #include 17 | 18 | namespace tsdf_localization 19 | { 20 | 21 | constexpr float SIGMA = 0.1f; 22 | constexpr float SIGMA_QUAD = SIGMA * SIGMA; 23 | 24 | /** 25 | * @brief Class that implements a likelihood model 26 | * 27 | */ 28 | class LikelihoodEvaluation : public EvaluationModel 29 | { 30 | // Sum of scan point evaluations 31 | FLOAT_T m_sum; 32 | // Maximum range of a considered scan point 33 | FLOAT_T m_range_max; 34 | 35 | public: 36 | /** 37 | * @brief Construct a new Likelihood Evaluation object 38 | * 39 | * @param range_max Maximum range of an inserted measurement. All measurements with a higher range are ignored 40 | */ 41 | LikelihoodEvaluation(FLOAT_T range_max); 42 | 43 | /** 44 | * @brief Insert a range measurement of the environment to the evaluation model by comparing with a simulated range 45 | * 46 | * @param observed_range Measured range in the real environment 47 | * @param simulated_range Range simulated in the used map 48 | */ 49 | virtual void insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range) override; 50 | 51 | /** 52 | * @brief Insert a range measurement of the environment to the evaluation model 53 | * 54 | * @param measurement Measured range in the real environment 55 | */ 56 | virtual void insertMeasurement(FLOAT_T measurement) override; 57 | 58 | /** 59 | * @brief Get the evaluation result consindering all inserted measurements after the last reset 60 | * 61 | * @return FLOAT_T Result of the evaluation model 62 | */ 63 | virtual FLOAT_T evaluate() override; 64 | 65 | /** 66 | * @brief Resets the current evaluation procedure of scan 67 | * 68 | */ 69 | virtual void reset() override; 70 | 71 | }; 72 | 73 | } // namespace tsdf_localization 74 | 75 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/evaluation/model/naiv_evaluation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file naiv_evaluation.h 3 | * @author Marc Eiosldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Class that implements a naiv evaluation model 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef NAIV_EVALUATION_H 14 | #define NAIV_EVALUATION_H 15 | 16 | #include 17 | 18 | namespace tsdf_localization 19 | { 20 | 21 | /** 22 | * @brief Class that implements a naiv evaluation model 23 | * 24 | */ 25 | class NaivEvaluation : public EvaluationModel 26 | { 27 | // Calculated result of the naiv evaluation model by condering all inserted measurements 28 | FLOAT_T m_error; 29 | 30 | public: 31 | /** 32 | * @brief Construct a new Naiv Evaluation object 33 | * 34 | */ 35 | NaivEvaluation(); 36 | 37 | /** 38 | * @brief Insert a range measurement of the environment to the evaluation model by comparing with a simulated range 39 | * 40 | * @param observed_range Measured range in the real environment 41 | * @param simulated_range Range simulated in the used map 42 | */ 43 | virtual void insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range); 44 | 45 | /** 46 | * @brief Insert a range measurement of the environment to the evaluation model 47 | * 48 | * @param measurement Measured range in the real environment 49 | */ 50 | virtual void insertMeasurement(FLOAT_T measurement) override; 51 | 52 | /** 53 | * @brief Get the evaluation result consindering all inserted measurements after the last reset 54 | * 55 | * @return FLOAT_T Result of the evaluation model 56 | */ 57 | virtual FLOAT_T evaluate(); 58 | 59 | /** 60 | * @brief Resets the current evaluation procedure of scan 61 | * 62 | */ 63 | virtual void reset(); 64 | }; 65 | 66 | } // namespace tsdf_localization 67 | 68 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/evaluation/model/omp_likelihood_evaluation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file likelihood_evaluation.h 3 | * @author Marc Eiosldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Class that implements a likelihood model 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | 14 | #ifndef OMPLIKELIHOODEVALUATION_H 15 | #define OMPLIKELIHOODEVALUATION_H 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace tsdf_localization 23 | { 24 | 25 | /** 26 | * @brief Class that implements a likelihood model 27 | * 28 | */ 29 | class OMPLikelihoodEvaluation : public EvaluationModel 30 | { 31 | // Sum of scan point evaluations 32 | std::array m_sum_; 33 | // Maximum range of a considered scan point 34 | FLOAT_T m_range_max_; 35 | 36 | public: 37 | /** 38 | * @brief Construct a new Likelihood Evaluation object 39 | * 40 | * @param range_max Maximum range of an inserted measurement. All measurements with a higher range are ignored 41 | */ 42 | OMPLikelihoodEvaluation(FLOAT_T range_max); 43 | 44 | /** 45 | * @brief Insert a range measurement of the environment to the evaluation model by comparing with a simulated range 46 | * 47 | * @param observed_range Measured range in the real environment 48 | * @param simulated_range Range simulated in the used map 49 | */ 50 | virtual void insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range) override; 51 | 52 | /** 53 | * @brief Insert a range measurement of the environment to the evaluation model 54 | * 55 | * @param measurement Measured range in the real environment 56 | */ 57 | virtual void insertMeasurement(FLOAT_T measurement) override; 58 | 59 | /** 60 | * @brief Get the evaluation result consindering all inserted measurements after the last reset 61 | * 62 | * @return FLOAT_T Result of the evaluation model 63 | */ 64 | virtual FLOAT_T evaluate() override; 65 | 66 | /** 67 | * @brief Resets the current evaluation procedure of scan 68 | * 69 | */ 70 | virtual void reset() override; 71 | 72 | }; 73 | 74 | } // namespace tsdf_localization 75 | 76 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/evaluation/particle_evaluator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file particle_cloud.h 3 | * @author Marc Eiosldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Interface of a class to evaluate a particle cloud in the Monte Carlo Localization 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef PARTICLE_EVALUATOR_H 14 | #define PARTICLE_EVALUATOR_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace tsdf_localization 27 | { 28 | 29 | /** 30 | * @brief Interface to represent classes that can evaluate particle clouds in the Monte Carlo Localization 31 | * 32 | */ 33 | class ParticleEvaluator 34 | { 35 | protected: 36 | virtual FLOAT_T evaluatePose(geometry_msgs::Pose& pose, const sensor_msgs::LaserScan& scan, EvaluationModel& model) const = 0; 37 | 38 | public: 39 | virtual geometry_msgs::PoseWithCovariance evaluateParticles(ParticleCloud& particle_cloud, const sensor_msgs::LaserScan& real_scan, EvaluationModel& model, const std::string& robot_frame = "base_footprint", const std::string& scan_frame = "scanner") const = 0; 40 | }; 41 | 42 | } // namespace tsdf_localization 43 | 44 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/evaluation/tsdf_evaluator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tsdf_evaluator.h 3 | * @author Marc Eiosldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Class of a particle evaluator that works with a TSDF map and can be accelerated by using CUDA 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef TSDF_EVALUATOR_H 14 | #define TSDF_EVALUATOR_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | namespace tsdf_localization 30 | { 31 | 32 | // constexpr double map_res = 0.256; // 0.256; 33 | // constexpr double map_res_half = map_res / 2; 34 | 35 | /** 36 | * @brief Implementation of the sensor update with a given TSDF map 37 | */ 38 | class TSDFEvaluator 39 | { 40 | // Pointer to the TSDF map 41 | std::shared_ptr> map_ptr_; 42 | // Pointer to the GPU accelerated implementation of the sensor update 43 | std::shared_ptr cuda_evaluator_; 44 | 45 | // Parameters for the sensor update 46 | FLOAT_T a_hit_; 47 | FLOAT_T a_range_; 48 | FLOAT_T a_max_; 49 | FLOAT_T max_range_; 50 | FLOAT_T inv_max_range_; 51 | FLOAT_T max_range_squared_; 52 | FLOAT_T map_res_; 53 | FLOAT_T map_res_half_; 54 | 55 | protected: 56 | /** 57 | * @brief Evaluation of given pose (particle) with the measured laser scan in the TSDF map 58 | */ 59 | virtual FLOAT_T evaluatePose(FLOAT_T* pose, const std::vector cloud, EvaluationModel& model) const; 60 | 61 | public: 62 | /** 63 | * @brief Initialization of the TSDF evaluator for the sensor update 64 | * 65 | * @param map_ptr Pointer to the TSDF map that should be used in the sensor update 66 | * @param per_point Should the the per point implementation be used in the sensor update? 67 | * @param a_hit Weighting for the normal distribution of the scan to map difference in the sensor update 68 | * @param a_range Weighting for a random occuring scna point in the receiced laser ascan 69 | * @param a_max Weighting for scan point occuring at the maximum range of the laser scanner 70 | * @param max_range Maximum range of the laser scanner 71 | */ 72 | TSDFEvaluator(const std::shared_ptr>& map_ptr, bool per_point = false, FLOAT_T a_hit = 0.9, FLOAT_T a_range = 0.1, FLOAT_T a_max = 0.0, FLOAT_T max_range = 100.0, FLOAT_T reduction_cell_size = 0.064) : 73 | map_ptr_(map_ptr), 74 | a_hit_{a_hit}, a_range_(a_range), a_max_(a_max), 75 | max_range_(max_range), inv_max_range_(1.0 / max_range), max_range_squared_(max_range * max_range), 76 | map_res_(reduction_cell_size), map_res_half_(reduction_cell_size / 2) 77 | { 78 | cuda_evaluator_.reset(new CudaEvaluator(*map_ptr, per_point, a_hit_, a_range_, a_max_, max_range_)); 79 | //cuda_evaluator_.reset(new Evaluator()); 80 | } 81 | 82 | /** 83 | * @brief Destroy the TSDFEvaluator object 84 | */ 85 | ~TSDFEvaluator() 86 | { 87 | cuda_evaluator_.reset(); 88 | } 89 | 90 | /** 91 | * @brief Evaluate a given particle cloud in the provided TSDF map of the environment based on a measured laser scan 92 | * 93 | * @param particle_cloud Particle cloud taht should be evaluation in the sensor update 94 | * @param real_cloud Measured laser scan of the environment 95 | * @param robot_frame TF frame of the robot 96 | * @param scan_frame TF frame of the laser scanner 97 | * @param use_cuda Should the gpu acceleration be used? 98 | * @param ignore_tf Should the transformation between the origin of the robot and the laser scanner be ignored during the sensor update? 99 | * 100 | * @return geometry_msgs::PoseWithCovariance Pose estimation of the robot determined by the sensor update with uncertainty 101 | */ 102 | virtual geometry_msgs::PoseWithCovariance evaluateParticles(ParticleCloud& particle_cloud, const sensor_msgs::PointCloud2& real_cloud, const std::string& robot_frame = "base_footprint", const std::string& scan_frame = "scanner", bool use_cuda = false, bool ignore_tf = false); 103 | 104 | /** 105 | * @brief Evaluate a given particle cloud in the provided TSDF map of the environment based on a measured laser scan 106 | * 107 | * @param particles Particle cloud taht should be evaluation in the sensor update 108 | * @param points Measured laser scan of the environment 109 | * @param tf_matrix Tranformation between the reference system of the laser scanner and the robot 110 | * @param use_cuda Should the gpu acceleration be used? 111 | * 112 | * @return geometry_msgs::PoseWithCovariance Pose estimation of the robot determined by the sensor update with uncertainty 113 | */ 114 | virtual geometry_msgs::PoseWithCovariance evaluate(std::vector& particles, const std::vector& points, FLOAT_T tf_matrix[16], bool use_cuda = false); 115 | }; 116 | 117 | } // namespace tsdf_localization 118 | 119 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/int_mcl/int_constant.h: -------------------------------------------------------------------------------- 1 | #ifndef INTCONSTANT_H 2 | #define INTCONSTANT_H 3 | 4 | #include 5 | 6 | namespace int_tsdf_localization 7 | { 8 | using FIXED_T = int; 9 | 10 | constexpr int INT_RESOLUTION = 1024; 11 | constexpr int MATRIX_RESOLUTION = 1000; 12 | 13 | constexpr long MAX_RANGE_FIXED = tsdf_localization::MAX_RANGE * INT_RESOLUTION; 14 | constexpr long INV_MAX_RANGE_FIXED = tsdf_localization::INV_MAX_RANGE * INT_RESOLUTION; 15 | 16 | constexpr FIXED_T A_HIT_FIXED = tsdf_localization::A_HIT * INT_RESOLUTION; 17 | constexpr FIXED_T A_RAND_FIXED = tsdf_localization::A_RAND * INT_RESOLUTION; 18 | constexpr FIXED_T A_MAX_FIXED = tsdf_localization::A_MAX * INT_RESOLUTION; 19 | } // namespace int_tsdf_localization 20 | 21 | #endif // INTCONSTANT_H -------------------------------------------------------------------------------- /include/tsdf_localization/map/device_map.h: -------------------------------------------------------------------------------- 1 | #ifndef DEVICE_MAP_H 2 | #define DEVICE_MAP_H 3 | 4 | #include 5 | 6 | namespace tsdf_localization 7 | { 8 | 9 | template 10 | class DeviceMap 11 | { 12 | public: 13 | CUDA_CALLABLE_MEMBER virtual DataT getEntry(IndexT x, IndexT y, IndexT z) const = 0; 14 | CUDA_CALLABLE_MEMBER virtual void setEntry(IndexT x, IndexT y, IndexT z, DataT value) = 0; 15 | }; 16 | 17 | } 18 | 19 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/map/distance_function.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "groot.hpp" 4 | 5 | #define BMW auto 6 | 7 | constexpr double truncation = 0.6; 8 | 9 | namespace _3ds 10 | { 11 | 12 | class DistanceFunction 13 | { 14 | public: 15 | 16 | DistanceFunction(_3ds::Groot>& groot, size_t kn, size_t ki) : groot_(groot), kn_(kn), ki_(ki) 17 | { 18 | query_ = flann::Matrix(new double[3], 1, 3); 19 | 20 | groot_.searchIndex(groot_.dataset_, kn_, ki_, neighbor_indices_, neighbor_dists_); 21 | groot_.calcNormals(normals_, kn_, ki_, neighbor_indices_); 22 | } 23 | 24 | ~DistanceFunction() 25 | { 26 | delete[] query_.ptr(); 27 | } 28 | 29 | double distance(double x, double y, double z, size_t k = 1) 30 | { 31 | // Get nearest k point + normal from x,y,z 32 | 33 | query_[0][0] = x; 34 | query_[0][1] = y; 35 | query_[0][2] = z; 36 | 37 | Eigen::Vector3d point; 38 | 39 | point[0] = x; 40 | point[1] = y; 41 | point[2] = z; 42 | 43 | std::vector> indices; 44 | std::vector> dists; 45 | groot_.searchIndex(query_, k, k, indices, dists); 46 | 47 | if (dists[0][0] > truncation) 48 | { 49 | return dists[0][0]; 50 | } 51 | 52 | // flann::Matrix neighbor_query(new double[k * 3], k, 3); 53 | // std::vector> neighbor_indices; 54 | // std::vector> neighbor_dists; 55 | 56 | // for (auto index = 0u; index < indices[0].size(); ++index) 57 | // { 58 | // neighbor_query[index][0] = groot_.dataset_[(indices[0][index])][0]; 59 | // neighbor_query[index][1] = groot_.dataset_[(indices[0][index])][1]; 60 | // neighbor_query[index][2] = groot_.dataset_[(indices[0][index])][2]; 61 | // } 62 | 63 | 64 | // groot_.searchIndex(neighbor_query, kn_, kn_, neighbor_indices, neighbor_dists); 65 | 66 | std::vector<_3ds::Point> neighbors; 67 | 68 | // std::vector<_3ds::Point> normals; 69 | 70 | // groot_.calcNormals(normals, kn_, ki_, neighbor_indices_); 71 | 72 | _3ds::to_eigen(indices[0], groot_.dataset_, neighbors, k); 73 | 74 | // for all points (p - c).dot(n) 75 | 76 | BMW distance = 0.0; 77 | 78 | for (BMW index = 0u; index < neighbors.size(); ++index) 79 | { 80 | // std::vector<_3ds::Point> plane_points; 81 | 82 | // _3ds::to_eigen(neighbor_indices_[index], groot_.dataset_, plane_points, kn_); 83 | 84 | // _3ds::Point plane_center(0, 0, 0); 85 | 86 | // for (const auto& plane_point : plane_points) 87 | // { 88 | // plane_center += plane_point; 89 | // } 90 | 91 | // plane_center /= plane_points.size(); 92 | 93 | // distance += (point - neighbors[index]).dot(normals_[index]); 94 | 95 | distance += fabs((point - neighbors[index]).dot(normals_[indices[0][index]])); 96 | } 97 | 98 | // return average distances 99 | 100 | // delete[] neighbor_query.ptr(); 101 | 102 | return distance / neighbors.size(); 103 | } 104 | 105 | double distance(const Eigen::Vector3d& point, size_t k = 1) 106 | { 107 | return distance(point[0], point[1], point[2], k); 108 | } 109 | 110 | private: 111 | 112 | _3ds::Groot>& groot_; 113 | size_t kn_; 114 | size_t ki_; 115 | flann::Matrix query_; 116 | 117 | std::vector<_3ds::Point> normals_; 118 | 119 | std::vector> neighbor_indices_; 120 | std::vector> neighbor_dists_; 121 | 122 | }; 123 | 124 | } // namespace _3ds -------------------------------------------------------------------------------- /include/tsdf_localization/map/grid_map.h: -------------------------------------------------------------------------------- 1 | #ifndef GRIDMAP_H 2 | #define GRIDMAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace tsdf_localization 16 | { 17 | 18 | constexpr int CHUNK_SHIFT = 6; 19 | constexpr int CHUNK_SIZE = 1 << CHUNK_SHIFT; 20 | 21 | constexpr int MAP_SHIFT = 6; 22 | constexpr int MAP_RESOLUTION = 1 << MAP_SHIFT; 23 | 24 | constexpr FLOAT_T truncation = 600; //600; 25 | 26 | template 27 | class GridMap 28 | { 29 | public: 30 | GridMap(IndexT min_x , IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value = 0) : 31 | dim_x_(std::ceil(std::abs(max_x - min_x) / resolution)), 32 | dim_y_(std::ceil(std::abs(max_y - min_y) / resolution)), 33 | dim_z_(std::ceil(std::abs(max_z - min_z) / resolution)), 34 | min_x_(min_x), 35 | min_y_(min_y), 36 | min_z_(min_z), 37 | max_x_(max_x), 38 | max_y_(max_y), 39 | max_z_(max_z), 40 | resolution_(resolution), 41 | init_value_(init_value) 42 | { 43 | 44 | } 45 | 46 | #ifdef __CUDACC__ 47 | GridMap(const GridMap&) = default; 48 | GridMap& operator=(const GridMap&) = default; 49 | #endif 50 | 51 | GridMap(GridMap&&) = default; 52 | GridMap& operator=(GridMap&&) = default; 53 | 54 | virtual void setData(const std::vector>& data) = 0; 55 | 56 | CUDA_CALLABLE_MEMBER virtual DataT getEntry(IndexT x, IndexT y, IndexT z) const = 0; 57 | CUDA_CALLABLE_MEMBER virtual void setEntry(IndexT x, IndexT y, IndexT z, DataT value) = 0; 58 | 59 | CUDA_CALLABLE_MEMBER virtual size_t size() = 0; 60 | 61 | CUDA_CALLABLE_MEMBER virtual size_t getDimX() 62 | { 63 | return dim_x_; 64 | } 65 | 66 | CUDA_CALLABLE_MEMBER virtual size_t getDimY() 67 | { 68 | return dim_y_; 69 | } 70 | 71 | CUDA_CALLABLE_MEMBER virtual size_t getDimZ() 72 | { 73 | return dim_z_; 74 | } 75 | 76 | CUDA_CALLABLE_MEMBER virtual IndexT getMinX() 77 | { 78 | return min_x_; 79 | } 80 | 81 | CUDA_CALLABLE_MEMBER virtual IndexT getMinY() 82 | { 83 | return min_y_; 84 | } 85 | 86 | CUDA_CALLABLE_MEMBER virtual IndexT getMinZ() 87 | { 88 | return min_z_; 89 | } 90 | 91 | CUDA_CALLABLE_MEMBER virtual IndexT getMaxX() 92 | { 93 | return max_x_; 94 | } 95 | 96 | CUDA_CALLABLE_MEMBER virtual IndexT getMaxY() 97 | { 98 | return max_y_; 99 | } 100 | 101 | CUDA_CALLABLE_MEMBER virtual IndexT getMaxZ() 102 | { 103 | return max_z_; 104 | } 105 | 106 | CUDA_CALLABLE_MEMBER virtual IndexT getResolution() 107 | { 108 | return resolution_; 109 | } 110 | 111 | CUDA_CALLABLE_MEMBER virtual DataT getInitValue() 112 | { 113 | return init_value_; 114 | } 115 | 116 | protected: 117 | size_t dim_x_; 118 | size_t dim_y_; 119 | size_t dim_z_; 120 | 121 | IndexT min_x_; 122 | IndexT min_y_; 123 | IndexT min_z_; 124 | 125 | IndexT max_x_; 126 | IndexT max_y_; 127 | IndexT max_z_; 128 | 129 | IndexT resolution_; 130 | 131 | DataT init_value_; 132 | }; 133 | 134 | } // namespace tsdf_localization 135 | 136 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/map/hash_grid_map.h: -------------------------------------------------------------------------------- 1 | #ifndef NAIVGRIDMAP_H 2 | #define NAIVGRIDMAP_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace tsdf_localization 9 | { 10 | 11 | template 12 | class HashGridMap : public GridMap 13 | { 14 | public: 15 | HashGridMap(IndexT min_x , IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value = 0); 16 | 17 | virtual DataT getEntry(IndexT x, IndexT y, IndexT z) const override; 18 | virtual void setEntry(IndexT x, IndexT y, IndexT z, DataT value) override; 19 | 20 | virtual size_t size() override 21 | { 22 | return data_.size(); 23 | } 24 | 25 | virtual void setData(const std::vector>& data) override; 26 | 27 | std::unordered_map& data() 28 | { 29 | return data_; 30 | } 31 | 32 | private: 33 | size_t getIndex(IndexT x, IndexT y, IndexT z) const; 34 | 35 | using Base = GridMap; 36 | 37 | std::unordered_map data_; 38 | }; 39 | 40 | } // namespace tsdf_localization 41 | 42 | #include 43 | 44 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/map/hash_grid_map.tcc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace tsdf_localization 6 | { 7 | 8 | template 9 | HashGridMap::HashGridMap(IndexT min_x, IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value) : 10 | Base(min_x, min_y, min_z, max_x, max_y, max_z, resolution, init_value) 11 | { 12 | //data_.resize(Base::dim_x_ * Base::dim_y_ * Base::dim_z_, init_value); 13 | } 14 | 15 | template 16 | size_t HashGridMap::getIndex(IndexT x, IndexT y, IndexT z) const 17 | { 18 | size_t grid_x = (x - Base::min_x_) / Base::resolution_; 19 | size_t grid_y = (y - Base::min_y_) / Base::resolution_; 20 | size_t grid_z = (z - Base::min_z_) / Base::resolution_; 21 | 22 | if (grid_x >= Base::dim_x_ || grid_y >= Base::dim_y_ || grid_z >= Base::dim_z_) 23 | { 24 | throw std::invalid_argument("Coordinates out of bounds!"); 25 | } 26 | 27 | return grid_x + grid_y * Base::dim_x_ + grid_z * Base::dim_x_ * Base::dim_y_; 28 | } 29 | 30 | template 31 | DataT HashGridMap::getEntry(IndexT x, IndexT y, IndexT z) const 32 | { 33 | try 34 | { 35 | auto find = data_.find(getIndex(x, y, z)); 36 | 37 | if (find == data_.end()) 38 | { 39 | return Base::init_value_; 40 | } 41 | 42 | return (*find).second; 43 | } 44 | catch(...) 45 | { 46 | return Base::init_value_; 47 | } 48 | } 49 | 50 | template 51 | void HashGridMap::setEntry(IndexT x, IndexT y, IndexT z, DataT value) 52 | { 53 | data_[getIndex(x, y, z)] = value; 54 | } 55 | 56 | template 57 | void HashGridMap::setData(const std::vector>& data) 58 | { 59 | for (const auto& cell : data) 60 | { 61 | setEntry(std::get<0>(cell), std::get<1>(cell), std::get<2>(cell), std::get<3>(cell)); 62 | } 63 | } 64 | 65 | } -------------------------------------------------------------------------------- /include/tsdf_localization/map/sub_voxel_map.h: -------------------------------------------------------------------------------- 1 | #ifndef SUBVOXELMAP_H 2 | #define SUBVOXELMAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace tsdf_localization 9 | { 10 | 11 | template 12 | class SubVoxelMap : public DeviceMap, public GridMap 13 | { 14 | public: 15 | CUDA_CALLABLE_MEMBER SubVoxelMap(IndexT min_x , IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value = 0); 16 | 17 | CUDA_CALLABLE_MEMBER virtual DataT getEntry(IndexT x, IndexT y, IndexT z) const override; 18 | CUDA_CALLABLE_MEMBER virtual void setEntry(IndexT x, IndexT y, IndexT z, DataT value) override; 19 | 20 | CUDA_CALLABLE_MEMBER virtual size_t size() override 21 | { 22 | return data_size_ * sizeof(DataT) + grid_occ_size_ * sizeof(long); 23 | } 24 | 25 | HOST_CALLABLE_MEMBER virtual size_t getUpDimX() 26 | { 27 | return up_dim_x_; 28 | } 29 | 30 | HOST_CALLABLE_MEMBER virtual size_t getUpDimY() 31 | { 32 | return up_dim_y_; 33 | } 34 | 35 | HOST_CALLABLE_MEMBER virtual size_t getUpDimZ() 36 | { 37 | return up_dim_z_; 38 | } 39 | 40 | HOST_CALLABLE_MEMBER virtual void setData(const std::vector>& data) override; 41 | 42 | CUDA_CALLABLE_MEMBER virtual ~SubVoxelMap() 43 | { 44 | #ifndef __CUDACC__ 45 | if (grid_occ_ != nullptr) 46 | { 47 | delete[] grid_occ_; 48 | grid_occ_ = nullptr; 49 | } 50 | 51 | if (data_ != nullptr) 52 | { 53 | delete[] data_; 54 | data_ = nullptr; 55 | } 56 | #endif 57 | } 58 | 59 | #ifndef __CUDACC__ 60 | CUDA_CALLABLE_MEMBER SubVoxelMap(const SubVoxelMap&) = delete; 61 | CUDA_CALLABLE_MEMBER SubVoxelMap& operator=(const SubVoxelMap&) = delete; 62 | #else 63 | 64 | CUDA_CALLABLE_MEMBER void copy(const SubVoxelMap& other) 65 | { 66 | this->grid_occ_ = other.grid_occ_; 67 | this->data_ = other.data_; 68 | 69 | this->grid_occ_size_ = other.grid_occ_size_; 70 | this->data_size_ = other.data_size_; 71 | 72 | this->up_dim_x_ = other.up_dim_x_; 73 | this->up_dim_y_ = other.up_dim_y_; 74 | this->up_dim_z_ = other.up_dim_z_; 75 | this->up_dim_2_ = other.up_dim_2_; 76 | 77 | this->sub_dim_ = other.sub_dim_; 78 | this->sub_dim_2_ = other.sub_dim_2_; 79 | 80 | this->up_shift_ = other.up_shift_; 81 | 82 | this->grid_occ_size_ = other.grid_occ_size_; 83 | this->data_size_ = other.data_size_; 84 | this->occupancy_ = other.occupancy; 85 | } 86 | 87 | CUDA_CALLABLE_MEMBER SubVoxelMap(const SubVoxelMap& other) : Base(other) 88 | { 89 | copy(other); 90 | } 91 | 92 | CUDA_CALLABLE_MEMBER SubVoxelMap& operator=(const SubVoxelMap& other) 93 | { 94 | if (this == &other) 95 | { 96 | return *this; 97 | } 98 | 99 | Base::operator=(other); 100 | copy(other); 101 | }; 102 | 103 | 104 | #endif 105 | 106 | HOST_CALLABLE_MEMBER SubVoxelMap(SubVoxelMap&& other) : Base(std::move(other)) 107 | { 108 | move(other); 109 | } 110 | 111 | HOST_CALLABLE_MEMBER SubVoxelMap& operator=(SubVoxelMap&& other) 112 | { 113 | if (this == &other) 114 | { 115 | return *this; 116 | } 117 | 118 | Base::operator=(std::move(other)); 119 | move(other); 120 | return *this; 121 | } 122 | 123 | CUDA_CALLABLE_MEMBER long* rawGridOcc() 124 | { 125 | return grid_occ_; 126 | } 127 | 128 | CUDA_CALLABLE_MEMBER long** rawGridOccPtr() 129 | { 130 | return &grid_occ_; 131 | } 132 | 133 | CUDA_CALLABLE_MEMBER size_t gridOccSize() const 134 | { 135 | return grid_occ_size_; 136 | } 137 | 138 | CUDA_CALLABLE_MEMBER size_t gridOccBytes() const 139 | { 140 | return grid_occ_size_ * sizeof(long); 141 | } 142 | 143 | CUDA_CALLABLE_MEMBER DataT* rawData() 144 | { 145 | return data_; 146 | } 147 | 148 | CUDA_CALLABLE_MEMBER DataT** rawDataPtr() 149 | { 150 | return &data_; 151 | } 152 | 153 | CUDA_CALLABLE_MEMBER size_t dataSize() const 154 | { 155 | return data_size_; 156 | } 157 | 158 | CUDA_CALLABLE_MEMBER size_t dataBytes() const 159 | { 160 | return data_size_ * sizeof(DataT); 161 | } 162 | 163 | HOST_CALLABLE_MEMBER double occupancy() const 164 | { 165 | return occupancy_; 166 | } 167 | 168 | HOST_CALLABLE_MEMBER size_t subSize() const 169 | { 170 | return sub_dim_ * sub_dim_ * sub_dim_; 171 | } 172 | 173 | HOST_CALLABLE_MEMBER size_t subBytes() const 174 | { 175 | return subSize() * sizeof(DataT); 176 | } 177 | 178 | private: 179 | void move(SubVoxelMap& other) 180 | { 181 | this->grid_occ_ = other.grid_occ_; 182 | this->data_ = other.data_; 183 | 184 | this->grid_occ_size_ = other.grid_occ_size_; 185 | this->data_size_ = other.data_size_; 186 | 187 | this->up_dim_x_ = other.up_dim_x_; 188 | this->up_dim_y_ = other.up_dim_y_; 189 | this->up_dim_z_ = other.up_dim_z_; 190 | this->up_dim_2_ = other.up_dim_2_; 191 | 192 | this->sub_dim_ = other.sub_dim_; 193 | this->sub_dim_2_ = other.sub_dim_2_; 194 | 195 | this->up_shift_ = other.up_shift_; 196 | 197 | this->grid_occ_size_ = other.grid_occ_size_; 198 | this->data_size_ = other.data_size_; 199 | 200 | this->occupancy_ = other.occupancy_; 201 | 202 | other.grid_occ_ = nullptr; 203 | other.data_ = nullptr; 204 | 205 | other.grid_occ_size_ = 0; 206 | other.data_size_ = 0; 207 | } 208 | 209 | CUDA_CALLABLE_MEMBER size_t getIndex(IndexT x, IndexT y, IndexT z) const; 210 | 211 | using Base = GridMap; 212 | 213 | long* grid_occ_; 214 | DataT* data_; 215 | 216 | size_t up_dim_x_; 217 | size_t up_dim_y_; 218 | size_t up_dim_z_; 219 | size_t up_dim_2_; 220 | 221 | size_t sub_dim_; 222 | size_t sub_dim_2_; 223 | 224 | IndexT up_shift_; 225 | 226 | size_t grid_occ_size_; 227 | size_t data_size_; 228 | 229 | double occupancy_; 230 | }; 231 | 232 | } // namespace tsdf_localization 233 | 234 | #include 235 | 236 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/map/sub_voxel_map.tcc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace tsdf_localization 5 | { 6 | 7 | template 8 | SubVoxelMap::SubVoxelMap(IndexT min_x , IndexT min_y, IndexT min_z, IndexT max_x , IndexT max_y, IndexT max_z, IndexT resolution, DataT init_value) : 9 | Base(min_x, min_y, min_z, max_x, max_y, max_z, resolution, init_value), 10 | grid_occ_(nullptr), 11 | data_(nullptr), 12 | up_dim_x_(std::ceil(static_cast(std::abs(max_x - min_x)) / sub_voxel_size)), 13 | up_dim_y_(std::ceil(static_cast(std::abs(max_y - min_y)) / sub_voxel_size)), 14 | up_dim_z_(std::ceil(static_cast(std::abs(max_z - min_z)) / sub_voxel_size)), 15 | up_dim_2_(this->up_dim_x_ * this->up_dim_y_), 16 | sub_dim_(std::ceil(static_cast(sub_voxel_size) / resolution)), 17 | sub_dim_2_(sub_dim_ * sub_dim_), 18 | grid_occ_size_(up_dim_x_ * up_dim_y_ * up_dim_z_), 19 | data_size_(0) 20 | { 21 | auto log_2 = std::log2(sub_voxel_size); 22 | 23 | #ifndef __CUDACC__ 24 | if constexpr (std::is_integral_v) 25 | { 26 | if ((log_2 - static_cast(log_2)) != 0.0) 27 | { 28 | throw std::runtime_error("sub_voxel_size must be a pow of base 2!"); 29 | } 30 | } 31 | #endif 32 | 33 | up_shift_ = log_2; 34 | 35 | //this->grid_occ_.resize(up_dim_x_ * up_dim_y_ * up_dim_z_, -1); 36 | 37 | grid_occ_ = new long[grid_occ_size_]; 38 | 39 | for (auto index = 0; index < grid_occ_size_; ++index) 40 | { 41 | grid_occ_[index] = -1; 42 | } 43 | 44 | //data_.resize(Base::dim_x_ * Base::dim_y_ * Base::dim_z_, init_value); 45 | } 46 | 47 | template 48 | size_t SubVoxelMap::getIndex(IndexT x, IndexT y, IndexT z) const 49 | { 50 | // size_t grid_x = (x - Base::min_x_) / Base::resolution_; 51 | // size_t grid_y = (y - Base::min_y_) / Base::resolution_; 52 | // size_t grid_z = (z - Base::min_z_) / Base::resolution_; 53 | 54 | size_t grid_x = x - Base::min_x_; 55 | size_t grid_y = y - Base::min_y_; 56 | size_t grid_z = z - Base::min_z_; 57 | 58 | #ifndef __CUDACC__ 59 | if constexpr (std::is_integral_v) 60 | { 61 | grid_x >>= MAP_SHIFT; 62 | grid_y >>= MAP_SHIFT; 63 | grid_z >>= MAP_SHIFT; 64 | } 65 | else 66 | #endif 67 | { 68 | grid_x /= Base::resolution_; 69 | grid_y /= Base::resolution_; 70 | grid_z /= Base::resolution_; 71 | } 72 | 73 | if (grid_x >= Base::dim_x_ || grid_y >= Base::dim_y_ || grid_z >= Base::dim_z_) 74 | { 75 | return data_size_; 76 | 77 | //throw std::invalid_argument("Coordinates out of bounds!"); 78 | } 79 | 80 | auto x_offset = x - Base::min_x_; 81 | auto y_offset = y - Base::min_y_; 82 | auto z_offset = z - Base::min_z_; 83 | 84 | // size_t up_x = (x_offset / sub_voxel_size); 85 | // size_t up_y = (y_offset / sub_voxel_size); 86 | // size_t up_z = (z_offset / sub_voxel_size); 87 | 88 | size_t up_x, up_y, up_z; 89 | 90 | #ifndef __CUDACC__ 91 | if constexpr (std::is_integral_v) 92 | { 93 | up_x = x_offset >> up_shift_; 94 | up_y = y_offset >> up_shift_; 95 | up_z = z_offset >> up_shift_; 96 | } 97 | else 98 | #endif 99 | { 100 | up_x = (x_offset / sub_voxel_size); 101 | up_y = (y_offset / sub_voxel_size); 102 | up_z = (z_offset / sub_voxel_size); 103 | } 104 | 105 | // auto sub_pos_x = x_offset - up_x * sub_voxel_size; 106 | // auto sub_pos_y = y_offset - up_y * sub_voxel_size; 107 | // auto sub_pos_z = z_offset - up_z * sub_voxel_size; 108 | 109 | IndexT sub_pos_x, sub_pos_y, sub_pos_z; 110 | 111 | #ifndef __CUDACC__ 112 | if constexpr (std::is_integral_v) 113 | { 114 | sub_pos_x = x_offset - (up_x << up_shift_); 115 | sub_pos_y = y_offset - (up_y << up_shift_); 116 | sub_pos_z = z_offset - (up_z << up_shift_); 117 | } 118 | else 119 | #endif 120 | { 121 | sub_pos_x = x_offset - up_x * sub_voxel_size; 122 | sub_pos_y = y_offset - up_y * sub_voxel_size; 123 | sub_pos_z = z_offset - up_z * sub_voxel_size; 124 | } 125 | 126 | size_t up_index = up_x + up_y * this->up_dim_x_ + up_z * up_dim_2_; 127 | 128 | if (up_index >= grid_occ_size_) 129 | { 130 | #ifndef __CUDACC__ 131 | throw std::runtime_error("Upper voxel index overflow!"); 132 | #else 133 | return data_size_; 134 | #endif 135 | } 136 | 137 | auto sub_index = grid_occ_[up_index]; 138 | 139 | if (sub_index < 0) 140 | { 141 | return data_size_; 142 | //throw std::invalid_argument("Upper voxel entry does not exist!"); 143 | } 144 | 145 | // size_t sub_x = sub_pos_x / Base::resolution_; 146 | // size_t sub_y = sub_pos_y / Base::resolution_; 147 | // size_t sub_z = sub_pos_z / Base::resolution_; 148 | 149 | size_t sub_x, sub_y, sub_z; 150 | 151 | #ifndef __CUDACC__ 152 | if constexpr (std::is_integral_v) 153 | { 154 | sub_x = sub_pos_x >> MAP_SHIFT; 155 | sub_y = sub_pos_y >> MAP_SHIFT; 156 | sub_z = sub_pos_z >> MAP_SHIFT; 157 | } 158 | else 159 | #endif 160 | { 161 | sub_x = sub_pos_x / Base::resolution_; 162 | sub_y = sub_pos_y / Base::resolution_; 163 | sub_z = sub_pos_z / Base::resolution_; 164 | } 165 | 166 | return sub_index + sub_x + sub_y * this->sub_dim_ + sub_z * this->sub_dim_2_ ; 167 | } 168 | 169 | template 170 | DataT SubVoxelMap::getEntry(IndexT x, IndexT y, IndexT z) const 171 | { 172 | if (this->data_ == nullptr || this->grid_occ_ == nullptr) 173 | { 174 | return this->init_value_; 175 | } 176 | 177 | auto index = this->getIndex(x, y, z); 178 | 179 | if (index < data_size_) 180 | { 181 | return this->data_[index]; 182 | } 183 | else 184 | { 185 | return this->init_value_; 186 | } 187 | } 188 | 189 | template 190 | void SubVoxelMap::setEntry(IndexT x, IndexT y, IndexT z, DataT value) 191 | { 192 | if (this->data_ == nullptr || this->grid_occ_ == nullptr) 193 | { 194 | return; 195 | } 196 | 197 | this->data_[this->getIndex(x, y, z)] = value; 198 | } 199 | 200 | template 201 | void SubVoxelMap::setData(const std::vector>& data) 202 | { 203 | if (data.size() == 0) 204 | { 205 | return; 206 | } 207 | 208 | for (const auto& cell : data) 209 | { 210 | auto x_offset = std::get<0>(cell) - Base::min_x_; 211 | auto y_offset = std::get<1>(cell) - Base::min_y_; 212 | auto z_offset = std::get<2>(cell) - Base::min_z_; 213 | 214 | size_t up_x = (x_offset / sub_voxel_size); 215 | size_t up_y = (y_offset / sub_voxel_size); 216 | size_t up_z = (z_offset / sub_voxel_size); 217 | 218 | auto up_index = up_x + up_y * this->up_dim_x_ + up_z * this->up_dim_x_ * this->up_dim_y_; 219 | 220 | if (up_index >= grid_occ_size_) 221 | { 222 | throw std::runtime_error("Upper voxel index overflow!"); 223 | } 224 | 225 | grid_occ_[up_index] = 0; 226 | } 227 | 228 | long current_offset = 0; 229 | 230 | auto sub_size = this->sub_dim_ * this->sub_dim_ * this->sub_dim_; 231 | 232 | //for (auto& up_cell : grid_occ_) 233 | for (auto occ_index = 0u; occ_index < grid_occ_size_; ++occ_index) 234 | { 235 | auto& up_cell = grid_occ_[occ_index]; 236 | 237 | if (up_cell >= 0) 238 | { 239 | up_cell = current_offset; 240 | current_offset += sub_size; 241 | } 242 | } 243 | 244 | //data_.resize(current_offset, this->init_value_); 245 | 246 | if (data_ != nullptr) 247 | { 248 | delete[] data_; 249 | } 250 | 251 | occupancy_ = static_cast(current_offset / sub_size) / grid_occ_size_; 252 | 253 | data_size_ = current_offset; 254 | data_ = new DataT[data_size_]; 255 | 256 | for (auto index = 0; index < data_size_; ++index) 257 | { 258 | data_[index] = this->init_value_; 259 | } 260 | 261 | for (const auto& cell : data) 262 | { 263 | setEntry(std::get<0>(cell), std::get<1>(cell), std::get<2>(cell), std::get<3>(cell)); 264 | } 265 | } 266 | 267 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /include/tsdf_localization/particle.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file particle.h 3 | * @author Marc Eisoldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Definition of a particle that can be used on the CPU and the GPU 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef PARTICLE_H 14 | #define PARTICLE_H 15 | 16 | #include 17 | #include 18 | 19 | namespace tsdf_localization 20 | { 21 | 22 | /** 23 | * @brief Data structure to use a particle on CPU and GPU 24 | * 25 | */ 26 | struct Particle 27 | { 28 | CUDA_CALLABLE_MEMBER Particle() 29 | { 30 | 31 | } 32 | 33 | CUDA_CALLABLE_MEMBER Particle(const Particle& other) 34 | { 35 | *this = other; 36 | } 37 | 38 | CUDA_CALLABLE_MEMBER Particle& operator=(const Particle& other) 39 | { 40 | first[0] = other.first[0]; 41 | first[1] = other.first[1]; 42 | first[2] = other.first[2]; 43 | first[3] = other.first[3]; 44 | first[4] = other.first[4]; 45 | first[5] = other.first[5]; 46 | second = other.second; 47 | 48 | return *this; 49 | } 50 | 51 | // Coordinates (x, y, z, roll, pitch, yaw) 52 | FLOAT_T first[6]; 53 | // Weight of the particle used the Monte Carlo Localization 54 | FLOAT_T second; 55 | }; 56 | 57 | } // namespace tsdf_localization 58 | 59 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/particle_cloud.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file particle_cloud.h 3 | * @author Marc Eisoldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Class of the particle cloud with an integrated motion model 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #ifndef PARTICLE_CLOUD_H 14 | #define PARTICLE_CLOUD_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | namespace tsdf_localization 36 | { 37 | 38 | /// One Particle should consist of an two dimensional pose (x, y, rotation) and a weight 39 | //typedef std::pair Particle; 40 | 41 | /** 42 | * @class ParticleCloud 43 | * 44 | * @brief Represents the handling of the initialisation and resampling of particles 45 | */ 46 | class ParticleCloud 47 | { 48 | /// Array of the current particles 49 | std::vector m_particles; 50 | 51 | /// random generator 52 | std::shared_ptr m_generator_ptr; 53 | 54 | ros::Time m_last_time; 55 | 56 | // Reference pose of the robot tto determine the pose change since the last sensor update 57 | std::array ref_pose; 58 | 59 | // Parameters for the uncertainty model of the motion update 60 | FLOAT_T a_1_ = 0.1; 61 | FLOAT_T a_2_ = 0.1; 62 | FLOAT_T a_3_ = 0.1; 63 | FLOAT_T a_4_ = 0.1; 64 | FLOAT_T a_5_ = 0.1; 65 | FLOAT_T a_6_ = 0.1; 66 | FLOAT_T a_7_ = 0.1; 67 | FLOAT_T a_8_ = 0.1; 68 | FLOAT_T a_9_ = 0.1; 69 | FLOAT_T a_10_ = 0.1; 70 | FLOAT_T a_11_ = 0.1; 71 | FLOAT_T a_12_ = 0.1; 72 | 73 | // bool ignore_motion_; 74 | 75 | void apply_model(std::normal_distribution<>& distribution_linear_x, std::normal_distribution<>& distribution_linear_y, std::normal_distribution<>& distribution_linear_z, 76 | std::normal_distribution<>& distribution_roll, std::normal_distribution<>& distribution_pitch, std::normal_distribution<>& distribution_yaw, std::vector& new_particles); 77 | 78 | public: 79 | /** 80 | * @brief Only initializes the random generator 81 | */ 82 | ParticleCloud(); 83 | 84 | /** 85 | * @brief Initialize the particle cloud local with a normal distribution 86 | */ 87 | ParticleCloud(const geometry_msgs::Pose& center_pose, unsigned int number_particles, FLOAT_T init_sigma_x, FLOAT_T init_sigma_y, FLOAT_T init_sigma_z, FLOAT_T init_sigma_roll, FLOAT_T init_sigma_pitch, FLOAT_T init_sigma_yaw); 88 | 89 | /** 90 | * @brief Initialize the particle cloud local with a uniform distribution 91 | */ 92 | ParticleCloud(unsigned int number_particles, const geometry_msgs::Pose& center_pose, FLOAT_T dx, FLOAT_T dy, FLOAT_T dz, FLOAT_T droll = M_PI, FLOAT_T dpitch = M_PI, FLOAT_T dyaw = M_PI); 93 | 94 | /** 95 | * @brief Initialize the particle cloud global in the given map 96 | */ 97 | ParticleCloud(unsigned int number_particles, const std::vector& free_map, const geometry_msgs::Pose& center_pose, FLOAT_T droll = M_PI, FLOAT_T dpitch = M_PI, FLOAT_T dyaw = M_PI); 98 | 99 | /** 100 | * @brief Initializes the particles by sampling a normal distribution with a given pose as average 101 | * 102 | * @param center_pose Center of the sampled normal distribution of two dimensional poses 103 | * @param number_particles The number of particles that should be sampled 104 | */ 105 | void initialize(const geometry_msgs::Pose& center_pose, unsigned int number_particles, FLOAT_T init_sigma_x, FLOAT_T init_sigma_y, FLOAT_T init_sigma_z, FLOAT_T init_sigma_roll, FLOAT_T init_sigma_pitch, FLOAT_T init_sigma_yaw); 106 | 107 | /** 108 | * @brief Initialize the particle cloud local with a uniform distribution 109 | */ 110 | void initialize(unsigned int number_particles, const geometry_msgs::Pose& center_pose, FLOAT_T dx, FLOAT_T dy, FLOAT_T dz, FLOAT_T droll = M_PI, FLOAT_T dpitch = M_PI, FLOAT_T dyaw = M_PI); 111 | 112 | /** 113 | * @brief Initialize the particle cloud global in the given map 114 | */ 115 | void initialize(unsigned int number_particles, const std::vector& free_map, const geometry_msgs::Pose& center_pose, FLOAT_T droll = M_PI, FLOAT_T dpitch = M_PI, FLOAT_T dyaw = M_PI); 116 | 117 | 118 | /** 119 | * @brief Performs a motion update on the particle cloud based on an odometry estimation 120 | * 121 | * @param odom Containing information about the current action of the robot 122 | */ 123 | void motionUpdate(const nav_msgs::Odometry& odom); 124 | 125 | /** 126 | * @brief Performs a motion update on the particle cloud based on IMU data 127 | * 128 | * @param imu_date Containing information about the current action of the robot 129 | */ 130 | void motionUpdate(const ImuAccumulator::Data& imu_data); 131 | 132 | /** 133 | * @brief Performs a motion update on the particle cloud based on normal distributed noise 134 | * 135 | * @param lin_scale Noise parameter for the translastion 136 | * @param ang_scale Noise parameter for the rotation 137 | */ 138 | void motionUpdate(FLOAT_T lin_scale, FLOAT_T ang_scale); 139 | 140 | /** 141 | * @brief Performs a motion update on the particle cloud based on IMU data normal distributed noise for the translation 142 | * 143 | * @param lin_scale Noise parameter for the translastion 144 | * @param imu_data Containing information about the current action of the robot 145 | */ 146 | void motionUpdate(FLOAT_T lin_scale, const ImuAccumulator::Data& imu_data); 147 | 148 | /** 149 | * @brief Performs a motion update on the particle cloud based on a delta pose 150 | * 151 | * @param dx Translation in x-direction 152 | * @param dy Translation in y-direction 153 | * @param dz Translation in z-direction 154 | * @param roll Rotation around the x-axis 155 | * @param pitch Rotation around the y-axis 156 | * @param yaw Rotation around the z-axis 157 | * @param lin_base_noise Base noise to be applied on the translation of the particles 158 | * @param ang_base_noise Base noise to be applied on the rotation of the particles 159 | */ 160 | void motion_update(FLOAT_T dx, FLOAT_T dy, FLOAT_T dz, FLOAT_T roll, FLOAT_T pitch, FLOAT_T yaw, FLOAT_T lin_base_noise = 0, FLOAT_T ang_base_noise = 0); 161 | 162 | /** 163 | * @brief Set parameters of the motion update 164 | */ 165 | void setMotionParameters(FLOAT_T a_1, FLOAT_T a_2, FLOAT_T a_3, FLOAT_T a_4, FLOAT_T a_5, FLOAT_T a_6, FLOAT_T a_7, FLOAT_T a_8, FLOAT_T a_9, FLOAT_T a_10, FLOAT_T a_11, FLOAT_T a_12) 166 | { 167 | a_1_ = a_1; 168 | a_2_ = a_2; 169 | a_3_ = a_3; 170 | a_4_ = a_4; 171 | a_5_ = a_5; 172 | a_6_ = a_6; 173 | a_7_ = a_7; 174 | a_8_ = a_8; 175 | a_9_ = a_9; 176 | a_10_ = a_10; 177 | a_11_ = a_11; 178 | a_12_ = a_12; 179 | } 180 | 181 | /** 182 | * @brief Access to single particles 183 | * 184 | * @throw std::out_of_range 185 | */ 186 | Particle& operator[](unsigned int index); 187 | 188 | /** 189 | * @brief Returns the number of particles 190 | */ 191 | std::size_t size() const; 192 | 193 | std::vector& particles() 194 | { 195 | return m_particles; 196 | } 197 | 198 | /** 199 | * @brief Return the reference pose to determine the pose change of the robot 200 | */ 201 | const std::array& refPose() 202 | { 203 | return ref_pose; 204 | } 205 | 206 | /** 207 | * @brief Reset the reference pose of the robot 208 | */ 209 | void resetRef() 210 | { 211 | ref_pose[0] = 0; 212 | ref_pose[1] = 0; 213 | ref_pose[2] = 0; 214 | ref_pose[3] = 0; 215 | ref_pose[4] = 0; 216 | ref_pose[5] = 0; 217 | } 218 | 219 | /** 220 | * @brief Get the distance traveled by the robot since the last sensor update 221 | */ 222 | FLOAT_T refDist() 223 | { 224 | return std::sqrt(ref_pose[0] * ref_pose[0] + ref_pose[1] * ref_pose[1] + ref_pose[2] * ref_pose[2]); 225 | } 226 | 227 | /** 228 | * @brief Get the rotation change since the last sensor update 229 | */ 230 | FLOAT_T refAngle() 231 | { 232 | return std::fabs(ref_pose[5]); 233 | } 234 | 235 | /** 236 | * @brief Changes the number of particles 237 | */ 238 | void resize(unsigned int number_of_particles); 239 | 240 | /** 241 | * @brief The particles were initialized? 242 | */ 243 | bool isInitialized() const; 244 | 245 | /** 246 | * @brief Gets a random particle based on the weights of the current ones 247 | */ 248 | const Particle& getRandomParticle() const; 249 | 250 | /** 251 | * @brief Option to ignore the sensor information of the odometry or the IMU during the morion update and only apply noise to all particles 252 | * 253 | * @param state Should the sensor information be ignored during the motion update? 254 | */ 255 | // void setIgnoreMotion(bool state) 256 | // { 257 | // ignore_motion_ = state; 258 | // } 259 | }; 260 | 261 | } // namespace tsdf_localization 262 | 263 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/resampling/novel_resampling.h: -------------------------------------------------------------------------------- 1 | #ifndef NOVEL_RESAMPLING_H 2 | #define NOVEL_RESAMPLING_H 3 | 4 | #include 5 | 6 | namespace tsdf_localization 7 | { 8 | 9 | class ResidualResampler : public Resampler 10 | { 11 | public: 12 | void resample(ParticleCloud& particle_cloud) override 13 | { 14 | std::uniform_int_distribution uniform_distribution(0, particle_cloud.size() - 1); 15 | 16 | std::vector new_particles; 17 | new_particles.reserve(particle_cloud.size()); 18 | 19 | while (new_particles.size() < particle_cloud.size()) 20 | { 21 | auto random_index = uniform_distribution(*m_generator_ptr); 22 | auto& particle = particle_cloud[random_index]; 23 | auto expected_insertions = particle.second * particle_cloud.size(); 24 | auto insertions_left = particle_cloud.size() - new_particles.size(); 25 | auto insertions = expected_insertions <= insertions_left ? expected_insertions : insertions_left; 26 | 27 | for (size_t index = 0; index < insertions; ++index) 28 | { 29 | new_particles.push_back(particle); 30 | } 31 | } 32 | 33 | particle_cloud.particles() = std::move(new_particles); 34 | } 35 | 36 | }; 37 | 38 | class SystematicResampler : public Resampler 39 | { 40 | public: 41 | void resample(ParticleCloud& particle_cloud) override 42 | { 43 | auto inverse_M = 1.0 / particle_cloud.size(); 44 | std::uniform_real_distribution uniform_distribution(0.0, inverse_M); 45 | 46 | std::vector new_particles; 47 | new_particles.reserve(particle_cloud.size()); 48 | 49 | auto U = uniform_distribution(*m_generator_ptr); 50 | auto s = 0.0; 51 | 52 | for (size_t m = 0; m < particle_cloud.size(); ++m) 53 | { 54 | auto& particle = particle_cloud[m]; 55 | size_t k = 0; 56 | 57 | s += particle.second; 58 | 59 | while (s > U) 60 | { 61 | ++k; 62 | U += inverse_M; 63 | } 64 | 65 | for (size_t index = 0; index < k; ++index) 66 | { 67 | new_particles.push_back(particle); 68 | } 69 | } 70 | 71 | particle_cloud.particles() = std::move(new_particles); 72 | } 73 | 74 | }; 75 | 76 | class ResidualSystematicResampler : public Resampler 77 | { 78 | public: 79 | void resample(ParticleCloud& particle_cloud) override 80 | { 81 | std::uniform_real_distribution uniform_distribution(0.0, 1.0); 82 | 83 | std::vector new_particles; 84 | new_particles.reserve(particle_cloud.size()); 85 | 86 | auto u = uniform_distribution(*m_generator_ptr); 87 | 88 | for (size_t m = 0; m < particle_cloud.size(); ++m) 89 | { 90 | auto& particle = particle_cloud[m]; 91 | auto temp = particle_cloud.size() * particle.second - u; 92 | size_t o = static_cast(temp + 1.0); 93 | u = o - temp; 94 | 95 | for (size_t index = 0; index < o; ++index) 96 | { 97 | new_particles.push_back(particle); 98 | } 99 | } 100 | 101 | particle_cloud.particles() = std::move(new_particles); 102 | } 103 | 104 | }; 105 | 106 | class MetropolisResampler : public Resampler 107 | { 108 | size_t sampling_steps_; 109 | 110 | public: 111 | MetropolisResampler(size_t sampling_steps) : sampling_steps_(sampling_steps) {} 112 | 113 | void resample(ParticleCloud& particle_cloud) override 114 | { 115 | std::uniform_real_distribution uniform_real_distribution(0.0, 1.0); 116 | std::uniform_int_distribution uniform_int_distribution(0, particle_cloud.size() - 1); 117 | 118 | std::vector new_particles; 119 | new_particles.reserve(particle_cloud.size()); 120 | 121 | for (size_t i = 0; i < particle_cloud.size(); ++i) 122 | { 123 | auto k = 0; 124 | 125 | auto& particle_k = particle_cloud[k]; 126 | 127 | for (auto n = 0u; n < sampling_steps_; ++n) 128 | { 129 | auto u = uniform_real_distribution(*m_generator_ptr); 130 | auto j = uniform_int_distribution(*m_generator_ptr); 131 | auto& particle_j = particle_cloud[j]; 132 | 133 | if (u <= particle_j.second / particle_k.second) 134 | { 135 | k = j; 136 | } 137 | } 138 | 139 | new_particles.push_back(particle_cloud[k]); 140 | } 141 | 142 | particle_cloud.particles() = std::move(new_particles); 143 | } 144 | }; 145 | 146 | class RejectionResampler : public Resampler 147 | { 148 | public: 149 | void resample(ParticleCloud& particle_cloud) override 150 | { 151 | std::uniform_real_distribution uniform_real_distribution(0.0, 1.0); 152 | std::uniform_int_distribution uniform_int_distribution(0, particle_cloud.size() - 1); 153 | 154 | std::vector new_particles; 155 | new_particles.reserve(particle_cloud.size()); 156 | 157 | auto sup_w = 0.0; 158 | 159 | for (size_t index = 0; index < particle_cloud.size(); ++index) 160 | { 161 | auto w = particle_cloud[index].second; 162 | 163 | if (sup_w < w) 164 | { 165 | sup_w = w; 166 | } 167 | } 168 | 169 | for (size_t i = 0; i < particle_cloud.size(); ++i) 170 | { 171 | auto j = i; 172 | auto u = uniform_real_distribution(*m_generator_ptr); 173 | 174 | while (u > (particle_cloud[j].second / sup_w)) 175 | { 176 | j = uniform_int_distribution(*m_generator_ptr); 177 | u = uniform_real_distribution(*m_generator_ptr); 178 | } 179 | 180 | new_particles.push_back(particle_cloud[j]); 181 | } 182 | 183 | particle_cloud.particles() = std::move(new_particles); 184 | } 185 | 186 | }; 187 | 188 | } // namespace tsdf_localization 189 | 190 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/resampling/resampler.h: -------------------------------------------------------------------------------- 1 | #ifndef RESAMPLER_H 2 | #define RESAMPLER_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | namespace tsdf_localization 12 | { 13 | 14 | class ParticleCloud; 15 | 16 | class Resampler 17 | { 18 | public: 19 | 20 | Resampler() 21 | { 22 | std::random_device device{}; 23 | m_generator_ptr.reset(new std::mt19937(device())); 24 | } 25 | 26 | virtual void resample(ParticleCloud& particle_cloud) = 0; 27 | 28 | protected: 29 | std::unique_ptr m_generator_ptr; 30 | 31 | }; 32 | 33 | } // namespace tsdf_localization 34 | 35 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/resampling/wheel_resampler.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEELRESAMPLER_H 2 | #define WHEELRESAMPLER_H 3 | 4 | #include 5 | 6 | namespace tsdf_localization 7 | { 8 | 9 | class WheelResampler : public Resampler 10 | { 11 | public: 12 | WheelResampler() {} 13 | 14 | void resample(ParticleCloud& particle_cloud) override; 15 | }; 16 | 17 | } // namespace tsdf_localization 18 | 19 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/ring_buffer/ring_buffer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ring_buffer.h 3 | * @author Steffen Hinderink 4 | * @author Juri Vana 5 | */ 6 | 7 | #ifndef RING_BUFFER_H_ 8 | #define RING_BUFFER_H_ 9 | 10 | #include 11 | 12 | /** 13 | * Three dimensional array that can be shifted without needing to copy every entry. 14 | * This is done by implementing it as a ring in every dimension. 15 | * The ring buffer can be used as a local map containing truncated signed distance function (tsdf) values. 16 | * @tparam T type of the values stored in the ring buffer 17 | * 18 | * In order to use the global map with the ring buffer, T has to be std::pair. 19 | * Otherwise the function shift will not work. 20 | */ 21 | template 22 | class RingBuffer 23 | { 24 | 25 | private: 26 | 27 | /** 28 | * Side lengths of the ring buffer. They are always odd, so that there is a central cell. 29 | * The ring buffer contains sizeX * sizeY * sizeZ values. 30 | */ 31 | int sizeX; 32 | int sizeY; 33 | int sizeZ; 34 | 35 | /// Actual data of the ring buffer. 36 | T* data; 37 | 38 | /// Position of the center of the cuboid in global coordinates. 39 | int posX; 40 | int posY; 41 | int posZ; 42 | 43 | /** 44 | * Offset of the data in the ring. 45 | * Each variable is the index of the center of the cuboid in the data array in its dimension. 46 | * 47 | * If size = 5, pos = 1 (-> indices range from -1 to 3) and offset = 3 (in one dimension), 48 | * the indices of the global map in the data array are for example: 49 | * 3 -1 0 1 2 50 | * ^ 51 | * offset 52 | */ 53 | int offsetX; 54 | int offsetY; 55 | int offsetZ; 56 | 57 | /// Pointer to the global map in which the values outside of the buffer are stored 58 | std::shared_ptr map; 59 | 60 | public: 61 | 62 | /** 63 | * Constructor of the ring buffer. 64 | * The position is initialized to (0, 0, 0). 65 | * The sizes are given as parameters. If they are even, they are initialized as s + 1. 66 | * The offsets are initialized to size / 2, so that the ring boundarys match the array bounds. 67 | * @param sX Side length of the ring buffer in the x direction 68 | * @param sY Side length of the ring buffer in the y direction 69 | * @param sZ Side length of the ring buffer in the z direction 70 | * @param map Pointer to the global map 71 | */ 72 | RingBuffer(unsigned int sX, unsigned int sY, unsigned int sZ, std::shared_ptr map); 73 | 74 | /** 75 | * Copy constructor of the ring buffer. Creates a deep copy. 76 | * @param other Ring buffer to be copied 77 | */ 78 | RingBuffer(const RingBuffer& other); 79 | 80 | /** 81 | * Assignment operator of the ring buffer. Creates a deep copy. 82 | * @param other Ring buffer to be copied 83 | */ 84 | RingBuffer& operator=(const RingBuffer& other); 85 | 86 | /** 87 | * Destructor of the ring buffer. 88 | * Deletes the array in particular. 89 | */ 90 | virtual ~RingBuffer(); 91 | 92 | /** 93 | * Returns a value from the ring buffer per reference. 94 | * Throws an exception if the index is out of bounds i.e. if it is more than size / 2 away from the position. 95 | * @param x x-coordinate of the index in global coordinates 96 | * @param y y-coordinate of the index in global coordinates 97 | * @param z z-coordinate of the index in global coordinates 98 | * @return Value of the ring buffer 99 | */ 100 | T& value(int x, int y, int z); 101 | 102 | /** 103 | * Returns a value from the ring buffer per reference. 104 | * Throws an exception if the index is out of bounds i.e. if it is more than size / 2 away from the position. 105 | * @param x x-coordinate of the index in global coordinates 106 | * @param y y-coordinate of the index in global coordinates 107 | * @param z z-coordinate of the index in global coordinates 108 | * @return Value of the ring buffer 109 | */ 110 | const T& value(int x, int y, int z) const; 111 | 112 | /** 113 | * Returns the size of the ring buffer per reference in an array of length 3. 114 | * @param size Array in which the size is stored 115 | */ 116 | void getSize(int* size) const; 117 | 118 | /** 119 | * Returns the position of the center of the cuboid in global coordinates per reference in an array of length 3. 120 | * @param pos Array in which the position is stored 121 | */ 122 | void getPos(int* pos) const; 123 | 124 | /** 125 | * Shifts the ring buffer, so that a new position is the center of the cuboid. 126 | * Entries, that stay in the buffer, stay in place. 127 | * Values outside of the buffer are loaded from and stored in the global map. 128 | * @param x x-coordinate of the new position 129 | * @param y y-coordinate of the new position 130 | * @param z z-coordinate of the new position 131 | */ 132 | void shift(int x, int y, int z); 133 | 134 | /** 135 | * Checks if x, y and z are within the current range 136 | * 137 | * @param x x-coordinate to check 138 | * @param y y-coordinate to check 139 | * @param z z-coordinate to check 140 | * @return true if (x, y, z) is within the area of the buffer 141 | */ 142 | bool inBounds(int x, int y, int z) const; 143 | 144 | T* getData() const 145 | { 146 | return data; 147 | } 148 | 149 | /** 150 | * Alternative method to handle map overflow (should replace modulo) 151 | * 152 | * @param val Index which should be proved 153 | * @param max Upper bound for the index (exclusiv) 154 | * @return int The reduced value so that it is in the interval [0, max) 155 | */ 156 | int overflow(int val, int max) const 157 | { 158 | if (val >= 2 * max) 159 | { 160 | return val - 2 * max; 161 | } 162 | else if (val >= max) 163 | { 164 | return val - max; 165 | } 166 | else 167 | { 168 | return val; 169 | } 170 | } 171 | 172 | /** 173 | * Get correct index in the one dimensional data array based on the three dimensional indexing 174 | * 175 | * @param x Index for the x axis 176 | * @param y Index for the y axis 177 | * @param z Index for the z axis 178 | * @return int The equivalent index in the encapsulated one dimensional data array 179 | */ 180 | int getIndex(int x, int y, int z) const 181 | { 182 | int x_offset = overflow(x - posX + offsetX + sizeX, sizeX) * sizeY * sizeZ; 183 | int y_offset = overflow(y - posY + offsetY + sizeY, sizeY) * sizeZ; 184 | int z_offset = overflow(z - posZ + offsetZ + sizeZ, sizeZ); 185 | int index = x_offset + y_offset + z_offset; 186 | return index; 187 | } 188 | 189 | }; 190 | 191 | #include 192 | 193 | #endif 194 | -------------------------------------------------------------------------------- /include/tsdf_localization/ring_buffer/ring_buffer.tcc: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ring_buffer.cpp 3 | * @author Steffen Hinderink 4 | * @author Juri Vana 5 | */ 6 | 7 | #include // for abs 8 | #include 9 | 10 | template 11 | RingBuffer::RingBuffer(unsigned int sX, unsigned int sY, unsigned int sZ, std::shared_ptr map) 12 | : posX(0), 13 | posY(0), 14 | posZ(0), 15 | sizeX(sX % 2 == 1 ? sX : sX + 1), 16 | sizeY(sY % 2 == 1 ? sY : sY + 1), 17 | sizeZ(sZ % 2 == 1 ? sZ : sZ + 1), 18 | map(map) 19 | { 20 | offsetX = sizeX / 2; 21 | offsetY = sizeY / 2; 22 | offsetZ = sizeZ / 2; 23 | data = new T[sizeX * sizeY * sizeZ]; 24 | auto default_value = map->get_value(Vector3i(0, 0, 0)); 25 | for (int i = 0; i < sizeX * sizeY * sizeZ; i++) 26 | { 27 | data[i] = default_value; 28 | } 29 | } 30 | 31 | template 32 | RingBuffer::RingBuffer(const RingBuffer& other) : data(nullptr) 33 | { 34 | *this = other; 35 | } 36 | 37 | template 38 | RingBuffer& RingBuffer::operator=(const RingBuffer& other) 39 | { 40 | 41 | if (data == other.data) 42 | { 43 | return *this; 44 | } 45 | 46 | if (data != nullptr) 47 | { 48 | delete [] data; 49 | data = nullptr; 50 | } 51 | 52 | posX = other.posX; 53 | posY = other.posY; 54 | posZ = other.posZ; 55 | 56 | sizeX = other.sizeX; 57 | sizeY = other.sizeY; 58 | sizeZ = other.sizeZ; 59 | 60 | offsetX = other.offsetX; 61 | offsetY = other.offsetY; 62 | offsetZ = other.offsetZ; 63 | 64 | map = other.map; 65 | 66 | auto total_size = sizeX * sizeY * sizeZ; 67 | 68 | data = new T[total_size]; 69 | 70 | std::copy(other.data, other.data + total_size, data); 71 | 72 | return *this; 73 | } 74 | 75 | template 76 | RingBuffer::~RingBuffer() 77 | { 78 | if (data != nullptr) 79 | { 80 | delete [] data; 81 | data = nullptr; 82 | } 83 | } 84 | 85 | template 86 | T& RingBuffer::value(int x, int y, int z) 87 | { 88 | if (!inBounds(x, y, z)) 89 | { 90 | throw std::out_of_range("Index out of bounds"); 91 | } 92 | 93 | return data[getIndex(x, y, z)]; 94 | } 95 | 96 | template 97 | const T& RingBuffer::value(int x, int y, int z) const 98 | { 99 | if (!inBounds(x, y, z)) 100 | { 101 | throw std::out_of_range("Index out of bounds"); 102 | } 103 | return data[getIndex(x, y, z)]; 104 | } 105 | 106 | template 107 | void RingBuffer::getSize(int* size) const 108 | { 109 | size[0] = sizeX; 110 | size[1] = sizeY; 111 | size[2] = sizeZ; 112 | } 113 | 114 | template 115 | void RingBuffer::getPos(int* pos) const 116 | { 117 | pos[0] = posX; 118 | pos[1] = posY; 119 | pos[2] = posZ; 120 | } 121 | 122 | template 123 | bool RingBuffer::inBounds(int x, int y, int z) const 124 | { 125 | return abs(x - posX) <= sizeX / 2 && abs(y - posY) <= sizeY / 2 && abs(z - posZ) <= sizeZ / 2; 126 | } 127 | 128 | template 129 | void RingBuffer::shift(int x, int y, int z) 130 | { 131 | // x 132 | int diffX = x - posX; 133 | for (int i = 0; i < abs(diffX); i++) 134 | { 135 | // step x 136 | for (int j = posY - sizeY / 2; j <= posY + sizeY / 2; j++) 137 | { 138 | for (int k = posZ - sizeZ / 2; k <= posZ + sizeZ / 2; k++) 139 | { 140 | if (diffX > 0) 141 | { 142 | // step forward 143 | T& inout = value(posX - sizeX / 2, j, k); 144 | if (inout.second != 0) 145 | { 146 | map->set_value(Vector3i(posX - sizeX / 2, j, k), inout); 147 | } 148 | inout = map->get_value(Vector3i(posX + sizeX / 2 + 1, j, k)); 149 | } 150 | else 151 | { 152 | // step backwards 153 | T& inout = value(posX + sizeX / 2, j, k); 154 | if (inout.second != 0) 155 | { 156 | map->set_value(Vector3i(posX + sizeX / 2, j, k), inout); 157 | } 158 | inout = map->get_value(Vector3i(posX - sizeX / 2 - 1, j, k)); 159 | } 160 | } 161 | } 162 | posX = diffX > 0 ? posX + 1 : posX - 1; 163 | offsetX = diffX > 0 ? (offsetX + 1) % sizeX : (offsetX - 1 + sizeX) % sizeX; 164 | } 165 | 166 | // y 167 | int diffY = y - posY; 168 | for (int i = 0; i < abs(diffY); i++) 169 | { 170 | // step y 171 | for (int j = posX - sizeX / 2; j <= posX + sizeX / 2; j++) 172 | { 173 | for (int k = posZ - sizeZ / 2; k <= posZ + sizeZ / 2; k++) 174 | { 175 | if (diffY > 0) 176 | { 177 | // step forward 178 | T& inout = value(j, posY - sizeY / 2, k); 179 | if (inout.second != 0) 180 | { 181 | map->set_value(Vector3i(j, posY - sizeY / 2, k), inout); 182 | } 183 | inout = map->get_value(Vector3i(j, posY + sizeY / 2 + 1, k)); 184 | } 185 | else 186 | { 187 | // step backwards 188 | T& inout = value(j, posY + sizeY / 2, k); 189 | if (inout.second != 0) 190 | { 191 | map->set_value(Vector3i(j, posY + sizeY / 2, k), inout); 192 | } 193 | inout = map->get_value(Vector3i(j, posY - sizeY / 2 - 1, k)); 194 | } 195 | } 196 | } 197 | posY = diffY > 0 ? posY + 1 : posY - 1; 198 | offsetY = diffY > 0 ? (offsetY + 1) % sizeY : (offsetY - 1 + sizeY) % sizeY; 199 | } 200 | 201 | // z 202 | int diffZ = z - posZ; 203 | for (int i = 0; i < abs(diffZ); i++) 204 | { 205 | // step z 206 | for (int j = posX - sizeX / 2; j <= posX + sizeX / 2; j++) 207 | { 208 | for (int k = posY - sizeY / 2; k <= posY + sizeY / 2; k++) 209 | { 210 | if (diffZ > 0) 211 | { 212 | // step forward 213 | T& inout = value(j, k, posZ - sizeZ / 2); 214 | if (inout.second != 0) 215 | { 216 | map->set_value(Vector3i(j, k, posZ - sizeZ / 2), inout); 217 | } 218 | inout = map->get_value(Vector3i(j, k, posZ + sizeZ / 2 + 1)); 219 | } 220 | else 221 | { 222 | // step backwards 223 | T& inout = value(j, k, posZ + sizeZ / 2); 224 | if (inout.second != 0) 225 | { 226 | map->set_value(Vector3i(j, k, posZ + sizeZ / 2), inout); 227 | } 228 | inout = map->get_value(Vector3i(j, k, posZ - sizeZ / 2 - 1)); 229 | } 230 | } 231 | } 232 | posZ = diffZ > 0 ? posZ + 1 : posZ - 1; 233 | offsetZ = diffZ > 0 ? (offsetZ + 1) % sizeZ : (offsetZ - 1 + sizeZ) % sizeZ; 234 | } 235 | } 236 | -------------------------------------------------------------------------------- /include/tsdf_localization/util/constant.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTANT 2 | #define CONSTANT 3 | 4 | constexpr unsigned int OMP_THREADS = 8; 5 | 6 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/util/filter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** 4 | * @file filter.h 5 | * @author Julian Gaal 6 | */ 7 | 8 | #include 9 | 10 | namespace tsdf_localization 11 | { 12 | 13 | /** 14 | * Filter Base class 15 | * @tparam T Type of data to be filtered 16 | */ 17 | template 18 | class Filter 19 | { 20 | protected: 21 | /// Default Construktor 22 | Filter() = default; 23 | 24 | /// Default Destructor 25 | virtual ~Filter() = default; 26 | 27 | /// delete copy assignment operator 28 | Filter& operator=(const Filter& other) = delete; 29 | 30 | /// delete move assignment operator 31 | Filter& operator=(Filter&&) noexcept = delete; 32 | 33 | // default copy constructor 34 | Filter(const Filter&) = default; 35 | 36 | /// delete move constructor 37 | Filter(Filter&&) noexcept = delete; 38 | 39 | /** 40 | * Update performs one filter step 41 | * @param new_value new measurement 42 | * @return mean value 43 | */ 44 | virtual const T &update(const T &new_value) = 0; 45 | }; 46 | 47 | /** 48 | * Performs Average calculation across a dynamic window 49 | * https://de.wikipedia.org/wiki/Gleitender_Mittelwert#Gleitender_Durchschnitt_mit_dynamischem_Fenster 50 | * 51 | * @tparam T Datatype to be filtered 52 | */ 53 | template 54 | class SlidingWindowFilter : public Filter 55 | { 56 | public: 57 | /** 58 | * Construct a Sliding Window Filter 59 | * @param window_size size of window 60 | */ 61 | explicit SlidingWindowFilter(size_t window_size) 62 | : Filter{} 63 | , window_size_{static_cast(window_size)} 64 | , buffer_{} 65 | , mean_{} 66 | {} 67 | 68 | /** 69 | * Performs update of average with new data across dynamic window 70 | * 71 | * @param new_value type T 72 | * @return updated mean 73 | */ 74 | const T &update(const T &new_value) 75 | { 76 | /// Window too small for sensible results 77 | if (window_size_ < 2) 78 | { 79 | return new_value; 80 | } 81 | 82 | buffer_.push(new_value); 83 | 84 | /// Fill buffer until window_size is reached (initialization) 85 | if (buffer_.size() <= window_size_) 86 | { 87 | mean_ += (new_value / window_size_); 88 | return new_value; 89 | } 90 | 91 | /// Calculate mean and throw away head of queue 92 | mean_ += (buffer_.back() - buffer_.front())/window_size_; 93 | buffer_.pop(); 94 | return mean_; 95 | } 96 | 97 | /// Get const reference to underlying buffer 98 | const std::queue& get_buffer() const 99 | { 100 | return buffer_; 101 | } 102 | 103 | /// Get const reference to current mean 104 | const T& get_mean() const 105 | { 106 | return mean_; 107 | } 108 | 109 | private: 110 | /// Window size: to not risk 111 | double window_size_; 112 | 113 | /// Queue thats used to implement the dynamic window 114 | std::queue buffer_; 115 | 116 | /// The current mean 117 | T mean_; 118 | }; 119 | 120 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /include/tsdf_localization/util/global_map.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** 4 | * @author Steffen Hinderink 5 | * @author Juri Vana 6 | * @author Malte Hillmann 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | using Eigen::Vector3i; 19 | 20 | struct ActiveChunk 21 | { 22 | std::vector data; 23 | Vector3i pos; 24 | int age; 25 | }; 26 | 27 | /** 28 | * Global map containing containing truncated signed distance function (tsdf) values and weights. 29 | * The map is divided into chunks. 30 | * An HDF5 file is used to store the chunks. 31 | * Additionally poses can be saved. 32 | */ 33 | class GlobalMap 34 | { 35 | 36 | private: 37 | 38 | /** 39 | * HDF5 file in which the chunks are stored. 40 | * The file structure looks like this: 41 | * 42 | * file.h5 43 | * | 44 | * |-/map 45 | * | | 46 | * | |-0_0_0 \ 47 | * | |-0_0_1 \ 48 | * | |-0_1_0 chunk datasets named after their tag 49 | * | |-0_1_1 / 50 | * | |-1_0_0 / 51 | * | 52 | * |-/poses 53 | * | | 54 | * | |-0 \ 55 | * | |-1 pose datasets named in ascending order containing 6 values each 56 | * | |-2 / 57 | */ 58 | HighFive::File file_; 59 | 60 | /// Initial default tsdf value. 61 | TSDFValue initial_tsdf_value_; 62 | 63 | 64 | /** 65 | * Vector of active chunks. 66 | */ 67 | std::vector active_chunks_; 68 | 69 | /// Number of poses that are saved in the HDF5 file 70 | int num_poses_; 71 | 72 | /** 73 | * Given a position in a chunk the tag of the chunk gets returned. 74 | * @param pos the position 75 | * @return tag of the chunk 76 | */ 77 | std::string tag_from_chunk_pos(const Vector3i& pos); 78 | 79 | /** 80 | * Returns the index of a global position in a chunk. 81 | * The returned index is that of the tsdf value. 82 | * The index of the weight is one greater. 83 | * @param pos the position 84 | * @return index in the chunk 85 | */ 86 | int index_from_pos(Vector3i pos, const Vector3i& chunkPos); 87 | 88 | /** 89 | * Activates a chunk and returns it by reference. 90 | * If the chunk was already active, it is simply returned. 91 | * Else the HDF5 file is checked for the chunk. 92 | * If it also doesn't exist there, a new empty chunk is created. 93 | * Chunks get replaced and written into the HDF5 file by a LRU strategy. 94 | * The age of the activated chunk is reset and all ages are updated. 95 | * @param chunk position of the chunk that gets activated 96 | * @return reference to the activated chunk 97 | */ 98 | std::vector& activate_chunk(const Vector3i& chunk); 99 | 100 | public: 101 | 102 | /** 103 | * log(CHUNK_SIZE). 104 | * The side length is a power of 2 so that divisions by the side length can be accomplished by shifting. 105 | */ 106 | static constexpr int CHUNK_SHIFT = 6; 107 | 108 | /// Side length of the cube-shaped chunks (2^CHUNK_SHIFT). 109 | static constexpr int CHUNK_SIZE = 1 << CHUNK_SHIFT; 110 | 111 | /// Number of voxels in one chunk (CHUNK_SIZE^3). 112 | static constexpr int SINGLE_SIZE = 1 << (3 * CHUNK_SHIFT); // = CHUNK_SIZE * CHUNK_SIZE * CHUNK_SIZE 113 | 114 | /** 115 | * Number of entries in one chunk (SINGLE_SIZE * 2). 116 | * Per voxel a tsdf value and a weight is stored. 117 | */ 118 | static constexpr int TOTAL_SIZE = SINGLE_SIZE * 2; 119 | 120 | /// Maximum number of active chunks. 121 | static constexpr int NUM_CHUNKS = 64; 122 | 123 | /** 124 | * Constructor of the global map. 125 | * It is initialized without chunks. 126 | * The chunks are instead created dynamically depending on which are used. 127 | * @param name name with path and extension (.h5) of the HDF5 file in which the map is stored 128 | * @param initialTsdfValue initial default tsdf value 129 | * @param initialWeight initial default weight 130 | */ 131 | GlobalMap(std::string name, TSDFValue::ValueType initial_tsdf_value, TSDFValue::WeightType initial_weight); 132 | 133 | /** 134 | * Returns a value pair consisting of a tsdf value and a weight from the map. 135 | * @param pos the position 136 | * @return value pair from the map 137 | */ 138 | TSDFValue get_value(const Vector3i& pos); 139 | 140 | /** 141 | * Sets a value pair consisting of a tsdf value and a weight on the map. 142 | * @param pos the position 143 | * @param value value pair that is set 144 | */ 145 | void set_value(const Vector3i& pos, const TSDFValue& value); 146 | 147 | /** 148 | * Saves a pose in the HDF5 file. 149 | * @param x x-coordinate of the position of the pose 150 | * @param y y-coordinate of the position of the pose 151 | * @param z z-coordinate of the position of the pose 152 | * @param roll roll value of the rotation of the pose 153 | * @param pitch pitch value of the rotation of the pose 154 | * @param yaw yaw value of the rotation of the pose 155 | */ 156 | void savePose(float x, float y, float z, float roll, float pitch, float yaw); 157 | 158 | /** 159 | * Writes all active chunks into the HDF5 file. 160 | */ 161 | void write_back(); 162 | 163 | }; 164 | -------------------------------------------------------------------------------- /include/tsdf_localization/util/imu_accumulator.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_ACC_H 2 | #define IMU_ACC_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace tsdf_localization 12 | { 13 | 14 | void bound_angle(FLOAT_T& angle); 15 | 16 | class ImuAccumulator 17 | { 18 | public: 19 | 20 | struct Data 21 | { 22 | FLOAT_T linear_vel = 0.0; 23 | FLOAT_T delta_x = 0.0; 24 | 25 | FLOAT_T angular_roll = 0.0; 26 | FLOAT_T angular_pitch = 0.0; 27 | FLOAT_T angular_yaw = 0.0; 28 | 29 | FLOAT_T delta_roll = 0.0; 30 | FLOAT_T delta_pitch = 0.0; 31 | FLOAT_T delta_yaw = 0.0; 32 | }; 33 | 34 | ImuAccumulator() : first_(true), data_{0.0, 0.0, 0.0, 0.0, 0.0} 35 | { 36 | 37 | } 38 | 39 | void update(const sensor_msgs::Imu& imu) 40 | { 41 | mutex_.lock(); 42 | 43 | if (first_) 44 | { 45 | last_ = ros::Time::now(); 46 | first_ = false; 47 | 48 | mutex_.unlock(); 49 | 50 | return; 51 | } 52 | 53 | ros::Time current = ros::Time::now(); 54 | 55 | auto dt = (current - last_).toSec(); 56 | 57 | data_.delta_x += 0.5 * -imu.linear_acceleration.x * dt * dt + data_.linear_vel * dt; 58 | data_.linear_vel += -imu.linear_acceleration.x * dt; 59 | 60 | data_.delta_roll += imu.angular_velocity.x * dt; 61 | bound_angle(data_.delta_roll); 62 | 63 | data_.delta_pitch += imu.angular_velocity.y * dt; 64 | bound_angle(data_.delta_pitch); 65 | 66 | data_.delta_yaw += imu.angular_velocity.z * dt; 67 | bound_angle(data_.delta_yaw); 68 | 69 | if (std::fabs(data_.angular_roll) < std::fabs(imu.angular_velocity.x)) 70 | { 71 | data_.angular_roll = imu.angular_velocity.x; 72 | } 73 | 74 | if (std::fabs(data_.angular_pitch) < std::fabs(imu.angular_velocity.y)) 75 | { 76 | data_.angular_pitch = imu.angular_velocity.y; 77 | } 78 | 79 | if (std::fabs(data_.angular_yaw) < std::fabs(imu.angular_velocity.z)) 80 | { 81 | data_.angular_yaw = imu.angular_velocity.z; 82 | } 83 | 84 | last_ = current; 85 | 86 | mutex_.unlock(); 87 | } 88 | 89 | void getData(Data& data) 90 | { 91 | // mutex_.lock(); 92 | data = data_; 93 | // mutex_.unlock(); 94 | } 95 | 96 | void reset() 97 | { 98 | // mutex_.lock(); 99 | 100 | data_.delta_x = 0.0; 101 | 102 | data_.delta_roll = 0.0; 103 | data_.delta_pitch = 0.0; 104 | data_.delta_yaw = 0.0; 105 | 106 | data_.angular_roll = 0.0; 107 | data_.angular_pitch = 0.0; 108 | data_.angular_yaw = 0.0; 109 | 110 | // mutex_.unlock(); 111 | } 112 | 113 | void getAndResetData(Data& data) 114 | { 115 | mutex_.lock(); 116 | 117 | getData(data); 118 | reset(); 119 | 120 | mutex_.unlock(); 121 | } 122 | 123 | private: 124 | bool first_; 125 | ros::Time last_; 126 | 127 | Data data_; 128 | 129 | std::mutex mutex_; 130 | }; 131 | 132 | } // namespace tsdf_localization 133 | 134 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/util/mcl_file.h: -------------------------------------------------------------------------------- 1 | #ifndef MCL_FILE_H 2 | #define MCL_FILE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #ifndef __CUDACC__ 13 | 14 | #include 15 | 16 | #endif 17 | 18 | namespace tsdf_localization 19 | { 20 | 21 | class MCLFile 22 | { 23 | public: 24 | MCLFile(const std::string& file_name); 25 | 26 | void write(const std::vector& points, const std::vector& rings, const std::vector& particles, const std::array& tf, FLOAT_T x, FLOAT_T y, FLOAT_T z, FLOAT_T q_1, FLOAT_T q_2, FLOAT_T q_3, FLOAT_T q_4) const; 27 | 28 | void read(std::vector& points, std::vector& rings, std::vector& particles, std::array& tf, FLOAT_T& x, FLOAT_T& y, FLOAT_T& z, FLOAT_T& q_1, FLOAT_T& q_2, FLOAT_T& q_3, FLOAT_T& q_4) const; 29 | 30 | private: 31 | std::string name_; 32 | }; 33 | 34 | } // namespace tsdf_localization 35 | 36 | #endif -------------------------------------------------------------------------------- /include/tsdf_localization/util/runtime_evaluator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** 4 | * @author Marc Eisoldt 5 | * @author Steffen Hinderink 6 | */ 7 | 8 | #include // for the vector of measurement variables 9 | #include // for singleton pointer 10 | #include // for custom exception 11 | #include // for maximum value 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | /** 18 | * Define this token to enable time measurements in the whole project. 19 | * Every measurement must be enclosed in 20 | * #ifdef TIME_MEASUREMENT 21 | * ... 22 | * #endif 23 | */ 24 | #define TIME_MEASUREMENT 25 | 26 | using measurement_unit = std::chrono::microseconds; 27 | 28 | namespace tsdf_localization 29 | { 30 | 31 | /** 32 | * Helper struct for managing the different measurement variables for every measured task. 33 | */ 34 | struct EvaluationFormular 35 | { 36 | /** 37 | * Constructor for the evaluation formular. All measurement variables are initialized. 38 | * @param name Name of the task 39 | * @param be_recorded Should the measurements be recorded? 40 | */ 41 | EvaluationFormular(const std::string& name, bool be_recorded = false) : 42 | name(name), active(false), accumulate(0), 43 | count(0), last(0), sum(0), min(std::numeric_limits::max()), max(0), 44 | filter(100), recorded(be_recorded) 45 | { 46 | 47 | } 48 | /// Name of the task that is used as an identifier for the measurement variables 49 | std::string name; 50 | /// Flag that indicates whether the task is currently being measured (i.e. it is between start and stop) 51 | bool active; 52 | /// Accumulated runtime for the task. The runtime of the methods of the evaluator are not included, so that the evaluator is transparent. 53 | unsigned long long accumulate; 54 | 55 | /// Number of measurements 56 | unsigned int count; 57 | /// Last measured runtime 58 | unsigned long long last; 59 | /// Sum of all measured runtimes 60 | unsigned long long sum; 61 | /// Minimum of all measured runtimes 62 | unsigned long long min; 63 | /// Maximum of all measured runtimes 64 | unsigned long long max; 65 | /// Gives an Average of the last 100 measurements 66 | SlidingWindowFilter filter; 67 | /// Should the measurements be recorded for over time analysis? 68 | bool recorded; 69 | /// All measurements during the evaluation process 70 | std::vector recorded_data; 71 | }; 72 | 73 | /** 74 | * Custom exception that is thrown when the protocol of calling start and stop is not followed. 75 | */ 76 | struct RuntimeEvaluationException : public std::exception 77 | { 78 | /** 79 | * Returns the message, that gives information over what caused the exception to occur. 80 | * @return Exception message 81 | */ 82 | const char* what() const throw() 83 | { 84 | return "Runtime evaluation exception:\nStart was called for an already started measurement or stop was called before calling start!"; 85 | } 86 | }; 87 | 88 | /** 89 | * Encapsulates the runtime measurement for different tasks. 90 | * Different tasks can be measured at the same time and nested. They are distinguished by names. 91 | * The evaluation is transparent in the runtime measurement, i.e. the runtime of the evaluator itself is not measured. 92 | * This is achieved by measuring the time only in between every call of the functions and 93 | * updating the measurements of those tasks, whose measurements are currently active, accordingly. 94 | * The evaluator is implemented as a singleton (except for the public constructor for measurements in a different thread). 95 | */ 96 | class RuntimeEvaluator 97 | { 98 | 99 | public: 100 | 101 | /** 102 | * Returns the singleton instance. Creates a new one the first time. 103 | * @return Singleton instance 104 | */ 105 | static RuntimeEvaluator& get_instance(); 106 | 107 | /** 108 | * Default Destructor 109 | */ 110 | ~RuntimeEvaluator() = default; 111 | 112 | /** 113 | * Don't allow copies of the instance by copy constructor to ensure singleton property. 114 | */ 115 | RuntimeEvaluator(RuntimeEvaluator&) = delete; 116 | 117 | /// delete copy constructor 118 | RuntimeEvaluator(const RuntimeEvaluator&) = delete; 119 | 120 | /// delete move constructor 121 | RuntimeEvaluator(RuntimeEvaluator&&) = delete; 122 | 123 | /** 124 | * Don't allow copies of the instance by assignment operator to ensure singleton property. 125 | */ 126 | RuntimeEvaluator& operator=(RuntimeEvaluator&) = delete; 127 | 128 | /// delete move assignment operator 129 | RuntimeEvaluator& operator=(RuntimeEvaluator&&) = delete; 130 | 131 | /** 132 | * @brief Clear all running time measurements 133 | */ 134 | void clear(); 135 | 136 | 137 | /** 138 | * Starts a new measurent for a task. 139 | * @param task_name Name of the task, which will be shown in the printed overview 140 | * and is used as an identifier to find the right measurement variables 141 | * @param recorded Should the measurement be recorded for analysis over time? 142 | * @throws RuntimeEvaluationException if a measurement for that task was already started 143 | */ 144 | void start(const std::string& task_name, bool recorded = false); 145 | 146 | /** 147 | * Stops the measurement and updates the variables for a task. 148 | * This function has to be called after the start function was called for that task. 149 | * @param task_name Name of the task, which will be shown in the printed overview 150 | * and is used as an identifier to find the right measurement variables 151 | * @throws RuntimeEvaluationException if the measurement for that task has not been started yet 152 | */ 153 | void stop(const std::string& task_name); 154 | 155 | /** 156 | * Getter for the vector of the different measurement variables for every measured task 157 | * @return Vector of the measurement variables 158 | */ 159 | const std::vector get_forms(); 160 | 161 | /** 162 | * Returns a string that contains a table with the measurements for every measured task. 163 | * @return String that contains a table with all measurements 164 | */ 165 | std::string to_string(bool with_recorded = false); 166 | 167 | /** 168 | * Do not use this constructor except in a different thread! 169 | * This was previously private to ensure singleton property. 170 | */ 171 | RuntimeEvaluator(); 172 | 173 | private: 174 | 175 | /** 176 | * Pauses the time measurements. The time that has past since the last resume is accumulated for every currently measured task. 177 | * This method is called at the beginning of every public method. 178 | */ 179 | void pause(); 180 | 181 | /** 182 | * Resumes the time measurements by simply storing the current time so that it can be used at the next pause. 183 | * This method is called at the end of every public method. 184 | */ 185 | void resume(); 186 | 187 | /** 188 | * Try to find the formular with the given task name 189 | * 190 | * @param task_name Task name of the wanted formular 191 | * @return int if found, the index of the formular 192 | * else -1 193 | */ 194 | int find_formular(const std::string& task_name); 195 | 196 | /// Vector of the different measurement variables for every measured task 197 | std::vector forms_; 198 | 199 | /// Histogram of the total time 200 | std::vector histogram_; 201 | const int HIST_BUCKET_SIZE = 10; 202 | 203 | /// Excess time from the previous Scans. Used to calculate how many Scans are dropped 204 | double overhang_time_; 205 | 206 | /// Number of Scans that were dropped because previous Scans took too long 207 | int num_dropped_scans; 208 | 209 | /// Number of Scans that exceeded the 50ms limit 210 | int num_scans_over_50ms; 211 | 212 | /// Temporary variable for storing the start time of the current measurement 213 | std::chrono::_V2::system_clock::time_point start_; 214 | }; 215 | 216 | /** 217 | * Puts a by a given evaluator (using its to_string method) into a given output stream. 218 | * @param os Output stream in which the evaluator is put 219 | * @param evaluator Runtime evaluator that is put into the stream 220 | * @return Output stream with the evaluator 221 | */ 222 | std::ostream& operator<<(std::ostream& os, RuntimeEvaluator& evaluator); 223 | 224 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /include/tsdf_localization/util/time_util.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace fastsense 6 | { 7 | 8 | namespace util 9 | { 10 | 11 | using HighResTime = std::chrono::high_resolution_clock; 12 | using HighResTimePoint = HighResTime::time_point; 13 | 14 | namespace time 15 | { 16 | using secs_double = std::chrono::duration; 17 | } 18 | 19 | } 20 | 21 | } // namespace time 22 | -------------------------------------------------------------------------------- /include/tsdf_localization/util/tsdf.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** 4 | * @file tsdf.h 5 | * @author Marcel Flottmann 6 | * @date 2021-01-13 7 | */ 8 | 9 | #include 10 | 11 | struct TSDFValueHW 12 | { 13 | using ValueType = int16_t; 14 | using WeightType = int16_t; 15 | 16 | ValueType value; 17 | WeightType weight; 18 | }; 19 | 20 | class TSDFValue 21 | { 22 | public: 23 | using RawType = uint32_t; 24 | using ValueType = TSDFValueHW::ValueType; 25 | using WeightType = TSDFValueHW::WeightType; 26 | 27 | private: 28 | union 29 | { 30 | RawType raw; 31 | TSDFValueHW tsdf; 32 | } data; 33 | static_assert(sizeof(RawType) == sizeof(TSDFValueHW)); // raw and TSDF types must be of the same size 34 | 35 | public: 36 | 37 | TSDFValue(ValueType value, WeightType weight) 38 | { 39 | data.tsdf.value = value; 40 | data.tsdf.weight = weight; 41 | } 42 | 43 | explicit TSDFValue(RawType raw) 44 | { 45 | data.raw = raw; 46 | } 47 | 48 | TSDFValue() = default; 49 | TSDFValue(const TSDFValue&) = default; 50 | ~TSDFValue() = default; 51 | TSDFValue& operator=(const TSDFValue&) = default; 52 | 53 | bool operator==(const TSDFValue& rhs) const 54 | { 55 | return raw() == rhs.raw(); 56 | } 57 | 58 | RawType raw() const 59 | { 60 | return data.raw; 61 | } 62 | 63 | void raw(RawType val) 64 | { 65 | data.raw = val; 66 | } 67 | 68 | ValueType value() const 69 | { 70 | return data.tsdf.value; 71 | } 72 | 73 | void value(ValueType val) 74 | { 75 | data.tsdf.value = val; 76 | } 77 | 78 | WeightType weight() const 79 | { 80 | return data.tsdf.weight; 81 | } 82 | 83 | void weight(WeightType val) 84 | { 85 | data.tsdf.weight = val; 86 | } 87 | }; 88 | 89 | static_assert(sizeof(TSDFValue) == sizeof(TSDFValueHW)); // HW and SW types must be of the same size 90 | 91 | -------------------------------------------------------------------------------- /include/tsdf_localization/util/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace tsdf_localization 9 | { 10 | 11 | using FLOAT_T = float; 12 | 13 | constexpr FLOAT_T MAX_RANGE = 100.0; 14 | constexpr FLOAT_T INV_MAX_RANGE = 1.0 / MAX_RANGE; 15 | 16 | constexpr FLOAT_T A_HIT = 0.9; 17 | constexpr FLOAT_T A_RAND = 0.1; 18 | constexpr FLOAT_T A_MAX = 0.0; 19 | 20 | /** 21 | * @brief Extract the yaw (two dimensional rotation) from a given quaternion 22 | * 23 | * @param quaternion Quaternion where the angle should be extracted 24 | * 25 | * @return The yaw angle from the given quaternion 26 | */ 27 | FLOAT_T getYawFromQuaternion(const geometry_msgs::Quaternion& quaternion); 28 | 29 | /** 30 | * @brief Extract the roll from a given quaternion 31 | * 32 | * @param quaternion Quaternion where the angle should be extracted 33 | * 34 | * @return The roll angle from the given quaternion 35 | */ 36 | FLOAT_T getRollFromQuaternion(const geometry_msgs::Quaternion& quaternion); 37 | 38 | /** 39 | * @brief Extract the pitch from a given quaternion 40 | * 41 | * @param quaternion Quaternion where the angle should be extracted 42 | * 43 | * @return The pitch angle from the given quaternion 44 | */ 45 | FLOAT_T getPitchFromQuaternion(const geometry_msgs::Quaternion& quaternion); 46 | 47 | /** 48 | * @brief Transform a given point based into a coordinate system represented by a pose 49 | * 50 | * @param point Point which should be transformed 51 | * @param transform Representation of the target coordinate system 52 | * 53 | * @return transformed point 54 | */ 55 | geometry_msgs::Point transformPoint(const geometry_msgs::Point& point, const geometry_msgs::Pose& transform); 56 | 57 | /** 58 | * @brief Builds a tf transform from a map representation to a global frame 59 | * 60 | * @param map_meta Information about the map representation 61 | * 62 | * @return Transformation from the map representation to the global coordinate system 63 | */ 64 | geometry_msgs::TransformStamped getMapToWorldTF(const nav_msgs::MapMetaData& map_meta); 65 | 66 | /** 67 | * @brief Builds a tf transform from a global frame to a map representation 68 | * 69 | * @param map_meta Information about the map representation 70 | * 71 | * @return Transformation from the global coordinate system to the map representation 72 | */ 73 | geometry_msgs::TransformStamped getWorldToMapTF(const nav_msgs::MapMetaData& map_meta); 74 | 75 | void getAngleFromMat(const FLOAT_T mat[16], FLOAT_T& roll, FLOAT_T& pitch, FLOAT_T& yaw); 76 | 77 | } // namespace tsdf_localization 78 | 79 | #endif -------------------------------------------------------------------------------- /launch/experiments/cache_eval.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/experiments/deskew_ouster.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/experiments/ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/experiments/hilti_imu_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/experiments/num_particles_eval.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/int_mcl_2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/int_mcl_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/mcl_2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/mcl_3d_hilti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 25 | 26 | 27 | 35 | 36 | 37 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /launch/mcl_3d_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 24 | 25 | 26 | 34 | 35 | 36 | 44 | 45 | 46 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tsdf_localization 4 | 1.0.0 5 | The tsdf_localization package 6 | 7 | 8 | 9 | 10 | Marc Eisoldt 11 | Alexander Mock 12 | 13 | 14 | 15 | 16 | BSD-3 17 | 18 | 19 | Marc Eisoldt 20 | 21 | catkin 22 | geometry_msgs 23 | roscpp 24 | sensor_msgs 25 | visualization_msgs 26 | nav_msgs 27 | tf2 28 | tf2_sensor_msgs 29 | geometry_msgs 30 | roscpp 31 | sensor_msgs 32 | visualization_msgs 33 | nav_msgs 34 | tf2 35 | tf2_sensor_msgs 36 | geometry_msgs 37 | roscpp 38 | sensor_msgs 39 | visualization_msgs 40 | nav_msgs 41 | tf2 42 | tf2_sensor_msgs 43 | 44 | message_generation 45 | message_runtime 46 | 47 | diagnostic_updater 48 | dynamic_reconfigure 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /src/cuda/cuda_sum.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace tsdf_localization 6 | { 7 | 8 | __device__ void warpReduce(volatile FLOAT_T* sdata, unsigned int tid) 9 | { 10 | if(blockDim.x >= 64) sdata[tid] += sdata[tid + 32]; 11 | if(blockDim.x >= 32) sdata[tid] += sdata[tid + 16]; 12 | if(blockDim.x >= 16) sdata[tid] += sdata[tid + 8]; 13 | if(blockDim.x >= 8) sdata[tid] += sdata[tid + 4]; 14 | if(blockDim.x >= 4) sdata[tid] += sdata[tid + 2]; 15 | if(blockDim.x >= 2) sdata[tid] += sdata[tid + 1]; 16 | } 17 | 18 | __global__ void chunk_sums_kernel(const FLOAT_T* data, unsigned int data_size, unsigned int chunkSize, FLOAT_T* res) 19 | { 20 | 21 | __shared__ FLOAT_T sdata[1024]; 22 | 23 | const unsigned int tid = threadIdx.x; 24 | const unsigned int globId = chunkSize * blockIdx.x + threadIdx.x; 25 | const unsigned int rows = (chunkSize + blockDim.x - 1) / blockDim.x; 26 | 27 | 28 | sdata[tid] = 0.0; 29 | for(unsigned int i=0; i 32; s >>= 1) 44 | { 45 | if(tid < s) 46 | { 47 | sdata[tid] += sdata[tid + s]; 48 | } 49 | __syncthreads(); 50 | } 51 | 52 | if(tid < blockDim.x / 2 && tid < 32) 53 | { 54 | warpReduce(sdata, tid); 55 | } 56 | 57 | if(tid == 0) 58 | { 59 | res[blockIdx.x] = sdata[0]; 60 | } 61 | } 62 | 63 | void chunk_sums(const FLOAT_T* data_d, unsigned int data_size, FLOAT_T* res_d, unsigned int Nchunks, unsigned int chunkSize) 64 | { 65 | if(chunkSize >= 1024) 66 | { 67 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 68 | } 69 | else if(chunkSize >= 512) 70 | { 71 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 72 | } 73 | else if(chunkSize >= 256) 74 | { 75 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 76 | } 77 | else if(chunkSize >= 128) 78 | { 79 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 80 | } 81 | else if(chunkSize >= 64) 82 | { 83 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 84 | } 85 | else if(chunkSize >= 32) 86 | { 87 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 88 | } 89 | else if(chunkSize >= 16) 90 | { 91 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 92 | } 93 | else if(chunkSize >= 8) 94 | { 95 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 96 | } 97 | else if(chunkSize >= 4) 98 | { 99 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 100 | } 101 | else if(chunkSize >= 2) 102 | { 103 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 104 | } 105 | else if(chunkSize >= 1) 106 | { 107 | chunk_sums_kernel<<>>(data_d, data_size, chunkSize, res_d); 108 | } 109 | } 110 | 111 | FLOAT_T* sumBatched(const FLOAT_T* data, size_t data_size, size_t batchSize) 112 | { 113 | size_t Nchunks = std::ceil(data_size / static_cast(batchSize)); 114 | 115 | FLOAT_T* sums; 116 | cudaMalloc((FLOAT_T**) &sums, sizeof(FLOAT_T) * Nchunks); 117 | chunk_sums(data, data_size, sums, Nchunks, batchSize); 118 | 119 | return sums; 120 | } 121 | 122 | double logN(double base, double x) 123 | { 124 | return log(x) / log(base); 125 | } 126 | 127 | FLOAT_T* sumBatchedRecursive(const FLOAT_T* data, size_t data_size) 128 | { 129 | if (data_size <= 1024) 130 | { 131 | return sumBatched(data, data_size, data_size); 132 | } 133 | 134 | size_t Nchunks = std::ceil(data_size / static_cast(1024)); 135 | 136 | FLOAT_T* result = sumBatched(data, data_size, 1024); 137 | cudaDeviceSynchronize(); 138 | size_t remain = std::ceil(data_size / static_cast(1024)); 139 | 140 | FLOAT_T* tmp_data; 141 | cudaMalloc((FLOAT_T**) &tmp_data, remain * sizeof(FLOAT_T)); 142 | 143 | while (remain > 1) 144 | { 145 | Nchunks = std::ceil(remain / static_cast(1024)); 146 | 147 | chunk_sums(result, remain, tmp_data, Nchunks, 1024); 148 | cudaDeviceSynchronize(); 149 | remain = std::ceil(remain / static_cast(1024)); 150 | 151 | auto swap = tmp_data; 152 | tmp_data = result; 153 | result = swap; 154 | } 155 | 156 | cudaFree(tmp_data); 157 | 158 | return result; 159 | } 160 | 161 | FLOAT_T weightSum(const FLOAT_T* data, size_t data_size) 162 | { 163 | FLOAT_T weight_sum = 0; 164 | 165 | auto d_sums = sumBatchedRecursive(data, data_size); 166 | cudaCheck(); 167 | 168 | cudaMemcpy(&weight_sum, d_sums, sizeof(FLOAT_T), cudaMemcpyDeviceToHost); 169 | 170 | cudaFree(d_sums); 171 | 172 | return weight_sum; 173 | } 174 | 175 | 176 | } // namespace tsdf_localization 177 | -------------------------------------------------------------------------------- /src/cuda/cuda_test_eval.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //#include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | using namespace tsdf_localization; 12 | 13 | int main(int argc, char** argv) 14 | { 15 | if (argc != 3) 16 | { 17 | std::cout << "usage: " << argv[0] << " " << std::endl; 18 | return 0; 19 | } 20 | 21 | std::string mcl_name(argv[1]); 22 | std::string map_name(argv[2]); 23 | 24 | std::cout << "Read mcl data from file..." << std::endl; 25 | 26 | MCLFile file(mcl_name); 27 | 28 | std::vector points; 29 | std::vector rings; 30 | std::vector particles; 31 | std::array tf_matrix; 32 | 33 | FLOAT_T x, y, z, q_1, q_2, q_3, q_4; 34 | 35 | file.read(points, rings, particles, tf_matrix, x, y, z, q_1, q_2, q_3, q_4); 36 | 37 | std::unordered_set point_set; 38 | 39 | for (auto index = 0u; index < points.size(); ++index) 40 | { 41 | CudaPoint center = {static_cast(std::floor(points[index].x / 0.064) * 0.064 + 0.032), 42 | static_cast(std::floor(points[index].y / 0.064) * 0.064 + 0.032), 43 | static_cast(std::floor(points[index].z / 0.064) * 0.064 + 0.032)}; 44 | 45 | point_set.insert(SortClass(rings[index], index, center)); 46 | } 47 | 48 | std::vector reduced_points; 49 | reduced_points.resize(point_set.size()); 50 | std::copy(point_set.begin(), point_set.end(), reduced_points.begin()); 51 | 52 | std::vector ordered_points; 53 | ordered_points.reserve(reduced_points.size()); 54 | 55 | std::vector>> sort_points(16); 56 | 57 | struct 58 | { 59 | bool operator()(std::pair& a, std::pair& b) 60 | { 61 | return a.first < b.first; 62 | } 63 | } customComp; 64 | 65 | 66 | for (auto& curr_ring : sort_points) 67 | { 68 | curr_ring.reserve(1500); 69 | } 70 | 71 | for (const auto& point : reduced_points) 72 | { 73 | sort_points[point.ring_].push_back(std::make_pair(point.index_, point.point_)); 74 | } 75 | 76 | for (auto& curr_ring : sort_points) 77 | { 78 | std::sort(curr_ring.begin(), curr_ring.end(), customComp); 79 | } 80 | 81 | for (auto index = 0u; index < sort_points.size(); ++index) 82 | { 83 | auto& curr_ring = sort_points[index]; 84 | 85 | for (auto& point : curr_ring) 86 | { 87 | ordered_points.push_back(point.second); 88 | } 89 | } 90 | 91 | std::cout << "Original cloud size: " << ordered_points.size() << std::endl; 92 | std::cout << "Reduced cloud size: " << reduced_points.size() << std::endl; 93 | std::cout << "Reduction ratio: " << static_cast(reduced_points.size()) / ordered_points.size() * 100 << "%" << std::endl; 94 | 95 | std::cout << "Create map from file..." << std::endl; 96 | 97 | std::vector free_map; 98 | auto map = createTSDFMap, FLOAT_T, FLOAT_T>(map_name, free_map); 99 | 100 | std::cout << "\nStart evaluation of the cuda kernel..." << std::endl; 101 | 102 | std::cout << "Generate device context..." << std::endl; 103 | 104 | CudaEvaluator evaluator(*map, false); 105 | 106 | std::cout << "Execute kernel..." << std::endl; 107 | 108 | geometry_msgs::Pose center_pose; 109 | center_pose.position.x = x; 110 | center_pose.position.y = y; 111 | center_pose.position.z = z; 112 | 113 | center_pose.orientation.w = q_1; 114 | center_pose.orientation.x = q_2; 115 | center_pose.orientation.y = q_3; 116 | center_pose.orientation.z = q_4; 117 | 118 | evaluator.evaluate(particles, ordered_points, tf_matrix.data()); 119 | 120 | std::cout << "\nEvaluation finished!" << std::endl; 121 | 122 | return 0; 123 | } -------------------------------------------------------------------------------- /src/cuda/cuda_util.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifdef __CUDACC__ 4 | 5 | #include 6 | #include 7 | 8 | void cudaCheck() 9 | { 10 | auto code = cudaPeekAtLastError(); 11 | 12 | if (code != cudaSuccess) 13 | { 14 | std::cout << "CUDA error: " << cudaGetErrorString(code) << std::endl; 15 | throw std::runtime_error("CUDA error occured!"); 16 | } 17 | } 18 | 19 | #endif -------------------------------------------------------------------------------- /src/evaluation/model/likelihood_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace tsdf_localization 7 | { 8 | 9 | LikelihoodEvaluation::LikelihoodEvaluation(FLOAT_T range_max) : m_sum(0.0), m_range_max(range_max) 10 | { 11 | 12 | } 13 | 14 | void LikelihoodEvaluation::insertMeasurement(FLOAT_T measurement) 15 | { 16 | FLOAT_T eval = 0.0; 17 | 18 | //eval += 0.5 * exp(-measurement * measurement / 0.08); 19 | //eval += 0.5 / m_range_max; 20 | 21 | eval = exp(-(measurement * measurement) / SIGMA_QUAD / 2) / (sqrt(2 * SIGMA_QUAD * M_PI)); 22 | 23 | m_sum += eval * eval; 24 | } 25 | 26 | void LikelihoodEvaluation::insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range) 27 | { 28 | if(observed_range >= m_range_max) 29 | { 30 | return; 31 | } 32 | 33 | insertMeasurement(observed_range - simulated_range); 34 | } 35 | 36 | FLOAT_T LikelihoodEvaluation::evaluate() 37 | { 38 | return m_sum; 39 | } 40 | 41 | void LikelihoodEvaluation::reset() 42 | { 43 | m_sum = 0.0; 44 | } 45 | 46 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /src/evaluation/model/naiv_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace tsdf_localization 6 | { 7 | 8 | NaivEvaluation::NaivEvaluation() : m_error(0.0) 9 | { 10 | 11 | } 12 | 13 | void NaivEvaluation::insertMeasurement(FLOAT_T measurement) 14 | { 15 | m_error += fabs(measurement); 16 | } 17 | 18 | void NaivEvaluation::insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range) 19 | { 20 | //m_error += error; 21 | insertMeasurement(observed_range - simulated_range); 22 | } 23 | 24 | FLOAT_T NaivEvaluation::evaluate() 25 | { 26 | return 1 / (1 + m_error); 27 | } 28 | 29 | void NaivEvaluation::reset() 30 | { 31 | m_error = 0.0; 32 | } 33 | 34 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /src/evaluation/model/omp_likelihood_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace tsdf_localization 9 | { 10 | 11 | OMPLikelihoodEvaluation::OMPLikelihoodEvaluation(FLOAT_T range_max) : m_range_max_(range_max) 12 | { 13 | m_sum_.fill(0); 14 | } 15 | 16 | void OMPLikelihoodEvaluation::insertMeasurement(FLOAT_T measurement) 17 | { 18 | FLOAT_T eval = 0.0; 19 | 20 | //eval += 0.5 * exp(-measurement * measurement / 0.08); 21 | //eval += 0.5 / m_range_max_; 22 | 23 | m_sum_[omp_get_thread_num()] += eval * eval * eval; 24 | } 25 | 26 | void OMPLikelihoodEvaluation::insertMeasurement(FLOAT_T observed_range, FLOAT_T simulated_range) 27 | { 28 | if(observed_range >= m_range_max_) 29 | { 30 | return; 31 | } 32 | 33 | insertMeasurement(observed_range - simulated_range); 34 | } 35 | 36 | FLOAT_T OMPLikelihoodEvaluation::evaluate() 37 | { 38 | return m_sum_[omp_get_thread_num()]; 39 | } 40 | 41 | void OMPLikelihoodEvaluation::reset() 42 | { 43 | m_sum_[omp_get_thread_num()] = 0; 44 | } 45 | 46 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /src/num_particles_eval.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file num_particles_eval.cpp 3 | * @author Marc Eisoldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Evaluate the runtime of the implemented evaluation model based an sensor snapshot depending on the number of particles 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | using namespace tsdf_localization; 26 | 27 | std::shared_ptr nh_p_; 28 | 29 | bool use_cuda = true; 30 | bool per_point = false; 31 | 32 | FLOAT_T sigma_trans = 0.5; 33 | FLOAT_T sigma_rot = 1.5; 34 | 35 | int repeat = 10; 36 | int inc = 1000; 37 | int num_particles = 100000; 38 | 39 | int main(int argc, char** argv) 40 | { 41 | ros::init(argc, argv, "num_particles_eval"); 42 | 43 | if (argc != 3) 44 | { 45 | std::cout << "usage: " << argv[0] << " " << std::endl; 46 | return 0; 47 | } 48 | 49 | static auto& eval = RuntimeEvaluator::get_instance(); 50 | 51 | ros::NodeHandle n; 52 | nh_p_.reset(new ros::NodeHandle("~")); 53 | 54 | nh_p_->getParam("num_particles", num_particles); 55 | nh_p_->getParam("inc", inc); 56 | nh_p_->getParam("repeat", repeat); 57 | nh_p_->getParam("sigma_trans", sigma_trans); 58 | nh_p_->getParam("sigma_rot", sigma_rot); 59 | nh_p_->getParam("use_cuda", use_cuda); 60 | nh_p_->getParam("per_point", per_point); 61 | 62 | 63 | std::stringstream param_ss; 64 | 65 | param_ss << "Used parameters:\n" 66 | << "\tnum_particles: " << num_particles << std::endl 67 | << "\tinc: " << inc << std::endl 68 | << "\trepeat: " << repeat << std::endl 69 | << "\tsigma_trans: " << sigma_trans << std::endl 70 | << "\tsigma_rot: " << sigma_rot << std::endl 71 | << "\tuse_cuda: " << use_cuda << std::endl; 72 | 73 | std::cout << param_ss.str() << std::endl; 74 | 75 | std::string mcl_name(argv[1]); 76 | std::string map_name(argv[2]); 77 | 78 | // if (!boost::filesystem::exists(mcl_name)) 79 | // { 80 | // std::cout << "mcl file \"" << mcl_name << "\" does not exist" << std::endl; 81 | // return 1; 82 | // } 83 | 84 | // if (!boost::filesystem::exists(map_name)) 85 | // { 86 | // std::cout << "map file \"" << map_name << "\" does not exist" << std::endl; 87 | // return 1; 88 | // } 89 | 90 | std::cout << "Read mcl data from file..." << std::endl; 91 | 92 | MCLFile file(mcl_name); 93 | 94 | std::vector points; 95 | std::vector rings; 96 | std::vector particles; 97 | std::array tf_matrix; 98 | 99 | FLOAT_T x, y, z, q_1, q_2, q_3, q_4; 100 | 101 | file.read(points, rings, particles, tf_matrix, x, y, z, q_1, q_2, q_3, q_4); 102 | 103 | /* std::multimap point_map; 104 | 105 | for (auto index = 0u; index < points.size(); ++index) 106 | { 107 | point_map.insert(std::pair(rings[index], {points[index].x, points[index].y, points[index].z})); 108 | } 109 | 110 | std::vector ordered_points; 111 | ordered_points.reserve(points.size()); 112 | 113 | 114 | for (const auto& entry : point_map) 115 | { 116 | ordered_points.push_back(entry.second); 117 | } 118 | 119 | std::unordered_set point_set; 120 | 121 | for (const auto& point : ordered_points) 122 | { 123 | CudaPoint center = {static_cast(std::floor(point.x / 0.064) * 0.064 + 0.032), 124 | static_cast(std::floor(point.y / 0.064) * 0.064 + 0.032), 125 | static_cast(std::floor(point.z / 0.064) * 0.064 + 0.032)}; 126 | 127 | point_set.insert(center); 128 | } 129 | 130 | std::vector reduced_points; 131 | reduced_points.resize(point_set.size()); 132 | std::copy(point_set.begin(), point_set.end(), reduced_points.begin());*/ 133 | 134 | std::unordered_set point_set; 135 | 136 | //int index = 0; 137 | 138 | for (auto index = 0u; index < points.size(); ++index) 139 | { 140 | CudaPoint center = {static_cast(std::floor(points[index].x / 0.064) * 0.064 + 0.032), 141 | static_cast(std::floor(points[index].y / 0.064) * 0.064 + 0.032), 142 | static_cast(std::floor(points[index].z / 0.064) * 0.064 + 0.032)}; 143 | 144 | //point_set.insert(std::make_pair(iter_ring[0], center)); 145 | point_set.insert(SortClass(rings[index], index, center)); 146 | 147 | //++index; 148 | } 149 | 150 | std::vector reduced_points; 151 | reduced_points.resize(point_set.size()); 152 | std::copy(point_set.begin(), point_set.end(), reduced_points.begin()); 153 | 154 | std::vector ordered_points; 155 | ordered_points.reserve(reduced_points.size()); 156 | 157 | std::vector>> sort_points(16); 158 | 159 | struct 160 | { 161 | bool operator()(std::pair& a, std::pair& b) 162 | { 163 | return a.first < b.first; 164 | } 165 | } customComp; 166 | 167 | 168 | for (auto& curr_ring : sort_points) 169 | { 170 | curr_ring.reserve(1500); 171 | } 172 | 173 | for (const auto& point : reduced_points) 174 | { 175 | sort_points[point.ring_].push_back(std::make_pair(point.index_, point.point_)); 176 | } 177 | 178 | for (auto& curr_ring : sort_points) 179 | { 180 | std::sort(curr_ring.begin(), curr_ring.end(), customComp); 181 | } 182 | 183 | for (auto index = 0u; index < sort_points.size(); ++index) 184 | { 185 | auto& curr_ring = sort_points[index]; 186 | 187 | for (auto& point : curr_ring) 188 | { 189 | ordered_points.push_back(point.second); 190 | } 191 | } 192 | 193 | std::cout << "Original cloud size: " << ordered_points.size() << std::endl; 194 | std::cout << "Reduced cloud size: " << reduced_points.size() << std::endl; 195 | std::cout << "Reduction ratio: " << static_cast(reduced_points.size()) / ordered_points.size() * 100 << "%" << std::endl; 196 | 197 | std::cout << "Create map from file..." << std::endl; 198 | 199 | std::vector free_map; 200 | auto map = createTSDFMap, FLOAT_T, FLOAT_T>(map_name, free_map); 201 | 202 | std::cout << "\nStart evaluation of the cuda kernel..." << std::endl; 203 | 204 | std::cout << "Generate device context..." << std::endl; 205 | 206 | TSDFEvaluator evaluator(map, per_point); 207 | 208 | std::cout << "Execute kernel..." << std::endl; 209 | 210 | ParticleCloud cloud; 211 | 212 | geometry_msgs::Pose center_pose; 213 | center_pose.position.x = x; 214 | center_pose.position.y = y; 215 | center_pose.position.z = z; 216 | 217 | center_pose.orientation.w = q_1; 218 | center_pose.orientation.x = q_2; 219 | center_pose.orientation.y = q_3; 220 | center_pose.orientation.z = q_4; 221 | 222 | auto start = std::chrono::high_resolution_clock::now(); 223 | auto stop = start; 224 | 225 | long measure = 0; 226 | 227 | std::vector numbers_of_particles; 228 | std::vector measurements; 229 | 230 | std::cout << "Warmup..." << std::endl; 231 | 232 | cloud.initialize(center_pose, num_particles, sigma_trans, sigma_trans, 0.1, sigma_rot, sigma_rot, sigma_rot); 233 | evaluator.evaluate(cloud.particles(), ordered_points, tf_matrix.data(), use_cuda); 234 | 235 | std::cout << "Evaluation..." << std::endl; 236 | 237 | for (auto step = inc; step <= num_particles; step += inc) 238 | { 239 | std::cout << "\r\tnum particles: " << step << " / " << num_particles; 240 | std::cout.flush(); 241 | 242 | cloud.initialize(center_pose, step, sigma_trans, sigma_trans, 0.1, sigma_rot, sigma_rot, sigma_rot); 243 | 244 | measure = 0; 245 | 246 | for (auto index = 0; index < repeat; ++index) 247 | { 248 | start = std::chrono::high_resolution_clock::now(); 249 | evaluator.evaluate(cloud.particles(), ordered_points, tf_matrix.data(), use_cuda); 250 | stop = std::chrono::high_resolution_clock::now(); 251 | 252 | measure += std::chrono::duration_cast(stop - start).count(); 253 | } 254 | 255 | numbers_of_particles.push_back(step); 256 | measurements.push_back(measure / repeat / 1000); 257 | } 258 | 259 | std::cout << std::endl; 260 | 261 | std::ostringstream filename; 262 | auto now = std::chrono::system_clock::now(); 263 | auto t = std::chrono::system_clock::to_time_t(now); 264 | 265 | filename << "mcl_num_particles_times_" << std::put_time(std::localtime(&t), "%Y-%m-%d-%H-%M-%S") << ".log"; 266 | 267 | std::ofstream time_output(filename.str()); 268 | 269 | std::stringstream eval_string; 270 | 271 | //time_output << RuntimeEvaluator::get_instance().to_string(true); 272 | 273 | eval_string << "| num. particles | runtime [ms] |\n"; 274 | 275 | for (auto index = 0; index < measurements.size(); ++index) 276 | { 277 | eval_string << numbers_of_particles[index] << " " << measurements[index] << "\n"; 278 | } 279 | 280 | std::cout << eval_string.str(); 281 | 282 | time_output << param_ss.str() << "\n" << eval_string.str(); 283 | 284 | time_output.close(); 285 | 286 | std::cout << "\nEvaluation finished!" << std::endl; 287 | return 0; 288 | } -------------------------------------------------------------------------------- /src/resampling/wheel_resampler.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace tsdf_localization 4 | { 5 | 6 | void WheelResampler::resample(ParticleCloud& particle_cloud) 7 | { 8 | // Initializing an uniform distribution for choosing a new particle 9 | static std::uniform_real_distribution<> uniform_distribution(0.0, 1.0); 10 | 11 | auto tmp_particle_cloud = particle_cloud; 12 | 13 | for (auto particle_index = 0u; particle_index < particle_cloud.size(); ++particle_index) 14 | { 15 | FLOAT_T random_value = uniform_distribution(*m_generator_ptr); 16 | FLOAT_T weight_sum = 0.0; 17 | FLOAT_T current_weight; 18 | 19 | for(auto index = 0u; index < particle_cloud.size(); index++) 20 | { 21 | current_weight = tmp_particle_cloud[index].second; 22 | weight_sum += current_weight; 23 | 24 | if(random_value <= weight_sum) 25 | { 26 | particle_cloud[particle_index] = tmp_particle_cloud[index]; 27 | break; 28 | } 29 | } 30 | 31 | //particle_cloud[particle_index] = tmp_particle_cloud[particle_cloud.size() - 1]; 32 | } 33 | 34 | } 35 | 36 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /src/snap_shot_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file snap_shot_mode.cpp 3 | * @author Marc Eisoldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Node to record a snap shot of all needed data during the Monte Carlo localization to be processed offline 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #include "ros/ros.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | #include 50 | 51 | #include 52 | #include 53 | 54 | #include 55 | 56 | #include 57 | #include 58 | 59 | #include 60 | 61 | using namespace tsdf_localization; 62 | 63 | std::vector free_map_; 64 | 65 | // Reference to the node 66 | std::shared_ptr nh_p_; 67 | 68 | geometry_msgs::Pose initial_pose_; 69 | bool pose_initialized_ = false; 70 | 71 | unsigned int number_particles_ = 800; 72 | ParticleCloud particle_cloud_; 73 | 74 | FLOAT_T init_sigma_x_; 75 | FLOAT_T init_sigma_y_; 76 | FLOAT_T init_sigma_z_; 77 | 78 | FLOAT_T init_sigma_roll_; 79 | FLOAT_T init_sigma_pitch_; 80 | FLOAT_T init_sigma_yaw_; 81 | 82 | int evaluation_model_; 83 | 84 | std::string robot_frame_ = "base_footprint"; 85 | std::string scan_frame_ = "velodyne"; 86 | std::string odom_frame_ = "odom_combined"; 87 | std::string map_frame_ = "map"; 88 | 89 | std::string save_dir = "mcl_snapshots/"; 90 | 91 | void responseCallback(tsdf_localization::MCLConfig& config, uint32_t level) 92 | { 93 | number_particles_ = config.number_of_particles; 94 | 95 | init_sigma_x_ = config.init_sigma_x; 96 | init_sigma_y_ = config.init_sigma_y; 97 | init_sigma_z_ = config.init_sigma_z; 98 | init_sigma_roll_ = config.init_sigma_roll * M_PI / 180.0; 99 | init_sigma_pitch_ = config.init_sigma_pitch * M_PI / 180.0; 100 | init_sigma_yaw_ = config.init_sigma_yaw * M_PI / 180.0; 101 | 102 | evaluation_model_ = config.evaluation_model; 103 | 104 | if(particle_cloud_.isInitialized()) 105 | { 106 | particle_cloud_.resize(number_particles_); 107 | } 108 | } 109 | 110 | void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& pose_with_covariance) 111 | { 112 | initial_pose_ = pose_with_covariance.pose.pose; 113 | 114 | initial_pose_.position.z = 0; //= -0.5; 115 | pose_initialized_ = true; 116 | 117 | ROS_INFO_STREAM("Initial pose received!"); 118 | } 119 | 120 | void scanOdomCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud, const nav_msgs::Odometry::ConstPtr& odom) 121 | { 122 | static tf2_ros::TransformBroadcaster broadcaster; 123 | static tf2_ros::Buffer tf_buffer; 124 | static tf2_ros::TransformListener tf_listener(tf_buffer); 125 | 126 | geometry_msgs::TransformStamped scan_to_base; 127 | 128 | /*** A priori ***/ 129 | 130 | // Init particles 131 | if(pose_initialized_) 132 | { 133 | //particle_cloud_.initialize(initial_pose_, number_particles_, init_sigma_x_, init_sigma_y_, init_sigma_z_, init_sigma_roll_, init_sigma_pitch_, init_sigma_yaw_); 134 | particle_cloud_.initialize(number_particles_, free_map_, initial_pose_); 135 | pose_initialized_ = false; 136 | 137 | if(!boost::filesystem::exists(save_dir)) 138 | { 139 | boost::filesystem::create_directory(save_dir); 140 | std::cout << "Create directory \"" << save_dir << '\"' << std::endl; 141 | } 142 | 143 | std::array tf_matrix; 144 | 145 | try 146 | { 147 | scan_to_base = tf_buffer.lookupTransform(robot_frame_, scan_frame_, cloud->header.stamp, ros::Duration(0.5)); 148 | } 149 | catch (tf2::TransformException& e) 150 | { 151 | ROS_ERROR_STREAM("Couldn't transform robot frame to scan frame: " << e.what()); 152 | throw; 153 | } 154 | 155 | tf2::Transform tf_scan_to_base; 156 | tf2::convert(scan_to_base.transform, tf_scan_to_base); 157 | 158 | tf_matrix[0] = tf_scan_to_base.getBasis()[0][0]; 159 | tf_matrix[1] = tf_scan_to_base.getBasis()[0][1]; 160 | tf_matrix[2] = tf_scan_to_base.getBasis()[0][2]; 161 | tf_matrix[3] = tf_scan_to_base.getOrigin().getX(); 162 | 163 | tf_matrix[4] = tf_scan_to_base.getBasis()[1][0]; 164 | tf_matrix[5] = tf_scan_to_base.getBasis()[1][1]; 165 | tf_matrix[6] = tf_scan_to_base.getBasis()[1][2]; 166 | tf_matrix[7] = tf_scan_to_base.getOrigin().getY(); 167 | 168 | tf_matrix[8] = tf_scan_to_base.getBasis()[2][0]; 169 | tf_matrix[9] = tf_scan_to_base.getBasis()[2][1]; 170 | tf_matrix[10] = tf_scan_to_base.getBasis()[2][2]; 171 | tf_matrix[11] = tf_scan_to_base.getOrigin().getZ(); 172 | 173 | std::vector points; 174 | points.reserve(cloud->width); 175 | std::vector rings; 176 | rings.reserve(cloud->width); 177 | CudaPoint point; 178 | 179 | sensor_msgs::PointCloud2ConstIterator iter_x(*cloud, "x"); 180 | sensor_msgs::PointCloud2ConstIterator iter_ring(*cloud, "ring"); 181 | 182 | for (; iter_x != iter_x.end(); ++iter_x) 183 | { 184 | point.x = iter_x[0]; 185 | point.y = iter_x[1]; 186 | point.z = iter_x[2]; 187 | 188 | points.push_back(point); 189 | rings.push_back(iter_ring[0]); 190 | 191 | ++iter_ring; 192 | } 193 | 194 | std::cout << "Save current mcl data..." << std::endl; 195 | MCLFile file(save_dir + "mcl_" + std::to_string(cloud->header.stamp.toNSec()) + ".mcl"); 196 | file.write(points, rings, particle_cloud_.particles(), tf_matrix, initial_pose_.position.x, initial_pose_.position.y, initial_pose_.position.z, initial_pose_.orientation.w, initial_pose_.orientation.x, initial_pose_.orientation.y, initial_pose_.orientation.z); 197 | std::cout << "Current mcl data saved" << std::endl; 198 | } 199 | } 200 | 201 | int main(int argc, char** argv) 202 | { 203 | if (argc != 2) 204 | { 205 | std::cout << "usage: " << argv[0] << " " << std::endl; 206 | return 0; 207 | } 208 | 209 | ros::init(argc, argv, "snap_shot_node"); 210 | ros::NodeHandle n; 211 | nh_p_.reset(new ros::NodeHandle("~")); 212 | 213 | auto map = createTSDFMap, FLOAT_T, FLOAT_T>(argv[1], free_map_); 214 | std::cout << "Loaded free map" << std::endl; 215 | 216 | dynamic_reconfigure::Server server; 217 | dynamic_reconfigure::Server::CallbackType callbackType; 218 | 219 | callbackType = boost::bind(&responseCallback, _1, _2); 220 | server.setCallback(callbackType); 221 | 222 | // Meaningfull initialization 223 | initial_pose_.orientation.w = 1.0; 224 | 225 | // Initialize subscribers 226 | ros::Subscriber sub_initial_pose = n.subscribe("initialpose", 1, initialPoseCallback); 227 | 228 | // Initialize synchronized subscriber for the scan and odom topic 229 | message_filters::Subscriber scan2_sub(n, "velodyne_points", 1); 230 | 231 | message_filters::Subscriber odom_sub(n, "odom", 1); 232 | 233 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 234 | 235 | message_filters::Synchronizer sync(MySyncPolicy(10), scan2_sub, odom_sub); 236 | sync.registerCallback(boost::bind(&scanOdomCallback, _1, _2)) ; 237 | 238 | std::cout << "Ready to save mcl data" << std::endl; 239 | 240 | ros::spin(); 241 | } -------------------------------------------------------------------------------- /src/snap_vis_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file snap_vis.cpp 3 | * @author Marc Eisoldt (meisoldt@uni-osnabrueck.de) 4 | * 5 | * @brief Node to visualize a MCL snapshot that consist of the current scan, particle cloud, initial pose and estimated transformation of the robot 6 | * 7 | * @version 0.1 8 | * @date 2022-06-18 9 | * 10 | * @copyright Copyright (c) 2022 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | 28 | using namespace tsdf_localization; 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "snap_vis_node"); 33 | 34 | ros::NodeHandle nh("~"); 35 | 36 | if (argc != 2) 37 | { 38 | std::cout << "usage: " << argv[0] << " " << std::endl; 39 | return 0; 40 | } 41 | 42 | std::string mcl_name(argv[1]); 43 | 44 | if (!boost::filesystem::exists(mcl_name)) 45 | { 46 | std::cout << "mcl file \"" << mcl_name << "\" does not exist" << std::endl; 47 | return 1; 48 | } 49 | 50 | std::cout << "Read mcl data from file..." << std::endl; 51 | 52 | MCLFile file(mcl_name); 53 | 54 | std::vector points; 55 | std::vector rings; 56 | std::vector particles; 57 | std::array tf_matrix; 58 | 59 | FLOAT_T x, y, z, q_1, q_2, q_3, q_4; 60 | 61 | file.read(points, rings, particles, tf_matrix, x, y, z, q_1, q_2, q_3, q_4); 62 | 63 | std::unordered_set point_set; 64 | 65 | for (const auto& point : points) 66 | { 67 | CudaPoint center = {static_cast(std::floor(point.x / 0.064) * 0.064 + 0.032), 68 | static_cast(std::floor(point.y / 0.064) * 0.064 + 0.032), 69 | static_cast(std::floor(point.z / 0.064) * 0.064 + 0.032)}; 70 | 71 | point_set.insert(center); 72 | } 73 | 74 | std::vector reduced_points; 75 | reduced_points.resize(point_set.size()); 76 | std::copy(point_set.begin(), point_set.end(), reduced_points.begin()); 77 | 78 | 79 | std::cout << "Convert data to ROS..." << std::endl; 80 | 81 | sensor_msgs::PointCloud cloud; 82 | cloud.header.frame_id = "map"; 83 | cloud.points.resize(reduced_points.size()); 84 | 85 | for (auto index = 0u; index < reduced_points.size(); ++index) 86 | { 87 | cloud.points[index].x = reduced_points[index].x; 88 | cloud.points[index].y = reduced_points[index].y; 89 | cloud.points[index].z = reduced_points[index].z; 90 | } 91 | 92 | cloud.points.resize(6000); 93 | 94 | geometry_msgs::Pose pose; 95 | geometry_msgs::PoseArray pose_array; 96 | pose_array.header.frame_id = "map"; 97 | pose_array.poses.resize(particles.size()); 98 | 99 | for (auto index = 0u; index < particles.size(); ++index) 100 | { 101 | pose.position.x = particles[index].first[0]; 102 | pose.position.y = particles[index].first[1]; 103 | pose.position.z = particles[index].first[2]; 104 | 105 | tf2::Quaternion tf_quaternion; 106 | tf_quaternion.setRPY(particles[index].first[3], particles[index].first[4], particles[index].first[5]); 107 | 108 | tf2::convert(tf_quaternion, pose.orientation); 109 | 110 | pose_array.poses[index] = pose; 111 | } 112 | 113 | geometry_msgs::PoseStamped init_pose_stamped_; 114 | init_pose_stamped_.header.frame_id = "map"; 115 | 116 | init_pose_stamped_.pose.position.x = x; 117 | init_pose_stamped_.pose.position.y = y; 118 | init_pose_stamped_.pose.position.z = z; 119 | init_pose_stamped_.pose.orientation.w = q_1; 120 | init_pose_stamped_.pose.orientation.x = q_2; 121 | init_pose_stamped_.pose.orientation.y = q_3; 122 | init_pose_stamped_.pose.orientation.z = q_4; 123 | 124 | geometry_msgs::PoseStamped tf_pose_stamped_; 125 | tf_pose_stamped_.header.frame_id = "map"; 126 | 127 | tf_pose_stamped_.pose.position.x = tf_matrix[3]; 128 | tf_pose_stamped_.pose.position.y = tf_matrix[7]; 129 | tf_pose_stamped_.pose.position.z = tf_matrix[11]; 130 | 131 | tf2::Matrix3x3 rot_mat; 132 | rot_mat[0][0] = tf_matrix[0]; 133 | rot_mat[0][1] = tf_matrix[1]; 134 | rot_mat[0][2] = tf_matrix[2]; 135 | rot_mat[1][0] = tf_matrix[4]; 136 | rot_mat[1][1] = tf_matrix[5]; 137 | rot_mat[1][2] = tf_matrix[6]; 138 | rot_mat[2][0] = tf_matrix[8]; 139 | rot_mat[2][1] = tf_matrix[9]; 140 | rot_mat[2][2] = tf_matrix[10]; 141 | 142 | tf2::Quaternion tf_quaternion; 143 | rot_mat.getRotation(tf_quaternion); 144 | tf2::convert(tf_quaternion, tf_pose_stamped_.pose.orientation); 145 | 146 | std::cout << "Publish data..." << std::endl; 147 | 148 | ros::Publisher cloud_pub = nh.advertise("read_cloud", 1); 149 | ros::Publisher particle_pub = nh.advertise("read_particles", 1); 150 | ros::Publisher tf_pub = nh.advertise("read_tf", 1); 151 | ros::Publisher init_pub = nh.advertise("read_init", 1); 152 | 153 | ros::Rate loop_rate(50); 154 | 155 | while (ros::ok()) 156 | { 157 | cloud.header.stamp = ros::Time::now(); 158 | cloud_pub.publish(cloud); 159 | 160 | pose_array.header.stamp = ros::Time::now(); 161 | particle_pub.publish(pose_array); 162 | 163 | tf_pose_stamped_.header.stamp = ros::Time::now(); 164 | tf_pub.publish(tf_pose_stamped_); 165 | 166 | init_pose_stamped_.header.stamp = ros::Time::now(); 167 | init_pub.publish(init_pose_stamped_); 168 | 169 | ros::spinOnce(); 170 | loop_rate.sleep(); 171 | } 172 | 173 | return 0; 174 | } -------------------------------------------------------------------------------- /src/util/global_map.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Steffen Hinderink 3 | * @author Juri Vana 4 | * @author Malte Hillmann 5 | */ 6 | 7 | #include 8 | 9 | #include 10 | 11 | GlobalMap::GlobalMap(std::string name, TSDFValue::ValueType initial_tsdf_value, TSDFValue::WeightType initial_weight) 12 | : file_{name, HighFive::File::OpenOrCreate | HighFive::File::Truncate}, // Truncate clears already existing file 13 | initial_tsdf_value_{initial_tsdf_value, initial_weight}, 14 | active_chunks_{}, 15 | num_poses_{0} 16 | { 17 | if (!file_.exist("/map")) 18 | { 19 | file_.createGroup("/map"); 20 | } 21 | if (!file_.exist("/poses")) 22 | { 23 | file_.createGroup("/poses"); 24 | } 25 | } 26 | 27 | std::string GlobalMap::tag_from_chunk_pos(const Vector3i& pos) 28 | { 29 | std::stringstream ss; 30 | ss << pos.x() << "_" << pos.y() << "_" << pos.z(); 31 | return ss.str(); 32 | } 33 | 34 | int GlobalMap::index_from_pos(Vector3i pos, const Vector3i& chunkPos) 35 | { 36 | pos -= chunkPos * CHUNK_SIZE; 37 | return (pos.x() * CHUNK_SIZE * CHUNK_SIZE + pos.y() * CHUNK_SIZE + pos.z()); 38 | } 39 | 40 | std::vector& GlobalMap::activate_chunk(const Vector3i& chunkPos) 41 | { 42 | int index = -1; 43 | int n = active_chunks_.size(); 44 | for (int i = 0; i < n; i++) 45 | { 46 | if (active_chunks_[i].pos == chunkPos) 47 | { 48 | // chunk is already active 49 | index = i; 50 | } 51 | } 52 | if (index == -1) 53 | { 54 | // chunk is not already active 55 | ActiveChunk newChunk; 56 | newChunk.pos = chunkPos; 57 | newChunk.age = 0; 58 | 59 | HighFive::Group g = file_.getGroup("/map"); 60 | auto tag = tag_from_chunk_pos(chunkPos); 61 | if (g.exist(tag)) 62 | { 63 | // read chunk from file 64 | HighFive::DataSet d = g.getDataSet(tag); 65 | d.read(newChunk.data); 66 | } 67 | else 68 | { 69 | // create new chunk 70 | newChunk.data = std::vector(CHUNK_SIZE * CHUNK_SIZE * CHUNK_SIZE, initial_tsdf_value_.raw()); //std::vector(TOTAL_SIZE); 71 | } 72 | // put new chunk into active_chunks_ 73 | if (n < NUM_CHUNKS) 74 | { 75 | // there is still room for active chunks 76 | index = n; 77 | newChunk.age = n; // temporarily assign oldest age so that all other ages get incremented 78 | active_chunks_.push_back(newChunk); 79 | } 80 | else 81 | { 82 | // write oldest chunk into file 83 | int max = -1; 84 | for (int i = 0; i < n; i++) 85 | { 86 | if (active_chunks_[i].age > max) 87 | { 88 | max = active_chunks_[i].age; 89 | index = i; 90 | } 91 | } 92 | auto tag = tag_from_chunk_pos(active_chunks_[index].pos); 93 | 94 | if (g.exist(tag)) 95 | { 96 | auto d = g.getDataSet(tag); 97 | d.write(active_chunks_[index].data); 98 | } 99 | else 100 | { 101 | g.createDataSet(tag, active_chunks_[index].data); 102 | } 103 | // overwrite with new chunk 104 | active_chunks_[index] = newChunk; 105 | } 106 | } 107 | // update ages 108 | int age = active_chunks_[index].age; 109 | for (auto& chunk : active_chunks_) 110 | { 111 | if (chunk.age < age) 112 | { 113 | chunk.age++; 114 | } 115 | } 116 | active_chunks_[index].age = 0; 117 | return active_chunks_[index].data; 118 | } 119 | 120 | static inline Vector3i floor_shift(const Vector3i& a, int shift) 121 | { 122 | return Vector3i( 123 | a[0] >> shift, 124 | a[1] >> shift, 125 | a[2] >> shift 126 | ); 127 | } 128 | 129 | TSDFValue GlobalMap::get_value(const Vector3i& pos) 130 | { 131 | Vector3i chunkPos = floor_shift(pos, CHUNK_SHIFT); 132 | const auto& chunk = activate_chunk(chunkPos); 133 | int index = index_from_pos(pos, chunkPos); 134 | return TSDFValue(chunk[index]); 135 | } 136 | 137 | void GlobalMap::set_value(const Vector3i& pos, const TSDFValue& value) 138 | { 139 | Vector3i chunkPos = floor_shift(pos, CHUNK_SHIFT); 140 | auto& chunk = activate_chunk(chunkPos); 141 | int index = index_from_pos(pos, chunkPos); 142 | chunk[index] = value.raw(); 143 | } 144 | 145 | void GlobalMap::savePose(float x, float y, float z, float roll, float pitch, float yaw) 146 | { 147 | HighFive::Group g = file_.getGroup("/poses"); 148 | std::vector pose {x, y, z, roll, pitch, yaw}; 149 | std::stringstream ss; 150 | ss << num_poses_; 151 | g.createDataSet(ss.str(), pose); 152 | num_poses_++; 153 | } 154 | 155 | void GlobalMap::write_back() 156 | { 157 | HighFive::Group g = file_.getGroup("/map"); 158 | for (auto& chunk : active_chunks_) 159 | { 160 | auto tag = tag_from_chunk_pos(chunk.pos); 161 | 162 | if (g.exist(tag)) 163 | { 164 | auto d = g.getDataSet(tag); 165 | d.write(chunk.data); 166 | } 167 | else 168 | { 169 | g.createDataSet(tag, chunk.data); 170 | } 171 | } 172 | file_.flush(); 173 | } 174 | -------------------------------------------------------------------------------- /src/util/imu_accumulator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace tsdf_localization 4 | { 5 | 6 | void bound_angle(FLOAT_T& angle) 7 | { 8 | if (angle >= M_PI) 9 | { 10 | angle = -M_PI + (angle - M_PI); 11 | } 12 | else if (angle < -M_PI) 13 | { 14 | angle = M_PI - (angle - M_PI); 15 | } 16 | } 17 | 18 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /src/util/mcl_file.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace tsdf_localization 7 | { 8 | 9 | MCLFile::MCLFile(const std::string& file_name) : name_(file_name) 10 | { 11 | 12 | } 13 | 14 | void MCLFile::write(const std::vector& points, const std::vector& rings, const std::vector& particles, const std::array& tf, FLOAT_T x, FLOAT_T y, FLOAT_T z, FLOAT_T q_1, FLOAT_T q_2, FLOAT_T q_3, FLOAT_T q_4) const 15 | { 16 | std::ofstream file(name_); 17 | 18 | if (!file) 19 | { 20 | throw std::ifstream::failure("Error while opening file for writing"); 21 | } 22 | 23 | std::stringstream data; 24 | 25 | data << points.size() << "\n\n"; 26 | 27 | for (const auto& point : points) 28 | { 29 | data << point.x << ' ' << point.y << ' ' << point.z << '\n'; 30 | } 31 | 32 | for (const auto& ring : rings) 33 | { 34 | data << ring << '\n'; 35 | } 36 | 37 | data << particles.size() << '\n'; 38 | 39 | for (const auto& particle : particles) 40 | { 41 | data << particle.first[0] << ' ' << particle.first[1] << ' ' << particle.first[2] << ' ' << particle.first[3] << ' ' << particle.first[4] << ' ' << particle.first[5] << " " << particle.second << '\n'; 42 | } 43 | 44 | data << '\n'; 45 | 46 | for (auto index = 0; index < 16; ++index) 47 | { 48 | data << tf[index] << ' '; 49 | } 50 | 51 | data << '\n'; 52 | 53 | data << x << " " << y << " " << z << " " << q_1 << " " << q_2 << " " << q_3 << " " << q_4; 54 | 55 | file << data.str(); 56 | file.flush(); 57 | 58 | if (!file) 59 | { 60 | throw std::ifstream::failure("Error while writing into file"); 61 | } 62 | 63 | file.close(); 64 | } 65 | 66 | void MCLFile::read(std::vector& points, std::vector& rings, std::vector& particles, std::array& tf, FLOAT_T& x, FLOAT_T& y, FLOAT_T& z, FLOAT_T& q_1, FLOAT_T& q_2, FLOAT_T& q_3, FLOAT_T& q_4) const 67 | { 68 | std::ifstream file(name_); 69 | 70 | if(!file) 71 | { 72 | throw std::ifstream::failure("Error while opening file for reading"); 73 | } 74 | 75 | size_t points_size = 0; 76 | size_t particles_size = 0; 77 | 78 | file >> points_size; 79 | 80 | points.resize(points_size); 81 | rings.resize(points_size); 82 | 83 | for (auto index = 0u; index < points_size; ++index) 84 | { 85 | file >> points[index].x >> points[index].y >> points[index].z; 86 | } 87 | 88 | for (auto index = 0u; index < points_size; ++index) 89 | { 90 | file >> rings[index]; 91 | } 92 | 93 | file >> particles_size; 94 | 95 | particles.resize(particles_size); 96 | 97 | for (auto index = 0u; index < particles_size; ++index) 98 | { 99 | file >> particles[index].first[0] >> particles[index].first[1] >> particles[index].first[2] >> particles[index].first[3] >> particles[index].first[4] >> particles[index].first[5] >> particles[index].second; 100 | } 101 | 102 | for (auto index = 0; index < 16; ++index) 103 | { 104 | file >> tf[index]; 105 | } 106 | 107 | file >> x >> y >> z >> q_1 >> q_2 >> q_3 >> q_4; 108 | 109 | if (!file) 110 | { 111 | throw std::ifstream::failure("Error: Could not read mcl data from file"); 112 | } 113 | } 114 | 115 | } // namespace tsdf_localization -------------------------------------------------------------------------------- /src/util/runtime_evaluator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file runtime_evaluator.cpp 3 | * @author Marc Eisoldt 4 | * @author Steffen Hinderink 5 | */ 6 | 7 | #include 8 | 9 | #include // for output 10 | #include // for formatting 11 | #include 12 | 13 | using namespace std::chrono; 14 | 15 | namespace tsdf_localization 16 | { 17 | 18 | RuntimeEvaluator& RuntimeEvaluator::get_instance() 19 | { 20 | static RuntimeEvaluator instance; 21 | return instance; 22 | } 23 | 24 | RuntimeEvaluator::RuntimeEvaluator() : forms_(), histogram_(8, 0) 25 | { 26 | overhang_time_ = 0.0; 27 | num_dropped_scans = num_scans_over_50ms = 0; 28 | // "unpause" for the first time 29 | start_ = high_resolution_clock::now(); 30 | } 31 | 32 | void RuntimeEvaluator::pause() 33 | { 34 | auto stop = high_resolution_clock::now(); 35 | auto duration = duration_cast(stop - start_); 36 | 37 | // add new interval to all active measurements 38 | for (uint i = 0; i < forms_.size(); i++) 39 | { 40 | if (forms_[i].active) 41 | { 42 | forms_[i].accumulate += duration.count(); 43 | } 44 | } 45 | } 46 | 47 | void RuntimeEvaluator::resume() 48 | { 49 | start_ = high_resolution_clock::now(); 50 | } 51 | 52 | int RuntimeEvaluator::find_formular(const std::string& task_name) 53 | { 54 | for (uint i = 0; i < forms_.size(); i++) 55 | { 56 | if (forms_[i].name == task_name) 57 | { 58 | return i; 59 | } 60 | } 61 | 62 | return -1; 63 | } 64 | 65 | void RuntimeEvaluator::start(const std::string& task_name, bool recorded) 66 | { 67 | #ifdef TIME_MEASUREMENT 68 | pause(); 69 | 70 | // get or create task that is started 71 | int index = find_formular(task_name); 72 | 73 | if (index == -1) 74 | { 75 | index = forms_.size(); 76 | forms_.push_back(EvaluationFormular(task_name, recorded)); 77 | } 78 | else if (forms_[index].active) 79 | { 80 | throw RuntimeEvaluationException(); 81 | } 82 | 83 | // start 84 | forms_[index].active = true; 85 | forms_[index].accumulate = 0; 86 | 87 | resume(); 88 | #endif 89 | } 90 | 91 | void RuntimeEvaluator::clear() 92 | { 93 | forms_.clear(); 94 | } 95 | 96 | void RuntimeEvaluator::stop(const std::string& task_name) 97 | { 98 | #ifdef TIME_MEASUREMENT 99 | pause(); 100 | 101 | // get task that is stopped 102 | auto index = find_formular(task_name); 103 | 104 | if (index == -1 || !forms_[index].active) 105 | { 106 | throw RuntimeEvaluationException(); 107 | } 108 | 109 | // stop 110 | unsigned long long time = forms_[index].accumulate; 111 | forms_[index].active = false; 112 | forms_[index].count++; 113 | forms_[index].last = time; 114 | forms_[index].sum += time; 115 | forms_[index].filter.update(time); 116 | if (time < forms_[index].min) 117 | { 118 | forms_[index].min = time; 119 | } 120 | if (time > forms_[index].max && forms_[index].count != 1) // ignore first 121 | { 122 | forms_[index].max = time; 123 | } 124 | 125 | if (forms_[index].recorded) 126 | { 127 | forms_[index].recorded_data.push_back(time); 128 | } 129 | 130 | if (task_name == "total") 131 | { 132 | double time_ms = time / 1000.0; 133 | 134 | if (time_ms > 50.0) 135 | { 136 | num_scans_over_50ms++; 137 | } 138 | 139 | overhang_time_ = std::max(overhang_time_ + time_ms - 50.0, 0.0); 140 | if (overhang_time_ >= 50.0) 141 | { 142 | num_dropped_scans++; 143 | overhang_time_ -= 50.0; 144 | } 145 | 146 | size_t bucket = time_ms / HIST_BUCKET_SIZE; 147 | bucket = std::min(bucket, histogram_.size() - 1); 148 | histogram_[bucket]++; 149 | } 150 | 151 | resume(); 152 | #endif 153 | } 154 | 155 | const std::vector RuntimeEvaluator::get_forms() 156 | { 157 | return forms_; 158 | } 159 | 160 | std::string RuntimeEvaluator::to_string(bool with_recorded) 161 | { 162 | pause(); 163 | 164 | constexpr int FIELD_COUNT = 7; 165 | static std::string fields[FIELD_COUNT] = { "task", "count", "last", "min", "max", "avg", "run_avg" }; 166 | 167 | size_t width = 0; 168 | for (const auto& ef : forms_) 169 | { 170 | width = std::max(width, ef.name.length()); 171 | } 172 | for (int i = 0; i < FIELD_COUNT; i++) 173 | { 174 | width = std::max(width, fields[i].length()); 175 | } 176 | 177 | std::stringstream ss; 178 | ss << std::setfill(' ') << "\n"; 179 | for (int i = 0; i < FIELD_COUNT; i++) 180 | { 181 | ss << std::setw(width) << fields[i] << (i == FIELD_COUNT - 1 ? "\n" : " | "); 182 | } 183 | ss << std::setfill('-'); 184 | for (int i = 0; i < FIELD_COUNT; i++) 185 | { 186 | ss << std::setw(width) << "" << (i == FIELD_COUNT - 1 ? "-\n" : "-+-"); 187 | } 188 | ss << std::setfill(' '); 189 | 190 | for (const auto& ef : forms_) 191 | { 192 | if (ef.active) 193 | { 194 | continue; 195 | } 196 | unsigned long long avg = ef.sum / ef.count; 197 | unsigned long long run_avg = (int)ef.filter.get_mean(); 198 | unsigned long long values[] = { ef.last, ef.min, ef.max, avg, run_avg }; 199 | ss << std::setw(width) << ef.name << " | " 200 | << std::setw(width) << ef.count << " | "; 201 | for (int i = 0; i < FIELD_COUNT - 2; i++) 202 | { 203 | ss << std::setw(width) << values[i] / 1000 << (i == FIELD_COUNT - 3 ? "\n" : " | "); 204 | } 205 | } 206 | 207 | // Histogram of total time 208 | int total_index = find_formular("total"); 209 | if (total_index != -1) 210 | { 211 | const auto& form = forms_[total_index]; 212 | 213 | ss << "Over 50ms: " << std::setw(width) << num_scans_over_50ms << " / " << form.count 214 | << " = " << std::setw(2) << 100 * num_scans_over_50ms / form.count << "%\n"; 215 | ss << "Dropped : " << std::setw(width) << num_dropped_scans << " / " << form.count 216 | << " = " << std::setw(2) << 100 * num_dropped_scans / form.count << "%\n"; 217 | 218 | // columns with padding + separators - start of line 219 | int line_length = FIELD_COUNT * (width + 2) + (FIELD_COUNT - 1) - std::string("10-20: ").length(); 220 | 221 | for (size_t i = 0; i < histogram_.size(); i++) 222 | { 223 | ss << std::setw(2) << (i * HIST_BUCKET_SIZE) << '-'; 224 | if (i < histogram_.size() - 1) 225 | { 226 | ss << std::setw(2) << (i + 1) * HIST_BUCKET_SIZE; 227 | } 228 | else 229 | { 230 | ss << " "; 231 | } 232 | int count = std::ceil((double)histogram_[i] * line_length / form.count); 233 | ss << ": " << std::string(count, '=') << "\n"; 234 | } 235 | } 236 | 237 | if (with_recorded) 238 | { 239 | ss << "\nRecorded data:\n\n"; 240 | 241 | for (const auto& ef : forms_) 242 | { 243 | if (ef.recorded) 244 | { 245 | ss << ef.name << "\n"; 246 | 247 | for (const auto& time : ef.recorded_data) 248 | { 249 | ss << time << "\n"; 250 | } 251 | 252 | ss << "\n"; 253 | } 254 | } 255 | } 256 | 257 | resume(); 258 | return ss.str(); 259 | } 260 | 261 | std::ostream& operator<<(std::ostream& os, RuntimeEvaluator& evaluator) 262 | { 263 | #ifdef TIME_MEASUREMENT 264 | os << evaluator.to_string(); 265 | #endif 266 | return os; 267 | } 268 | 269 | } // namespace tsdf_localization 270 | -------------------------------------------------------------------------------- /src/util/util.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace tsdf_localization 14 | { 15 | 16 | FLOAT_T getYawFromQuaternion(const geometry_msgs::Quaternion& quaternion) 17 | { 18 | double roll, pitch, yaw; 19 | tf2::Quaternion tf_quaternion; 20 | 21 | tf2::convert(quaternion, tf_quaternion); 22 | tf2::Matrix3x3(tf_quaternion).getRPY(roll, pitch, yaw); 23 | 24 | return yaw; 25 | } 26 | 27 | FLOAT_T getRollFromQuaternion(const geometry_msgs::Quaternion& quaternion) 28 | { 29 | double roll, pitch, yaw; 30 | tf2::Quaternion tf_quaternion; 31 | 32 | tf2::convert(quaternion, tf_quaternion); 33 | tf2::Matrix3x3(tf_quaternion).getRPY(roll, pitch, yaw); 34 | 35 | return roll; 36 | } 37 | 38 | FLOAT_T getPitchFromQuaternion(const geometry_msgs::Quaternion& quaternion) 39 | { 40 | double roll, pitch, yaw; 41 | tf2::Quaternion tf_quaternion; 42 | 43 | tf2::convert(quaternion, tf_quaternion); 44 | tf2::Matrix3x3(tf_quaternion).getRPY(roll, pitch, yaw); 45 | 46 | return pitch; 47 | } 48 | 49 | geometry_msgs::Point transformPoint(const geometry_msgs::Point& point, const geometry_msgs::Pose& origin) 50 | { 51 | geometry_msgs::Point result; 52 | geometry_msgs::TransformStamped stamped_transform; 53 | stamped_transform.transform.translation.x = origin.position.x; 54 | stamped_transform.transform.translation.y = origin.position.y; 55 | stamped_transform.transform.translation.z = origin.position.z; 56 | stamped_transform.transform.rotation = origin.orientation; 57 | 58 | tf2::doTransform(point, result, stamped_transform); 59 | 60 | return result; 61 | } 62 | 63 | 64 | geometry_msgs::TransformStamped getMapToWorldTF(const nav_msgs::MapMetaData& map_meta) 65 | { 66 | geometry_msgs::TransformStamped map_to_world; 67 | map_to_world.transform.translation.x = map_meta.origin.position.x; 68 | map_to_world.transform.translation.y = map_meta.origin.position.y; 69 | map_to_world.transform.translation.z = map_meta.origin.position.z; 70 | map_to_world.transform.rotation = map_meta.origin.orientation; 71 | 72 | return map_to_world; 73 | } 74 | 75 | geometry_msgs::TransformStamped getWorldToMapTF(const nav_msgs::MapMetaData& map_meta) 76 | { 77 | geometry_msgs::TransformStamped map_to_world = getMapToWorldTF(map_meta); 78 | geometry_msgs::TransformStamped world_to_map; 79 | tf2::Transform tf_tmp; 80 | tf2::convert(map_to_world.transform, tf_tmp); 81 | tf2::convert(tf_tmp.inverse(), world_to_map.transform); 82 | 83 | return world_to_map; 84 | } 85 | 86 | void getAngleFromMat(const FLOAT_T mat[16], FLOAT_T& roll, FLOAT_T& pitch, FLOAT_T& yaw) 87 | { 88 | if (fabs(mat[8]) >= 1) 89 | { 90 | yaw = 0; 91 | 92 | auto delta = atan2(mat[9], mat[10]); 93 | 94 | if (mat[8] < 0) 95 | { 96 | pitch = M_PI / 2.0; 97 | roll = delta; 98 | } 99 | else 100 | { 101 | pitch = -M_PI / 2.0; 102 | roll = delta; 103 | } 104 | } 105 | else 106 | { 107 | pitch = -asin(mat[8]); 108 | roll = atan2(mat[9] / cos(pitch), mat[10] / cos(pitch)); 109 | yaw = atan2(mat[4] / cos(pitch), mat[0] / cos(pitch)); 110 | } 111 | } 112 | 113 | } // namespace tsdf_localization --------------------------------------------------------------------------------