├── .gitignore ├── CMakeLists.txt.ros1 ├── CMakeLists.txt.ros2 ├── LICENSE ├── README.md ├── cmake ├── voxelized_geometry_tools-dependencies.cmake └── voxelized_geometry_tools-extras.cmake ├── example ├── estimate_distance.cpp ├── pointcloud_voxelization.cpp ├── spatial_segments.cpp └── tutorial.cpp ├── include └── voxelized_geometry_tools │ ├── cl.hpp │ ├── collision_map.hpp │ ├── cpu_pointcloud_voxelization.hpp │ ├── cuda_voxelization_helpers.h │ ├── device_pointcloud_voxelization.hpp │ ├── device_voxelization_interface.hpp │ ├── dynamic_spatial_hashed_collision_map.hpp │ ├── mesh_rasterizer.hpp │ ├── opencl_voxelization_helpers.h │ ├── pointcloud_voxelization.hpp │ ├── pointcloud_voxelization_interface.hpp │ ├── pointcloud_voxelization_ros_interface.hpp │ ├── ros_interface.hpp │ ├── signed_distance_field.hpp │ ├── signed_distance_field_generation.hpp │ ├── tagged_object_collision_map.hpp │ ├── topology_computation.hpp │ └── vgt_namespace.hpp ├── msg ├── CollisionMapMessage.msg ├── DynamicSpatialHashedCollisionMapMessage.msg ├── SignedDistanceFieldMessage.msg └── TaggedObjectCollisionMapMessage.msg ├── package.xml.ros1 ├── package.xml.ros2 ├── src └── voxelized_geometry_tools │ ├── collision_map.cpp │ ├── cpu_pointcloud_voxelization.cpp │ ├── cuda_voxelization_helpers.cu │ ├── device_pointcloud_voxelization.cpp │ ├── dummy_cuda_voxelization_helpers.cc │ ├── dummy_opencl_voxelization_helpers.cc │ ├── dynamic_spatial_hashed_collision_map.cpp │ ├── mesh_rasterizer.cpp │ ├── opencl_voxelization_helpers.cc │ ├── pointcloud_voxelization.cpp │ ├── pointcloud_voxelization_ros_interface.cpp │ ├── ros_interface.cpp │ ├── signed_distance_field.cpp │ ├── signed_distance_field_generation.cpp │ └── tagged_object_collision_map.cpp └── test ├── mesh_rasterization_test.cpp ├── pointcloud_voxelization_test.cpp └── sdf_generation_test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt 2 | package.xml 3 | -------------------------------------------------------------------------------- /CMakeLists.txt.ros1: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxelized_geometry_tools) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp 8 | std_msgs 9 | geometry_msgs 10 | visualization_msgs 11 | message_generation 12 | common_robotics_utilities) 13 | find_package(Eigen3 REQUIRED) 14 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 15 | find_package(OpenMP) 16 | find_package(CUDA) 17 | find_package(OpenCL) 18 | 19 | ## We don't depend on Drake, but we do use different build flags if present. 20 | find_package(drake QUIET) 21 | 22 | ####################################### 23 | ## Declare ROS messages and services ## 24 | ####################################### 25 | 26 | ## Generate messages in the 'msg' folder 27 | add_message_files(DIRECTORY msg 28 | FILES 29 | CollisionMapMessage.msg 30 | DynamicSpatialHashedCollisionMapMessage.msg 31 | SignedDistanceFieldMessage.msg 32 | TaggedObjectCollisionMapMessage.msg) 33 | 34 | ## Generate added messages and services with any dependencies listed here 35 | generate_messages(DEPENDENCIES geometry_msgs std_msgs) 36 | 37 | ################################### 38 | ## catkin specific configuration ## 39 | ################################### 40 | ## The catkin_package macro generates cmake config files for your package 41 | ## Declare things to be passed to dependent projects 42 | ## LIBRARIES: libraries you create in this project 43 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 44 | ## DEPENDS: system dependencies of this project 45 | catkin_package(INCLUDE_DIRS 46 | include 47 | LIBRARIES 48 | ${PROJECT_NAME} 49 | ${PROJECT_NAME}_pointcloud_voxelization 50 | ${PROJECT_NAME}_pointcloud_voxelization_ros_interface 51 | ${PROJECT_NAME}_ros_interface 52 | CATKIN_DEPENDS 53 | roscpp 54 | std_msgs 55 | geometry_msgs 56 | visualization_msgs 57 | message_runtime 58 | common_robotics_utilities 59 | DEPENDS 60 | Eigen3 61 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake) 62 | 63 | ########### 64 | ## Build ## 65 | ########### 66 | 67 | ## Specify additional locations of header files 68 | ## Your package locations should be listed before other locations 69 | include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} 70 | ${Eigen3_INCLUDE_DIRS}) 71 | 72 | ## Build options 73 | set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) 74 | cmake_policy(SET CMP0069 NEW) 75 | 76 | add_compile_options(-std=c++11) 77 | add_compile_options(-Wall) 78 | add_compile_options(-Wextra) 79 | add_compile_options(-Werror) 80 | add_compile_options(-Wconversion) 81 | add_compile_options(-Wshadow) 82 | add_compile_options(-O3) 83 | add_compile_options(-g) 84 | add_compile_options(-Werror=non-virtual-dtor) 85 | add_compile_options(-Wold-style-cast) 86 | add_compile_options(-Wpessimizing-move) 87 | add_compile_options(-Wuninitialized) 88 | add_compile_options(-Wmissing-declarations) 89 | 90 | if(drake_FOUND) 91 | message(STATUS "Drake found, disabling -march=native") 92 | else() 93 | message(STATUS "Drake NOT found, enabling -march=native") 94 | add_compile_options(-march=native) 95 | endif() 96 | 97 | add_definitions(-DVOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION=1) 98 | 99 | ## It's not clear if add_compile_options does the right things for flags that 100 | ## may differ between languages and target type. 101 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 102 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 103 | set(CMAKE_EXE_LINKER_FLAGS 104 | "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 105 | set(CMAKE_SHARED_LINKER_FLAGS 106 | "${CMAKE_SHARED_LINKER_FLAGS} ${OpenMP_SHARED_LINKER_FLAGS}") 107 | 108 | # Voxelized geometry tools library 109 | add_library(${PROJECT_NAME} 110 | include/${PROJECT_NAME}/collision_map.hpp 111 | include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp 112 | include/${PROJECT_NAME}/mesh_rasterizer.hpp 113 | include/${PROJECT_NAME}/signed_distance_field.hpp 114 | include/${PROJECT_NAME}/signed_distance_field_generation.hpp 115 | include/${PROJECT_NAME}/tagged_object_collision_map.hpp 116 | include/${PROJECT_NAME}/topology_computation.hpp 117 | src/${PROJECT_NAME}/collision_map.cpp 118 | src/${PROJECT_NAME}/mesh_rasterizer.cpp 119 | src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp 120 | src/${PROJECT_NAME}/signed_distance_field.cpp 121 | src/${PROJECT_NAME}/signed_distance_field_generation.cpp 122 | src/${PROJECT_NAME}/tagged_object_collision_map.cpp) 123 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 124 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 125 | 126 | # Cuda voxelization helpers library 127 | if(CUDA_FOUND) 128 | message(STATUS "CUDA found. Building CUDA voxelization helpers") 129 | include_directories(SYSTEM ${CUDA_INCLUDE_DIRS}) 130 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -std=c++11 -O3 -use_fast_math") 131 | set(CUDA_NVCC_FLAGS 132 | "${CUDA_NVCC_FLAGS} -Xcompiler -D__CORRECT_ISO_CPP11_MATH_H_PROTO") 133 | cuda_add_library(${PROJECT_NAME}_cuda_voxelization_helpers 134 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 135 | include/${PROJECT_NAME}/cuda_voxelization_helpers.h 136 | src/${PROJECT_NAME}/cuda_voxelization_helpers.cu) 137 | target_link_libraries(${PROJECT_NAME}_cuda_voxelization_helpers 138 | cuda ${CUDA_LIBRARIES}) 139 | add_library(${PROJECT_NAME}_dummy_cuda_voxelization_helpers 140 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 141 | include/${PROJECT_NAME}/cuda_voxelization_helpers.h 142 | src/${PROJECT_NAME}/dummy_cuda_voxelization_helpers.cc) 143 | else() 144 | message(WARNING "CUDA not found. Building stubs for CUDA helpers.") 145 | add_library(${PROJECT_NAME}_cuda_voxelization_helpers 146 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 147 | include/${PROJECT_NAME}/cuda_voxelization_helpers.h 148 | src/${PROJECT_NAME}/dummy_cuda_voxelization_helpers.cc) 149 | endif() 150 | 151 | # OpenCL voxelization helpers library 152 | if(OpenCL_FOUND) 153 | message(STATUS "OpenCL found. Building OpenCL voxelization helpers") 154 | include_directories(SYSTEM ${OpenCL_INCLUDE_DIRS}) 155 | add_library(${PROJECT_NAME}_opencl_voxelization_helpers 156 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 157 | include/${PROJECT_NAME}/opencl_voxelization_helpers.h 158 | src/${PROJECT_NAME}/opencl_voxelization_helpers.cc) 159 | target_link_libraries(${PROJECT_NAME}_opencl_voxelization_helpers 160 | ${OpenCL_LIBRARIES}) 161 | add_library(${PROJECT_NAME}_dummy_opencl_voxelization_helpers 162 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 163 | include/${PROJECT_NAME}/opencl_voxelization_helpers.h 164 | src/${PROJECT_NAME}/dummy_opencl_voxelization_helpers.cc) 165 | else() 166 | message(WARNING "OpenCL not found. Building stubs for OpenCL helpers.") 167 | add_library(${PROJECT_NAME}_opencl_voxelization_helpers 168 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 169 | include/${PROJECT_NAME}/opencl_voxelization_helpers.h 170 | src/${PROJECT_NAME}/dummy_opencl_voxelization_helpers.cc) 171 | endif() 172 | 173 | # Pointcloud voxelization 174 | add_library(${PROJECT_NAME}_pointcloud_voxelization 175 | include/${PROJECT_NAME}/cpu_pointcloud_voxelization.hpp 176 | include/${PROJECT_NAME}/device_pointcloud_voxelization.hpp 177 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 178 | include/${PROJECT_NAME}/pointcloud_voxelization_interface.hpp 179 | include/${PROJECT_NAME}/pointcloud_voxelization.hpp 180 | src/${PROJECT_NAME}/cpu_pointcloud_voxelization.cpp 181 | src/${PROJECT_NAME}/device_pointcloud_voxelization.cpp 182 | src/${PROJECT_NAME}/pointcloud_voxelization.cpp) 183 | add_dependencies(${PROJECT_NAME}_pointcloud_voxelization 184 | ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp) 185 | target_link_libraries(${PROJECT_NAME}_pointcloud_voxelization 186 | ${PROJECT_NAME}_cuda_voxelization_helpers 187 | ${PROJECT_NAME}_opencl_voxelization_helpers 188 | ${PROJECT_NAME} ${catkin_LIBRARIES}) 189 | 190 | # ROS interface libraries 191 | add_library(${PROJECT_NAME}_ros_interface 192 | include/${PROJECT_NAME}/ros_interface.hpp 193 | src/${PROJECT_NAME}/ros_interface.cpp) 194 | add_dependencies(${PROJECT_NAME}_ros_interface ${catkin_EXPORTED_TARGETS} 195 | ${PROJECT_NAME}_gencpp) 196 | target_link_libraries(${PROJECT_NAME}_ros_interface ${PROJECT_NAME} 197 | ${catkin_LIBRARIES}) 198 | 199 | add_library(${PROJECT_NAME}_pointcloud_voxelization_ros_interface 200 | include/${PROJECT_NAME}/pointcloud_voxelization_ros_interface.hpp 201 | src/${PROJECT_NAME}/pointcloud_voxelization_ros_interface.cpp) 202 | add_dependencies(${PROJECT_NAME}_pointcloud_voxelization_ros_interface 203 | ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp) 204 | target_link_libraries(${PROJECT_NAME}_pointcloud_voxelization_ros_interface 205 | ${PROJECT_NAME}_pointcloud_voxelization 206 | ${catkin_LIBRARIES}) 207 | 208 | # Examples 209 | add_executable(voxelized_geometry_tools_tutorial example/tutorial.cpp) 210 | add_dependencies(voxelized_geometry_tools_tutorial 211 | ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp) 212 | target_link_libraries(voxelized_geometry_tools_tutorial 213 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME} ${catkin_LIBRARIES}) 214 | 215 | add_executable(estimate_distance_example example/estimate_distance.cpp) 216 | add_dependencies(estimate_distance_example 217 | ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp) 218 | target_link_libraries(estimate_distance_example 219 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME} ${catkin_LIBRARIES}) 220 | 221 | add_executable(spatial_segments example/spatial_segments.cpp) 222 | add_dependencies(spatial_segments 223 | ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp) 224 | target_link_libraries(spatial_segments 225 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME} ${catkin_LIBRARIES}) 226 | 227 | add_executable(pointcloud_voxelization example/pointcloud_voxelization.cpp) 228 | add_dependencies(pointcloud_voxelization 229 | ${PROJECT_NAME}_pointcloud_voxelization ${PROJECT_NAME} 230 | ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp) 231 | target_link_libraries(pointcloud_voxelization 232 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME}_pointcloud_voxelization 233 | ${PROJECT_NAME} ${catkin_LIBRARIES}) 234 | 235 | if(CATKIN_ENABLE_TESTING) 236 | # Tests 237 | catkin_add_gtest(pointcloud_voxelization_test 238 | test/pointcloud_voxelization_test.cpp) 239 | add_dependencies(pointcloud_voxelization_test 240 | ${PROJECT_NAME}_pointcloud_voxelization) 241 | target_link_libraries(pointcloud_voxelization_test 242 | ${PROJECT_NAME}_pointcloud_voxelization) 243 | 244 | catkin_add_gtest(mesh_rasterization_test test/mesh_rasterization_test.cpp) 245 | add_dependencies(mesh_rasterization_test ${PROJECT_NAME}) 246 | target_link_libraries(mesh_rasterization_test ${PROJECT_NAME}) 247 | 248 | catkin_add_gtest(sdf_generation_test test/sdf_generation_test.cpp) 249 | add_dependencies(sdf_generation_test ${PROJECT_NAME}) 250 | target_link_libraries(sdf_generation_test ${PROJECT_NAME}) 251 | endif() 252 | 253 | ############# 254 | ## Install ## 255 | ############# 256 | 257 | ## Mark library for installation 258 | install(TARGETS ${PROJECT_NAME} 259 | ${PROJECT_NAME}_cuda_voxelization_helpers 260 | ${PROJECT_NAME}_opencl_voxelization_helpers 261 | ${PROJECT_NAME}_pointcloud_voxelization 262 | ${PROJECT_NAME}_pointcloud_voxelization_ros_interface 263 | ${PROJECT_NAME}_ros_interface 264 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 265 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 266 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 267 | ) 268 | 269 | # Mark examples for installation 270 | install(TARGETS 271 | voxelized_geometry_tools_tutorial 272 | estimate_distance_example 273 | spatial_segments 274 | pointcloud_voxelization 275 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 276 | ) 277 | 278 | ## Mark cpp header files for installation 279 | install(DIRECTORY include/${PROJECT_NAME}/ 280 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 281 | FILES_MATCHING PATTERN "*.hpp" 282 | PATTERN ".svn" EXCLUDE 283 | ) 284 | -------------------------------------------------------------------------------- /CMakeLists.txt.ros2: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(voxelized_geometry_tools) 3 | 4 | find_package(ament_cmake_ros REQUIRED) 5 | find_package(geometry_msgs REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rosidl_default_generators REQUIRED) 8 | find_package(sensor_msgs REQUIRED) 9 | find_package(std_msgs REQUIRED) 10 | find_package(visualization_msgs REQUIRED) 11 | 12 | find_package(common_robotics_utilities REQUIRED) 13 | 14 | find_package(Eigen3 REQUIRED) 15 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 16 | find_package(OpenMP) 17 | find_package(CUDA) 18 | find_package(OpenCL) 19 | 20 | ## We don't depend on Drake, but we do use different build flags if present. 21 | find_package(drake QUIET) 22 | 23 | ####################################### 24 | ## Declare ROS messages and services ## 25 | ####################################### 26 | 27 | ## Generate messages in the 'msg' folder 28 | 29 | rosidl_generate_interfaces( 30 | ${PROJECT_NAME}_interfaces 31 | msg/CollisionMapMessage.msg 32 | msg/DynamicSpatialHashedCollisionMapMessage.msg 33 | msg/SignedDistanceFieldMessage.msg 34 | msg/TaggedObjectCollisionMapMessage.msg 35 | DEPENDENCIES std_msgs 36 | ) 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | 42 | ## Specify additional locations of header files 43 | ## Your package locations should be listed before other locations 44 | include_directories(include SYSTEM ${Eigen3_INCLUDE_DIRS}) 45 | 46 | ## Build options 47 | set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) 48 | cmake_policy(SET CMP0069 NEW) 49 | 50 | add_compile_options(-std=c++17) 51 | add_compile_options(-Wall) 52 | add_compile_options(-Wextra) 53 | add_compile_options(-Werror) 54 | add_compile_options(-Wconversion) 55 | add_compile_options(-Wshadow) 56 | add_compile_options(-O3) 57 | add_compile_options(-g) 58 | add_compile_options(-Werror=non-virtual-dtor) 59 | add_compile_options(-Wold-style-cast) 60 | add_compile_options(-Wpessimizing-move) 61 | add_compile_options(-Wuninitialized) 62 | add_compile_options(-Wmissing-declarations) 63 | 64 | if(drake_FOUND) 65 | message(STATUS "Drake found, disabling -march=native") 66 | else() 67 | message(STATUS "Drake NOT found, enabling -march=native") 68 | add_compile_options(-march=native) 69 | endif() 70 | 71 | add_definitions(-DVOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION=2) 72 | 73 | ## It's not clear if add_compile_options does the right things for flags that 74 | ## may differ between languages and target type. 75 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 76 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 77 | set(CMAKE_EXE_LINKER_FLAGS 78 | "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 79 | set(CMAKE_SHARED_LINKER_FLAGS 80 | "${CMAKE_SHARED_LINKER_FLAGS} ${OpenMP_SHARED_LINKER_FLAGS}") 81 | 82 | # Voxelized geometry tools library 83 | add_library(${PROJECT_NAME} 84 | include/${PROJECT_NAME}/collision_map.hpp 85 | include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp 86 | include/${PROJECT_NAME}/mesh_rasterizer.hpp 87 | include/${PROJECT_NAME}/signed_distance_field.hpp 88 | include/${PROJECT_NAME}/signed_distance_field_generation.hpp 89 | include/${PROJECT_NAME}/tagged_object_collision_map.hpp 90 | include/${PROJECT_NAME}/topology_computation.hpp 91 | src/${PROJECT_NAME}/collision_map.cpp 92 | src/${PROJECT_NAME}/mesh_rasterizer.cpp 93 | src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp 94 | src/${PROJECT_NAME}/signed_distance_field.cpp 95 | src/${PROJECT_NAME}/signed_distance_field_generation.cpp 96 | src/${PROJECT_NAME}/tagged_object_collision_map.cpp) 97 | ament_target_dependencies(${PROJECT_NAME} common_robotics_utilities) 98 | 99 | # Cuda voxelization helpers library 100 | if(CUDA_FOUND) 101 | message(STATUS "CUDA found. Building CUDA voxelization helpers") 102 | include_directories(SYSTEM ${CUDA_INCLUDE_DIRS}) 103 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -std=c++11 -O3 -use_fast_math") 104 | set(CUDA_NVCC_FLAGS 105 | "${CUDA_NVCC_FLAGS} -Xcompiler -D__CORRECT_ISO_CPP11_MATH_H_PROTO") 106 | cuda_add_library(${PROJECT_NAME}_cuda_voxelization_helpers 107 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 108 | include/${PROJECT_NAME}/cuda_voxelization_helpers.h 109 | src/${PROJECT_NAME}/cuda_voxelization_helpers.cu) 110 | target_link_libraries(${PROJECT_NAME}_cuda_voxelization_helpers 111 | cuda ${CUDA_LIBRARIES}) 112 | add_library(${PROJECT_NAME}_dummy_cuda_voxelization_helpers 113 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 114 | include/${PROJECT_NAME}/cuda_voxelization_helpers.h 115 | src/${PROJECT_NAME}/dummy_cuda_voxelization_helpers.cc) 116 | else() 117 | message(WARNING "CUDA not found. Building stubs for CUDA helpers.") 118 | add_library(${PROJECT_NAME}_cuda_voxelization_helpers 119 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 120 | include/${PROJECT_NAME}/cuda_voxelization_helpers.h 121 | src/${PROJECT_NAME}/dummy_cuda_voxelization_helpers.cc) 122 | endif() 123 | 124 | # OpenCL voxelization helpers library 125 | if(OpenCL_FOUND) 126 | message(STATUS "OpenCL found. Building OpenCL voxelization helpers") 127 | include_directories(SYSTEM ${OpenCL_INCLUDE_DIRS}) 128 | add_library(${PROJECT_NAME}_opencl_voxelization_helpers 129 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 130 | include/${PROJECT_NAME}/opencl_voxelization_helpers.h 131 | src/${PROJECT_NAME}/opencl_voxelization_helpers.cc) 132 | target_link_libraries(${PROJECT_NAME}_opencl_voxelization_helpers 133 | ${OpenCL_LIBRARIES}) 134 | add_library(${PROJECT_NAME}_dummy_opencl_voxelization_helpers 135 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 136 | include/${PROJECT_NAME}/opencl_voxelization_helpers.h 137 | src/${PROJECT_NAME}/dummy_opencl_voxelization_helpers.cc) 138 | else() 139 | message(WARNING "OpenCL not found. Building stubs for OpenCL helpers.") 140 | add_library(${PROJECT_NAME}_opencl_voxelization_helpers 141 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 142 | include/${PROJECT_NAME}/opencl_voxelization_helpers.h 143 | src/${PROJECT_NAME}/dummy_opencl_voxelization_helpers.cc) 144 | endif() 145 | 146 | # Pointcloud voxelization 147 | add_library(${PROJECT_NAME}_pointcloud_voxelization 148 | include/${PROJECT_NAME}/cpu_pointcloud_voxelization.hpp 149 | include/${PROJECT_NAME}/device_pointcloud_voxelization.hpp 150 | include/${PROJECT_NAME}/device_voxelization_interface.hpp 151 | include/${PROJECT_NAME}/pointcloud_voxelization_interface.hpp 152 | include/${PROJECT_NAME}/pointcloud_voxelization.hpp 153 | src/${PROJECT_NAME}/cpu_pointcloud_voxelization.cpp 154 | src/${PROJECT_NAME}/device_pointcloud_voxelization.cpp 155 | src/${PROJECT_NAME}/pointcloud_voxelization.cpp) 156 | target_link_libraries(${PROJECT_NAME}_pointcloud_voxelization 157 | ${PROJECT_NAME}_cuda_voxelization_helpers 158 | ${PROJECT_NAME}_opencl_voxelization_helpers 159 | ${PROJECT_NAME}) 160 | 161 | # ROS interface libraries 162 | add_library(${PROJECT_NAME}_ros_interface 163 | include/${PROJECT_NAME}/ros_interface.hpp 164 | src/${PROJECT_NAME}/ros_interface.cpp) 165 | ament_target_dependencies(${PROJECT_NAME}_ros_interface 166 | common_robotics_utilities) 167 | ament_target_dependencies(${PROJECT_NAME}_ros_interface SYSTEM 168 | rclcpp 169 | std_msgs 170 | visualization_msgs) 171 | add_dependencies(${PROJECT_NAME}_ros_interface 172 | ${PROJECT_NAME}_interfaces) 173 | target_include_directories( 174 | ${PROJECT_NAME}_ros_interface 175 | PUBLIC 176 | "$" 177 | "$" 178 | ) 179 | target_link_libraries(${PROJECT_NAME}_ros_interface ${PROJECT_NAME}) 180 | 181 | add_library(${PROJECT_NAME}_pointcloud_voxelization_ros_interface 182 | include/${PROJECT_NAME}/pointcloud_voxelization_ros_interface.hpp 183 | src/${PROJECT_NAME}/pointcloud_voxelization_ros_interface.cpp) 184 | ament_target_dependencies(${PROJECT_NAME}_pointcloud_voxelization_ros_interface 185 | SYSTEM sensor_msgs) 186 | target_link_libraries(${PROJECT_NAME}_pointcloud_voxelization_ros_interface 187 | ${PROJECT_NAME}_pointcloud_voxelization) 188 | 189 | # Examples 190 | add_executable(voxelized_geometry_tools_tutorial example/tutorial.cpp) 191 | ament_target_dependencies(voxelized_geometry_tools_tutorial 192 | common_robotics_utilities) 193 | ament_target_dependencies(voxelized_geometry_tools_tutorial 194 | SYSTEM rclcpp visualization_msgs) 195 | target_link_libraries(voxelized_geometry_tools_tutorial 196 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME}) 197 | 198 | add_executable(estimate_distance_example example/estimate_distance.cpp) 199 | ament_target_dependencies(estimate_distance_example 200 | common_robotics_utilities) 201 | ament_target_dependencies(estimate_distance_example 202 | SYSTEM rclcpp geometry_msgs std_msgs visualization_msgs) 203 | target_link_libraries(estimate_distance_example 204 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME}) 205 | 206 | add_executable(spatial_segments example/spatial_segments.cpp) 207 | ament_target_dependencies(spatial_segments common_robotics_utilities) 208 | ament_target_dependencies(spatial_segments SYSTEM rclcpp visualization_msgs) 209 | target_link_libraries(spatial_segments 210 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME}) 211 | 212 | add_executable(pointcloud_voxelization example/pointcloud_voxelization.cpp) 213 | ament_target_dependencies(pointcloud_voxelization 214 | common_robotics_utilities) 215 | ament_target_dependencies(pointcloud_voxelization 216 | SYSTEM rclcpp std_msgs visualization_msgs) 217 | target_link_libraries(pointcloud_voxelization 218 | ${PROJECT_NAME}_ros_interface ${PROJECT_NAME}_pointcloud_voxelization 219 | ${PROJECT_NAME}) 220 | 221 | if(BUILD_TESTING) 222 | # Tests 223 | find_package(ament_cmake_gtest REQUIRED) 224 | 225 | ament_add_gtest(pointcloud_voxelization_test 226 | test/pointcloud_voxelization_test.cpp) 227 | target_link_libraries(pointcloud_voxelization_test 228 | ${PROJECT_NAME}_pointcloud_voxelization) 229 | 230 | ament_add_gtest(mesh_rasterization_test test/mesh_rasterization_test.cpp) 231 | target_link_libraries(mesh_rasterization_test ${PROJECT_NAME}) 232 | 233 | ament_add_gtest(sdf_generation_test test/sdf_generation_test.cpp) 234 | target_link_libraries(sdf_generation_test ${PROJECT_NAME}) 235 | endif() 236 | 237 | ############# 238 | ## Install ## 239 | ############# 240 | 241 | ## Mark library for installation 242 | install(TARGETS 243 | ${PROJECT_NAME} 244 | ${PROJECT_NAME}_cuda_voxelization_helpers 245 | ${PROJECT_NAME}_opencl_voxelization_helpers 246 | ${PROJECT_NAME}_pointcloud_voxelization 247 | ${PROJECT_NAME}_pointcloud_voxelization_ros_interface 248 | ${PROJECT_NAME}_ros_interface 249 | ARCHIVE DESTINATION lib 250 | LIBRARY DESTINATION lib 251 | RUNTIME DESTINATION bin 252 | ) 253 | 254 | # Mark examples for installation 255 | install(TARGETS 256 | voxelized_geometry_tools_tutorial 257 | estimate_distance_example 258 | spatial_segments 259 | pointcloud_voxelization 260 | RUNTIME DESTINATION lib/${PROJECT_NAME} 261 | ) 262 | 263 | ## Mark cpp header files for installation 264 | install( 265 | DIRECTORY include/ 266 | DESTINATION include 267 | FILES_MATCHING PATTERN "*.hpp" 268 | PATTERN ".svn" EXCLUDE 269 | ) 270 | 271 | ament_export_definitions(-DVOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION=2) 272 | ament_export_include_directories(include) 273 | ament_export_libraries( 274 | ${PROJECT_NAME} 275 | ${PROJECT_NAME}_pointcloud_voxelization 276 | ${PROJECT_NAME}_pointcloud_voxelization_ros_interface 277 | ${PROJECT_NAME}_ros_interface 278 | ) 279 | ament_export_dependencies( 280 | rclcpp 281 | sensor_msgs 282 | std_msgs 283 | geometry_msgs 284 | visualization_msgs 285 | common_robotics_utilities 286 | ) 287 | 288 | ament_package(CONFIG_EXTRAS cmake/voxelized_geometry_tools-dependencies.cmake) 289 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, 4 | Portions Calder Phillips-Grafflin, Worcester Polytechnic Institute, 5 | The University of Michigan, and Toyota Research Institute. The included copy of 6 | the OpenCL C++ bindings in include/voxelized_geometry_tools/cl.hpp is covered by 7 | the permissive OpenCL license; see the license header in that file for details. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # voxelized_geometry_tools 2 | Voxelized geometry tools (Voxel-based collision/occupancy maps, signed-distance fields, discrete geometry tools) 3 | 4 | ## Setup 5 | 6 | `voxelized_geometry_tools` is a ROS package. 7 | 8 | Thus, it is best to build it within a ROS workspace: 9 | 10 | ```sh 11 | mkdir -p ~/ws/src 12 | cd ~/ws/src 13 | git clone https://github.com/calderpg/voxelized_geometry_tools.git 14 | ``` 15 | 16 | This package supports [ROS 1 Kinetic+](http://wiki.ros.org/ROS/Installation) 17 | and [ROS 2 Dashing+](https://index.ros.org/doc/ros2/Installation/) distributions. 18 | Make sure to symlink the corresponding `CMakeLists.txt` and `package.xml` files 19 | for the ROS distribution of choice: 20 | 21 | *For ROS 1 Kinetic+* 22 | ```sh 23 | cd ~/ws/src/voxelized_geometry_tools 24 | ln -sT CMakeLists.txt.ros1 CMakeLists.txt 25 | ln -sT package.xml.ros1 package.xml 26 | ``` 27 | 28 | *For ROS 2 Dashing+* 29 | ```sh 30 | cd ~/ws/src/voxelized_geometry_tools 31 | ln -sT CMakeLists.txt.ros2 CMakeLists.txt 32 | ln -sT package.xml.ros2 package.xml 33 | ``` 34 | 35 | Finally, use [`rosdep`](https://docs.ros.org/independent/api/rosdep/html/) 36 | to ensure all dependencies in the `package.xml` are satisfied: 37 | 38 | ```sh 39 | cd ~/ws 40 | rosdep install -i -y --from-path src 41 | ``` 42 | 43 | ## Building 44 | 45 | Use [`catkin_make`](http://wiki.ros.org/catkin/commands/catkin_make) or 46 | [`colcon`](https://colcon.readthedocs.io/en/released/) accordingly. 47 | 48 | *For ROS 1 Kinetic+* 49 | ```sh 50 | cd ~/ws 51 | catkin_make # the entire workspace 52 | catkin_make --pkg voxelized_geometry_tools # the package only 53 | ``` 54 | 55 | *For ROS 2 Dashing+* 56 | ```sh 57 | cd ~/ws 58 | colcon build # the entire workspace 59 | colcon build --packages-select voxelized_geometry_tools # the package only 60 | ``` 61 | 62 | ## Running examples 63 | 64 | Use [`rosrun`](http://wiki.ros.org/rosbash#rosrun) or 65 | [`ros2 run`](https://index.ros.org/doc/ros2/Tutorials/Understanding-ROS2-Nodes/#ros2-run) 66 | accordingly. 67 | 68 | *For ROS 1 Kinetic+* 69 | ```sh 70 | cd ~/ws 71 | source ./devel/setup.bash 72 | rosrun voxelized_geometry_tools 73 | ``` 74 | 75 | *For ROS 2 Dashing+* 76 | ```sh 77 | cd ~/ws 78 | source ./install/setup.bash 79 | ros2 run voxelized_geometry_tools 80 | ``` 81 | -------------------------------------------------------------------------------- /cmake/voxelized_geometry_tools-dependencies.cmake: -------------------------------------------------------------------------------- 1 | find_package(Eigen3 REQUIRED) 2 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 3 | include_directories(SYSTEM ${Eigen3_INCLUDE_DIRS}) 4 | -------------------------------------------------------------------------------- /cmake/voxelized_geometry_tools-extras.cmake: -------------------------------------------------------------------------------- 1 | # force automatic escaping of preprocessor definitions 2 | cmake_policy(PUSH) 3 | cmake_policy(SET CMP0005 NEW) 4 | 5 | add_definitions(-DVOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION=1) 6 | 7 | cmake_policy(POP) 8 | -------------------------------------------------------------------------------- /example/estimate_distance.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 7 | #include 8 | #include 9 | #include 10 | #include 11 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 12 | #include 13 | #include 14 | #include 15 | #include 16 | #else 17 | #error "Undefined or unknown VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION" 18 | #endif 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 26 | using Point = geometry_msgs::msg::Point; 27 | using ColorRGBA = std_msgs::msg::ColorRGBA; 28 | using Marker = visualization_msgs::msg::Marker; 29 | using MarkerArray = visualization_msgs::msg::MarkerArray; 30 | 31 | using Duration = rclcpp::Duration; 32 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 33 | using Point = geometry_msgs::Point; 34 | using ColorRGBA = std_msgs::ColorRGBA; 35 | using Marker = visualization_msgs::Marker; 36 | using MarkerArray = visualization_msgs::MarkerArray; 37 | 38 | using Duration = ros::Duration; 39 | #endif 40 | 41 | namespace 42 | { 43 | void test_estimate_distance( 44 | const std::function& display_fn) 45 | { 46 | const double res = 1.0; //0.125; //1.0; 47 | const double size = 10.0; 48 | const Eigen::Isometry3d origin_transform 49 | = Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond( 50 | Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitZ())); 51 | const common_robotics_utilities::voxel_grid::GridSizes map_sizes( 52 | res, size, size, 1.0); 53 | const voxelized_geometry_tools::CollisionCell empty_cell(0.0f); 54 | const voxelized_geometry_tools::CollisionCell filled_cell(1.0f); 55 | auto map = voxelized_geometry_tools::CollisionMap( 56 | origin_transform, "world", map_sizes, empty_cell); 57 | 58 | map.SetLocation4d( 59 | origin_transform * Eigen::Vector4d(5.0, 5.0, 0.0, 1.0), filled_cell); 60 | map.SetLocation4d( 61 | origin_transform * Eigen::Vector4d(5.0, 6.0, 0.0, 1.0), filled_cell); 62 | map.SetLocation4d( 63 | origin_transform * Eigen::Vector4d(6.0, 5.0, 0.0, 1.0), filled_cell); 64 | map.SetLocation4d( 65 | origin_transform * Eigen::Vector4d(6.0, 6.0, 0.0, 1.0), filled_cell); 66 | map.SetLocation4d( 67 | origin_transform * Eigen::Vector4d(7.0, 7.0, 0.0, 1.0), filled_cell); 68 | map.SetLocation4d( 69 | origin_transform * Eigen::Vector4d(2.0, 2.0, 0.0, 1.0), filled_cell); 70 | map.SetLocation4d( 71 | origin_transform * Eigen::Vector4d(3.0, 2.0, 0.0, 1.0), filled_cell); 72 | map.SetLocation4d( 73 | origin_transform * Eigen::Vector4d(4.0, 2.0, 0.0, 1.0), filled_cell); 74 | map.SetLocation4d( 75 | origin_transform * Eigen::Vector4d(2.0, 3.0, 0.0, 1.0), filled_cell); 76 | map.SetLocation4d( 77 | origin_transform * Eigen::Vector4d(2.0, 4.0, 0.0, 1.0), filled_cell); 78 | map.SetLocation4d( 79 | origin_transform * Eigen::Vector4d(2.0, 7.0, 0.0, 1.0), filled_cell); 80 | 81 | const ColorRGBA collision_color = 82 | common_robotics_utilities::color_builder::MakeFromFloatColors( 83 | 1.0, 0.0, 0.0, 0.5); 84 | const ColorRGBA free_color = 85 | common_robotics_utilities::color_builder::MakeFromFloatColors( 86 | 0.0, 0.0, 0.0, 0.0); 87 | const ColorRGBA unknown_color = 88 | common_robotics_utilities::color_builder::MakeFromFloatColors( 89 | 0.0, 0.0, 0.0, 0.0); 90 | const auto map_marker = 91 | voxelized_geometry_tools::ros_interface::ExportForDisplay( 92 | map, collision_color, free_color, unknown_color); 93 | 94 | const float oob_value = 1e6; 95 | const auto parallelism = 96 | common_robotics_utilities::parallelism::DegreeOfParallelism::None(); 97 | const bool unknown_is_filled = true; 98 | const bool add_virtual_border = false; 99 | const voxelized_geometry_tools::SignedDistanceFieldGenerationParameters 100 | sdf_parameters( 101 | oob_value, parallelism, unknown_is_filled, add_virtual_border); 102 | const auto sdf = map.ExtractSignedDistanceFieldFloat(sdf_parameters); 103 | const auto sdf_marker = 104 | voxelized_geometry_tools::ros_interface::ExportSDFForDisplay(sdf, 0.05f); 105 | 106 | // Assemble a visualization_markers::Marker representation of the SDF to 107 | // display estimated distance in RViz. 108 | Marker distance_rep; 109 | // Populate the header 110 | distance_rep.header.frame_id = "world"; 111 | // Populate the options 112 | distance_rep.ns = "estimated_distance_display"; 113 | distance_rep.id = 1; 114 | distance_rep.type = Marker::CUBE_LIST; 115 | distance_rep.action = Marker::ADD; 116 | distance_rep.lifetime = Duration(0, 0); 117 | distance_rep.frame_locked = false; 118 | distance_rep.pose = 119 | common_robotics_utilities::ros_conversions::EigenIsometry3dToGeometryPose( 120 | sdf.GetOriginTransform()); 121 | const double step = sdf.GetResolution() * 0.125 * 0.25; 122 | distance_rep.scale.x = sdf.GetResolution() * step; // * 0.125; 123 | distance_rep.scale.y = sdf.GetResolution() * step; // * 0.125; 124 | distance_rep.scale.z = sdf.GetResolution() * 0.95; // * 0.125; 125 | // Add all the cells of the SDF to the message 126 | double min_distance = 0.0; 127 | double max_distance = 0.0; 128 | // Add colors for all the cells of the SDF to the message 129 | for (double x = 0; x < sdf.GetXSize(); x += step) 130 | { 131 | for (double y = 0; y < sdf.GetYSize(); y += step) 132 | { 133 | double z = 0.5; 134 | //for (double z = 0; z <= sdf.GetZSize(); z += step) 135 | { 136 | const Eigen::Vector4d point_in_grid_frame(x, y, z, 1.0); 137 | const Eigen::Vector4d point = origin_transform * point_in_grid_frame; 138 | // Update minimum/maximum distance variables 139 | const double distance = sdf.EstimateLocationDistance4d(point).Value(); 140 | if (distance > max_distance) 141 | { 142 | max_distance = distance; 143 | } 144 | if (distance < min_distance) 145 | { 146 | min_distance = distance; 147 | } 148 | } 149 | } 150 | } 151 | 152 | std::cout << "Min dist " << min_distance << " Max dist " << max_distance 153 | << std::endl; 154 | 155 | for (double x = 0; x < sdf.GetXSize(); x += step) 156 | { 157 | for (double y = 0; y < sdf.GetYSize(); y += step) 158 | { 159 | double z = 0.5; 160 | //for (double z = 0; z <= sdf.GetZSize(); z += step) 161 | { 162 | const Eigen::Vector4d point_in_grid_frame(x, y, z, 1.0); 163 | const Eigen::Vector4d point = origin_transform * point_in_grid_frame; 164 | const double distance = sdf.EstimateLocationDistance4d(point).Value(); 165 | if (distance >= 0.0) 166 | { 167 | const ColorRGBA new_color = common_robotics_utilities::color_builder 168 | ::InterpolateHotToCold(distance, 0.0, max_distance); 169 | distance_rep.colors.push_back(new_color); 170 | } 171 | else 172 | { 173 | const ColorRGBA new_color = common_robotics_utilities::color_builder 174 | ::MakeFromFloatColors(1.0, 0.0, 1.0, 1.0); 175 | distance_rep.colors.push_back(new_color); 176 | } 177 | Point new_point; 178 | new_point.x = x; 179 | new_point.y = y; 180 | new_point.z = z; 181 | distance_rep.points.push_back(new_point); 182 | } 183 | } 184 | } 185 | 186 | MarkerArray markers; 187 | markers.markers = {map_marker, sdf_marker, distance_rep}; 188 | 189 | // Make gradient markers 190 | for (int64_t x_idx = 0; x_idx < sdf.GetNumXCells(); x_idx++) 191 | { 192 | for (int64_t y_idx = 0; y_idx < sdf.GetNumYCells(); y_idx++) 193 | { 194 | for (int64_t z_idx = 0; z_idx < sdf.GetNumZCells(); z_idx++) 195 | { 196 | const Eigen::Vector4d location = 197 | sdf.GridIndexToLocation(x_idx, y_idx, z_idx); 198 | const auto coarse_gradient = 199 | sdf.GetLocationCoarseGradient4d(location, true); 200 | const auto fine_gradient = sdf.GetLocationFineGradient4d( 201 | location, sdf.GetResolution() * 0.125); 202 | 203 | if (coarse_gradient) 204 | { 205 | std::cout << "Coarse gradient " 206 | << common_robotics_utilities::print::Print( 207 | coarse_gradient.Value()) 208 | << std::endl; 209 | const Eigen::Vector4d& gradient_vector = coarse_gradient.Value(); 210 | Marker gradient_rep; 211 | // Populate the header 212 | gradient_rep.header.frame_id = "world"; 213 | // Populate the options 214 | gradient_rep.ns = "coarse_gradient"; 215 | gradient_rep.id = 216 | static_cast(sdf.HashDataIndex(x_idx, y_idx, z_idx)); 217 | gradient_rep.type = Marker::ARROW; 218 | gradient_rep.action = Marker::ADD; 219 | gradient_rep.lifetime = Duration(0, 0); 220 | gradient_rep.frame_locked = false; 221 | gradient_rep.pose = common_robotics_utilities::ros_conversions 222 | ::EigenIsometry3dToGeometryPose(Eigen::Isometry3d::Identity()); 223 | gradient_rep.points.push_back( 224 | common_robotics_utilities::ros_conversions 225 | ::EigenVector4dToGeometryPoint(location)); 226 | gradient_rep.points.push_back( 227 | common_robotics_utilities::ros_conversions 228 | ::EigenVector4dToGeometryPoint(location + gradient_vector)); 229 | gradient_rep.scale.x = sdf.GetResolution() * 0.06125; 230 | gradient_rep.scale.y = sdf.GetResolution() * 0.125; 231 | gradient_rep.scale.z = 0.0; 232 | gradient_rep.color = common_robotics_utilities::color_builder 233 | ::MakeFromFloatColors(1.0, 0.5, 0.0, 1.0); 234 | 235 | markers.markers.push_back(gradient_rep); 236 | } 237 | 238 | if (fine_gradient) 239 | { 240 | std::cout << "Fine gradient " 241 | << common_robotics_utilities::print::Print( 242 | fine_gradient.Value()) 243 | << std::endl; 244 | const Eigen::Vector4d& gradient_vector = fine_gradient.Value(); 245 | Marker gradient_rep; 246 | // Populate the header 247 | gradient_rep.header.frame_id = "world"; 248 | // Populate the options 249 | gradient_rep.ns = "fine_gradient"; 250 | gradient_rep.id = 251 | static_cast(sdf.HashDataIndex(x_idx, y_idx, z_idx)); 252 | gradient_rep.type = Marker::ARROW; 253 | gradient_rep.action = Marker::ADD; 254 | gradient_rep.lifetime = Duration(0, 0); 255 | gradient_rep.frame_locked = false; 256 | gradient_rep.pose = common_robotics_utilities::ros_conversions 257 | ::EigenIsometry3dToGeometryPose(Eigen::Isometry3d::Identity()); 258 | gradient_rep.points.push_back( 259 | common_robotics_utilities::ros_conversions 260 | ::EigenVector4dToGeometryPoint(location)); 261 | gradient_rep.points.push_back( 262 | common_robotics_utilities::ros_conversions 263 | ::EigenVector4dToGeometryPoint(location + gradient_vector)); 264 | gradient_rep.scale.x = sdf.GetResolution() * 0.06125; 265 | gradient_rep.scale.y = sdf.GetResolution() * 0.125; 266 | gradient_rep.scale.z = 0.0; 267 | gradient_rep.color = common_robotics_utilities::color_builder 268 | ::MakeFromFloatColors(0.0, 0.5, 1.0, 1.0); 269 | 270 | markers.markers.push_back(gradient_rep); 271 | } 272 | } 273 | } 274 | } 275 | 276 | display_fn(markers); 277 | } 278 | } // namespace 279 | 280 | int main(int argc, char** argv) 281 | { 282 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 283 | rclcpp::init(argc, argv); 284 | auto node = std::make_shared("estimate_distance_test"); 285 | auto display_pub = node->create_publisher( 286 | "display_test_voxel_grid", rclcpp::QoS(1).transient_local()); 287 | const std::function& display_fn 288 | = [&] (const MarkerArray& markers) 289 | { 290 | display_pub->publish(markers); 291 | }; 292 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 293 | ros::init(argc, argv, "estimate_distance_test"); 294 | ros::NodeHandle nh; 295 | ros::Publisher display_pub = nh.advertise( 296 | "display_test_voxel_grid", 1, true); 297 | const std::function& display_fn 298 | = [&] (const MarkerArray& markers) 299 | { 300 | display_pub.publish(markers); 301 | }; 302 | #endif 303 | 304 | test_estimate_distance(display_fn); 305 | 306 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 307 | rclcpp::spin(node); 308 | rclcpp::shutdown(); 309 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 310 | ros::spin(); 311 | #endif 312 | return 0; 313 | } 314 | -------------------------------------------------------------------------------- /example/pointcloud_voxelization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 13 | #include 14 | #include 15 | #include 16 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 17 | #include 18 | #include 19 | #include 20 | #else 21 | #error "Undefined or unknown VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION" 22 | #endif 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | using voxelized_geometry_tools::pointcloud_voxelization 31 | ::CpuPointCloudVoxelizer; 32 | using voxelized_geometry_tools::pointcloud_voxelization 33 | ::CudaPointCloudVoxelizer; 34 | using voxelized_geometry_tools::pointcloud_voxelization 35 | ::OpenCLPointCloudVoxelizer; 36 | using voxelized_geometry_tools::pointcloud_voxelization 37 | ::PointCloudVoxelizationFilterOptions; 38 | using voxelized_geometry_tools::pointcloud_voxelization 39 | ::PointCloudVoxelizationInterface; 40 | using voxelized_geometry_tools::pointcloud_voxelization::PointCloudWrapper; 41 | using voxelized_geometry_tools::pointcloud_voxelization 42 | ::PointCloudWrapperSharedPtr; 43 | 44 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 45 | using ColorRGBA = std_msgs::msg::ColorRGBA; 46 | using MarkerArray = visualization_msgs::msg::MarkerArray; 47 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 48 | using ColorRGBA = std_msgs::ColorRGBA; 49 | using MarkerArray = visualization_msgs::MarkerArray; 50 | #endif 51 | 52 | namespace 53 | { 54 | class VectorVector3dPointCloudWrapper : public PointCloudWrapper 55 | { 56 | public: 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | 59 | void PushBack(const Eigen::Vector3d& point) 60 | { 61 | points_.push_back(point); 62 | } 63 | 64 | double MaxRange() const override 65 | { 66 | return std::numeric_limits::infinity(); 67 | } 68 | 69 | int64_t Size() const override { return static_cast(points_.size()); } 70 | 71 | const Eigen::Isometry3d& GetPointCloudOriginTransform() const override 72 | { 73 | return origin_transform_; 74 | } 75 | 76 | void SetPointCloudOriginTransform( 77 | const Eigen::Isometry3d& origin_transform) override 78 | { 79 | origin_transform_ = origin_transform; 80 | } 81 | 82 | private: 83 | void CopyPointLocationIntoDoublePtrImpl( 84 | const int64_t point_index, double* destination) const override 85 | { 86 | const Eigen::Vector3d& point = points_.at(static_cast(point_index)); 87 | std::memcpy(destination, point.data(), sizeof(double) * 3); 88 | } 89 | 90 | void CopyPointLocationIntoFloatPtrImpl( 91 | const int64_t point_index, float* destination) const override 92 | { 93 | const Eigen::Vector3f point = 94 | points_.at(static_cast(point_index)).cast(); 95 | std::memcpy(destination, point.data(), sizeof(float) * 3); 96 | } 97 | 98 | common_robotics_utilities::math::VectorVector3d points_; 99 | Eigen::Isometry3d origin_transform_ = Eigen::Isometry3d::Identity(); 100 | }; 101 | 102 | void check_equal(const float v1, const float v2) 103 | { 104 | if (v1 != v2) 105 | { 106 | const std::string msg = 107 | std::to_string(v1) + " does not equal " + std::to_string(v2); 108 | throw std::runtime_error(msg); 109 | } 110 | } 111 | 112 | void check_voxelization(const voxelized_geometry_tools::CollisionMap& occupancy) 113 | { 114 | // Make sure the grid is properly filled 115 | for (int64_t xidx = 0; xidx < occupancy.GetNumXCells(); xidx++) 116 | { 117 | for (int64_t yidx = 0; yidx < occupancy.GetNumYCells(); yidx++) 118 | { 119 | for (int64_t zidx = 0; zidx < occupancy.GetNumZCells(); zidx++) 120 | { 121 | // Check grid querying 122 | const auto occupancy_query = 123 | occupancy.GetIndexImmutable(xidx, yidx, zidx); 124 | // Check grid values 125 | const float cmap_occupancy = occupancy_query.Value().Occupancy(); 126 | // Check the bottom cells 127 | if (zidx == 0) 128 | { 129 | check_equal(cmap_occupancy, 1.0f); 130 | } 131 | // Check a few "seen empty" cells 132 | if ((xidx == 3) && (yidx >= 3) && (zidx >= 1)) 133 | { 134 | check_equal(cmap_occupancy, 0.0f); 135 | } 136 | if ((xidx >= 3) && (yidx == 3) && (zidx >= 1)) 137 | { 138 | check_equal(cmap_occupancy, 0.0f); 139 | } 140 | // Check a few "seen filled" cells 141 | if ((xidx == 4) && (yidx >= 4) && (zidx >= 1)) 142 | { 143 | check_equal(cmap_occupancy, 1.0f); 144 | } 145 | if ((xidx >= 4) && (yidx == 4) && (zidx >= 1)) 146 | { 147 | check_equal(cmap_occupancy, 1.0f); 148 | } 149 | // Check shadowed cells 150 | if ((xidx > 4) && (yidx > 4) && (zidx >= 1)) 151 | { 152 | check_equal(cmap_occupancy, 0.5f); 153 | } 154 | } 155 | } 156 | } 157 | } 158 | 159 | void test_pointcloud_voxelization( 160 | const std::function& display_fn) 161 | { 162 | // Make the static environment 163 | const Eigen::Isometry3d X_WG(Eigen::Translation3d(-1.0, -1.0, -1.0)); 164 | // Grid 2m in each axis 165 | const double x_size = 2.0; 166 | const double y_size = 2.0; 167 | const double z_size = 2.0; 168 | // 1/4 meter resolution, so 8 cells/axis 169 | const double grid_resolution = 0.25; 170 | const double step_size_multiplier = 0.5; 171 | const voxelized_geometry_tools::CollisionCell default_cell(0.0f); 172 | const common_robotics_utilities::voxel_grid::GridSizes grid_size( 173 | grid_resolution, x_size, y_size, z_size); 174 | voxelized_geometry_tools::CollisionMap static_environment( 175 | X_WG, "world", grid_size, default_cell); 176 | // Set the bottom cells filled 177 | for (int64_t xidx = 0; xidx < static_environment.GetNumXCells(); xidx++) 178 | { 179 | for (int64_t yidx = 0; yidx < static_environment.GetNumYCells(); yidx++) 180 | { 181 | static_environment.SetIndex( 182 | xidx, yidx, 0, voxelized_geometry_tools::CollisionCell(1.0f)); 183 | } 184 | } 185 | // Make some test pointclouds 186 | // Make the physical->optical frame transform 187 | const Eigen::Isometry3d X_CO = Eigen::Translation3d(0.0, 0.0, 0.0) * 188 | Eigen::Quaterniond(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitZ()) * 189 | Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX())); 190 | // Camera 1 pose 191 | const Eigen::Isometry3d X_WC1(Eigen::Translation3d(-2.0, 0.0, 0.0)); 192 | const Eigen::Isometry3d X_WC1O = X_WC1 * X_CO; 193 | PointCloudWrapperSharedPtr cam1_cloud(new VectorVector3dPointCloudWrapper()); 194 | static_cast(cam1_cloud.get()) 195 | ->SetPointCloudOriginTransform(X_WC1O); 196 | for (double x = -2.0; x <= 2.0; x += 0.03125) 197 | { 198 | for (double y = -2.0; y <= 2.0; y += 0.03125) 199 | { 200 | const double z = (x <= 0.0) ? 2.125 : 4.0; 201 | static_cast(cam1_cloud.get())->PushBack( 202 | Eigen::Vector3d(x, y, z)); 203 | } 204 | } 205 | // Camera 2 pose 206 | const Eigen::Isometry3d X_WC2 = Eigen::Translation3d(0.0, -2.0, 0.0) * 207 | Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); 208 | const Eigen::Isometry3d X_WC2O = X_WC2 * X_CO; 209 | PointCloudWrapperSharedPtr cam2_cloud(new VectorVector3dPointCloudWrapper()); 210 | static_cast(cam2_cloud.get()) 211 | ->SetPointCloudOriginTransform(X_WC2O); 212 | for (double x = -2.0; x <= 2.0; x += 0.03125) 213 | { 214 | for (double y = -2.0; y <= 2.0; y += 0.03125) 215 | { 216 | const double z = (x >= 0.0) ? 2.125 : 4.0; 217 | static_cast(cam2_cloud.get())->PushBack( 218 | Eigen::Vector3d(x, y, z)); 219 | } 220 | } 221 | // Make control parameters 222 | // We require that 100% of points from the camera see through to see a voxel 223 | // as free. 224 | const double percent_seen_free = 1.0; 225 | // We don't worry about outliers 226 | const int32_t outlier_points_threshold = 1; 227 | // We only need one camera to see a voxel as free. 228 | const int32_t num_cameras_seen_free = 1; 229 | const PointCloudVoxelizationFilterOptions filter_options( 230 | percent_seen_free, outlier_points_threshold, num_cameras_seen_free); 231 | // Voxelize them 232 | MarkerArray display_markers; 233 | const ColorRGBA free_color 234 | = common_robotics_utilities::color_builder 235 | ::MakeFromFloatColors( 236 | 0.0, 0.25, 0.0, 0.5); 237 | const ColorRGBA filled_color 238 | = common_robotics_utilities::color_builder 239 | ::MakeFromFloatColors( 240 | 0.25, 0.0, 0.0, 0.5); 241 | const ColorRGBA unknown_color 242 | = common_robotics_utilities::color_builder 243 | ::MakeFromFloatColors( 244 | 0.0, 0.0, 0.25, 0.5); 245 | // Voxelizer options (leave as default) 246 | const std::map options; 247 | try 248 | { 249 | std::cout << "Trying CUDA PointCloud Voxelizer..." << std::endl; 250 | std::unique_ptr voxelizer( 251 | new CudaPointCloudVoxelizer(options)); 252 | const auto cuda_voxelized = voxelizer->VoxelizePointClouds( 253 | static_environment, step_size_multiplier, filter_options, 254 | {cam1_cloud, cam2_cloud}); 255 | check_voxelization(cuda_voxelized); 256 | auto environment_display = 257 | voxelized_geometry_tools::ros_interface 258 | ::ExportForSeparateDisplay( 259 | cuda_voxelized, filled_color, free_color, unknown_color); 260 | for (auto& marker : environment_display.markers) 261 | { 262 | marker.ns = "cuda_" + marker.ns; 263 | } 264 | display_markers.markers.insert(display_markers.markers.end(), 265 | environment_display.markers.begin(), 266 | environment_display.markers.end()); 267 | } 268 | catch (const std::runtime_error& ex) 269 | { 270 | std::cerr << ex.what() << std::endl; 271 | } 272 | try 273 | { 274 | std::cout << "Trying OpenCL PointCloud Voxelizer..." << std::endl; 275 | std::unique_ptr voxelizer( 276 | new OpenCLPointCloudVoxelizer(options)); 277 | const auto opencl_voxelized = voxelizer->VoxelizePointClouds( 278 | static_environment, step_size_multiplier, filter_options, 279 | {cam1_cloud, cam2_cloud}); 280 | check_voxelization(opencl_voxelized); 281 | auto environment_display = 282 | voxelized_geometry_tools::ros_interface 283 | ::ExportForSeparateDisplay( 284 | opencl_voxelized, filled_color, free_color, unknown_color); 285 | for (auto& marker : environment_display.markers) 286 | { 287 | marker.ns = "opencl_" + marker.ns; 288 | } 289 | display_markers.markers.insert(display_markers.markers.end(), 290 | environment_display.markers.begin(), 291 | environment_display.markers.end()); 292 | } 293 | catch (const std::runtime_error& ex) 294 | { 295 | std::cerr << ex.what() << std::endl; 296 | } 297 | try 298 | { 299 | std::cout << "Trying CPU PointCloud Voxelizer..." << std::endl; 300 | std::unique_ptr voxelizer( 301 | new CpuPointCloudVoxelizer(options)); 302 | const auto cpu_voxelized = voxelizer->VoxelizePointClouds( 303 | static_environment, step_size_multiplier, filter_options, 304 | {cam1_cloud, cam2_cloud}); 305 | check_voxelization(cpu_voxelized); 306 | auto environment_display = 307 | voxelized_geometry_tools::ros_interface 308 | ::ExportForSeparateDisplay( 309 | cpu_voxelized, filled_color, free_color, unknown_color); 310 | for (auto& marker : environment_display.markers) 311 | { 312 | marker.ns = "cpu_" + marker.ns; 313 | } 314 | display_markers.markers.insert(display_markers.markers.end(), 315 | environment_display.markers.begin(), 316 | environment_display.markers.end()); 317 | } 318 | catch (const std::runtime_error&) 319 | { 320 | throw std::runtime_error("CPU PointCloud Voxelizer is not available"); 321 | } 322 | // Draw the results 323 | display_fn(display_markers); 324 | } 325 | } // namespace 326 | 327 | int main(int argc, char** argv) 328 | { 329 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 330 | rclcpp::init(argc, argv); 331 | auto node = std::make_shared("pointcloud_voxelization_test"); 332 | auto display_pub = node->create_publisher( 333 | "pointcloud_occupancy", rclcpp::QoS(1).transient_local()); 334 | const std::function& display_fn 335 | = [&] (const MarkerArray& markers) 336 | { 337 | display_pub->publish(markers); 338 | }; 339 | const int32_t iterations = 340 | static_cast(node->declare_parameter( 341 | "iterations", rclcpp::ParameterValue(1)).get()); 342 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 343 | ros::init(argc, argv, "pointcloud_voxelization_test"); 344 | ros::NodeHandle nh; 345 | ros::NodeHandle nhp("~"); 346 | ros::Publisher display_pub = nh.advertise( 347 | "pointcloud_occupancy", 1, true); 348 | const std::function& display_fn 349 | = [&] (const MarkerArray& markers) 350 | { 351 | display_pub.publish(markers); 352 | }; 353 | const int32_t iterations = nhp.param(std::string("iterations"), 1); 354 | #endif 355 | 356 | for (int32_t iter = 0; iter < iterations; iter++) 357 | { 358 | test_pointcloud_voxelization(display_fn); 359 | } 360 | 361 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 362 | rclcpp::spin(node); 363 | rclcpp::shutdown(); 364 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 365 | ros::spin(); 366 | #endif 367 | return 0; 368 | } 369 | -------------------------------------------------------------------------------- /example/spatial_segments.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 7 | #include 8 | #include 9 | #include 10 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 11 | #include 12 | #include 13 | #include 14 | #else 15 | #error "Undefined or unknown VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION" 16 | #endif 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 24 | using ColorRGBA = std_msgs::msg::ColorRGBA; 25 | using Marker = visualization_msgs::msg::Marker; 26 | using MarkerArray = visualization_msgs::msg::MarkerArray; 27 | 28 | using Duration = rclcpp::Duration; 29 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 30 | using ColorRGBA = std_msgs::ColorRGBA; 31 | using Marker = visualization_msgs::Marker; 32 | using MarkerArray = visualization_msgs::MarkerArray; 33 | 34 | using Duration = ros::Duration; 35 | #endif 36 | 37 | namespace 38 | { 39 | void test_spatial_segments( 40 | const std::function& display_fn) 41 | { 42 | const double res = 1.0; 43 | const int64_t x_size = 100; 44 | const int64_t y_size = 100; 45 | const int64_t z_size = 50; 46 | const Eigen::Isometry3d origin_transform 47 | = Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond( 48 | Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ())); 49 | const common_robotics_utilities::voxel_grid::GridSizes grid_sizes( 50 | res, x_size, y_size, z_size); 51 | const voxelized_geometry_tools::TaggedObjectCollisionCell empty_cell(0.0, 0u); 52 | voxelized_geometry_tools::TaggedObjectCollisionMap tocmap( 53 | origin_transform, "world", grid_sizes, empty_cell); 54 | 55 | const voxelized_geometry_tools::TaggedObjectCollisionCell filled_1(1.0, 1u); 56 | const voxelized_geometry_tools::TaggedObjectCollisionCell filled_2(1.0, 2u); 57 | 58 | for (int64_t x_idx = 0; x_idx < tocmap.GetNumXCells(); x_idx++) 59 | { 60 | for (int64_t y_idx = 0; y_idx < tocmap.GetNumYCells(); y_idx++) 61 | { 62 | for (int64_t z_idx = 0; z_idx < tocmap.GetNumZCells(); z_idx++) 63 | { 64 | if ((x_idx < 10) 65 | || (y_idx < 10) 66 | || (x_idx >= tocmap.GetNumXCells() - 10) 67 | || (y_idx >= tocmap.GetNumYCells() - 10)) 68 | { 69 | tocmap.SetIndex(x_idx, y_idx, z_idx, filled_1); 70 | } 71 | else if ((x_idx >= 40) && (y_idx >= 40) && (x_idx < 60) && (y_idx < 60)) 72 | { 73 | tocmap.SetIndex(x_idx, y_idx, z_idx, filled_2); 74 | } 75 | 76 | if (((x_idx >= 45) && (x_idx < 55)) || ((y_idx >= 45) && (y_idx < 55))) 77 | { 78 | tocmap.SetIndex(x_idx, y_idx, z_idx, empty_cell); 79 | } 80 | } 81 | } 82 | } 83 | 84 | MarkerArray display_markers; 85 | Marker env_marker = 86 | voxelized_geometry_tools::ros_interface::ExportForDisplay(tocmap); 87 | env_marker.id = 1; 88 | env_marker.ns = "environment"; 89 | display_markers.markers.push_back(env_marker); 90 | Marker components_marker = voxelized_geometry_tools::ros_interface 91 | ::ExportConnectedComponentsForDisplay(tocmap, false); 92 | components_marker.id = 1; 93 | components_marker.ns = "environment_components"; 94 | display_markers.markers.push_back(components_marker); 95 | 96 | const float oob_value = std::numeric_limits::infinity(); 97 | const auto parallelism = 98 | common_robotics_utilities::parallelism::DegreeOfParallelism::None(); 99 | const bool unknown_is_filled = true; 100 | 101 | const voxelized_geometry_tools::SignedDistanceFieldGenerationParameters 102 | no_border_sdf_parameters( 103 | oob_value, parallelism, unknown_is_filled, false); 104 | const voxelized_geometry_tools::SignedDistanceFieldGenerationParameters 105 | virtual_border_sdf_parameters( 106 | oob_value, parallelism, unknown_is_filled, true); 107 | 108 | const double connected_threshold = 1.75; 109 | const uint32_t number_of_spatial_segments_manual_border = 110 | tocmap.UpdateSpatialSegments( 111 | connected_threshold, no_border_sdf_parameters); 112 | 113 | std::cout << "Identified " << number_of_spatial_segments_manual_border 114 | << " spatial segments via SDF->maxima map->connected components" 115 | << " (no border added)" 116 | << std::endl; 117 | 118 | for (uint32_t object_id = 0u; object_id <= 4u; object_id++) 119 | { 120 | for (uint32_t spatial_segment = 1u; 121 | spatial_segment <= number_of_spatial_segments_manual_border; 122 | spatial_segment++) 123 | { 124 | Marker segment_marker = voxelized_geometry_tools::ros_interface 125 | ::ExportSpatialSegmentForDisplay(tocmap, object_id, spatial_segment); 126 | if (segment_marker.points.size() > 0) 127 | { 128 | segment_marker.ns += "_no_border"; 129 | display_markers.markers.push_back(segment_marker); 130 | } 131 | } 132 | } 133 | 134 | const uint32_t number_of_spatial_segments_virtual_border = 135 | tocmap.UpdateSpatialSegments( 136 | connected_threshold, virtual_border_sdf_parameters); 137 | std::cout << "Identified " << number_of_spatial_segments_virtual_border 138 | << " spatial segments via SDF->maxima map->connected components" 139 | << " (virtual border added)" 140 | << std::endl; 141 | 142 | for (uint32_t object_id = 0u; object_id <= 4u; object_id++) 143 | { 144 | for (uint32_t spatial_segment = 1u; 145 | spatial_segment <= number_of_spatial_segments_virtual_border; 146 | spatial_segment++) 147 | { 148 | Marker segment_marker = voxelized_geometry_tools::ros_interface 149 | ::ExportSpatialSegmentForDisplay(tocmap, object_id, spatial_segment); 150 | if (segment_marker.points.size() > 0) 151 | { 152 | segment_marker.ns += "_virtual_border"; 153 | display_markers.markers.push_back(segment_marker); 154 | } 155 | } 156 | } 157 | 158 | const auto sdf = tocmap.ExtractSignedDistanceFieldFloat( 159 | std::vector(), no_border_sdf_parameters); 160 | Marker sdf_marker = 161 | voxelized_geometry_tools::ros_interface::ExportSDFForDisplay(sdf, 1.0f); 162 | sdf_marker.id = 1; 163 | sdf_marker.ns = "environment_sdf_no_border"; 164 | display_markers.markers.push_back(sdf_marker); 165 | 166 | const auto virtual_border_sdf = tocmap.ExtractSignedDistanceFieldFloat( 167 | std::vector(), virtual_border_sdf_parameters); 168 | Marker virtual_border_sdf_marker = voxelized_geometry_tools::ros_interface 169 | ::ExportSDFForDisplay(virtual_border_sdf, 1.0f); 170 | virtual_border_sdf_marker.id = 1; 171 | virtual_border_sdf_marker.ns = "environment_sdf_virtual_border"; 172 | display_markers.markers.push_back(virtual_border_sdf_marker); 173 | 174 | // Make extrema markers 175 | const auto maxima_map = virtual_border_sdf.ComputeLocalExtremaMap(); 176 | for (int64_t x_idx = 0; x_idx < maxima_map.GetNumXCells(); x_idx++) 177 | { 178 | for (int64_t y_idx = 0; y_idx < maxima_map.GetNumYCells(); y_idx++) 179 | { 180 | for (int64_t z_idx = 0; z_idx < maxima_map.GetNumZCells(); z_idx++) 181 | { 182 | const Eigen::Vector4d location 183 | = maxima_map.GridIndexToLocation(x_idx, y_idx, z_idx); 184 | const Eigen::Vector3d extrema = 185 | maxima_map.GetIndexImmutable(x_idx, y_idx, z_idx).Value(); 186 | if (!std::isinf(extrema.x()) 187 | && !std::isinf(extrema.y()) 188 | && !std::isinf(extrema.z())) 189 | { 190 | const double distance = (extrema - location.block<3, 1>(0, 0)).norm(); 191 | if (distance < sdf.GetResolution()) 192 | { 193 | Marker maxima_rep; 194 | // Populate the header 195 | maxima_rep.header.frame_id = "world"; 196 | // Populate the options 197 | maxima_rep.ns = "extrema"; 198 | maxima_rep.id = 199 | static_cast(sdf.HashDataIndex(x_idx, y_idx, z_idx)); 200 | maxima_rep.action = Marker::ADD; 201 | maxima_rep.lifetime = Duration(0, 0); 202 | maxima_rep.frame_locked = false; 203 | maxima_rep.pose.position = 204 | common_robotics_utilities::ros_conversions 205 | ::EigenVector4dToGeometryPoint(location); 206 | maxima_rep.pose.orientation = 207 | common_robotics_utilities::ros_conversions 208 | ::EigenQuaterniondToGeometryQuaternion( 209 | Eigen::Quaterniond::Identity()); 210 | maxima_rep.type = Marker::SPHERE; 211 | maxima_rep.scale.x = sdf.GetResolution(); 212 | maxima_rep.scale.y = sdf.GetResolution(); 213 | maxima_rep.scale.z = sdf.GetResolution(); 214 | maxima_rep.color = common_robotics_utilities::color_builder 215 | ::MakeFromFloatColors(1.0, 0.5, 0.0, 1.0); 216 | display_markers.markers.push_back(maxima_rep); 217 | } 218 | } 219 | else 220 | { 221 | std::cout << "Encountered inf extrema @ (" 222 | << x_idx << "," << y_idx << "," << z_idx << ")" 223 | << std::endl; 224 | } 225 | } 226 | } 227 | } 228 | 229 | std::cout << "(0,0,0) " 230 | << common_robotics_utilities::print::Print( 231 | virtual_border_sdf.GetIndexCoarseGradient( 232 | 0, 0, 0, true).Value()) 233 | << std::endl; 234 | std::cout << "(1,1,1) " 235 | << common_robotics_utilities::print::Print( 236 | virtual_border_sdf.GetIndexCoarseGradient( 237 | 1, 1, 1, true).Value()) 238 | << std::endl; 239 | std::cout << "(2,2,2) " 240 | << common_robotics_utilities::print::Print( 241 | virtual_border_sdf.GetIndexCoarseGradient( 242 | 2, 2, 2, true).Value()) 243 | << std::endl; 244 | std::cout << "(0,0,0) " 245 | << common_robotics_utilities::print::Print( 246 | virtual_border_sdf.GetIndexFineGradient(0, 0, 0, res).Value()) 247 | << std::endl; 248 | std::cout << "(1,1,1) " 249 | << common_robotics_utilities::print::Print( 250 | virtual_border_sdf.GetIndexFineGradient(1, 1, 1, res).Value()) 251 | << std::endl; 252 | std::cout << "(2,2,2) " 253 | << common_robotics_utilities::print::Print( 254 | virtual_border_sdf.GetIndexFineGradient(2, 2, 2, res).Value()) 255 | << std::endl; 256 | std::cout << "(0,0,0) " 257 | << common_robotics_utilities::print::Print( 258 | maxima_map.GetIndexImmutable(0, 0, 0).Value()) 259 | << std::endl; 260 | std::cout << "(1,1,1) " 261 | << common_robotics_utilities::print::Print( 262 | maxima_map.GetIndexImmutable(1, 1, 1).Value()) 263 | << std::endl; 264 | std::cout << "(2,2,2) " 265 | << common_robotics_utilities::print::Print( 266 | maxima_map.GetIndexImmutable(2, 2, 2).Value()) 267 | << std::endl; 268 | 269 | display_fn(display_markers); 270 | } 271 | } // namespace 272 | 273 | int main(int argc, char** argv) 274 | { 275 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 276 | rclcpp::init(argc, argv); 277 | auto node = std::make_shared("compute_spatial_segments_test"); 278 | auto display_pub = node->create_publisher( 279 | "display_test_voxel_grid", rclcpp::QoS(1).transient_local()); 280 | const std::function& display_fn 281 | = [&] (const MarkerArray& markers) 282 | { 283 | display_pub->publish(markers); 284 | }; 285 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 286 | ros::init(argc, argv, "compute_spatial_segments_test"); 287 | ros::NodeHandle nh; 288 | ros::Publisher display_pub = nh.advertise( 289 | "display_test_voxel_grid", 1, true); 290 | const std::function& display_fn 291 | = [&] (const MarkerArray& markers) 292 | { 293 | display_pub.publish(markers); 294 | }; 295 | #endif 296 | 297 | test_spatial_segments(display_fn); 298 | 299 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 300 | rclcpp::spin(node); 301 | rclcpp::shutdown(); 302 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 303 | ros::spin(); 304 | #endif 305 | return 0; 306 | } 307 | -------------------------------------------------------------------------------- /example/tutorial.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 6 | #include 7 | #include 8 | #include 9 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 10 | #include 11 | #include 12 | #include 13 | #else 14 | #error "Undefined or unknown VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION" 15 | #endif 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | int main(int argc, char** argv) 23 | { 24 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 25 | using ColorRGBA = std_msgs::msg::ColorRGBA; 26 | using Marker = visualization_msgs::msg::Marker; 27 | 28 | rclcpp::init(argc, argv); 29 | auto node = std::make_shared("sdf_tools_tutorial"); 30 | auto visualization_pub = node->create_publisher( 31 | "sdf_tools_tutorial_visualization", rclcpp::QoS(1).transient_local()); 32 | const std::function display_fn 33 | = [&] (const Marker& marker) 34 | { 35 | visualization_pub->publish(marker); 36 | }; 37 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 38 | using ColorRGBA = std_msgs::ColorRGBA; 39 | using Marker = visualization_msgs::Marker; 40 | 41 | // Make a ROS node, which we'll use to publish copies of the data in the 42 | // CollisionMap and SDF and Rviz markers that allow us to visualize them. 43 | ros::init(argc, argv, "sdf_tools_tutorial"); 44 | // Get a handle to the current node 45 | ros::NodeHandle nh; 46 | // Make a publisher for visualization messages 47 | ros::Publisher visualization_pub = nh.advertise( 48 | "sdf_tools_tutorial_visualization", 1, true); 49 | const std::function display_fn 50 | = [&] (const Marker& marker) 51 | { 52 | visualization_pub.publish(marker); 53 | }; 54 | #endif 55 | 56 | // In preparation, we want to set a couple common paramters 57 | const double resolution = 0.25; 58 | const double x_size = 10.0; 59 | const double y_size = 10.0; 60 | const double z_size = 10.0; 61 | const common_robotics_utilities::voxel_grid::GridSizes grid_sizes( 62 | resolution, x_size, y_size, z_size); 63 | // Let's center the grid around the origin 64 | const Eigen::Translation3d origin_translation(-5.0, -5.0, -5.0); 65 | const Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0); 66 | const Eigen::Isometry3d origin_transform = 67 | origin_translation * origin_rotation; 68 | const std::string frame = "tutorial_frame"; 69 | 70 | /////////////////////////////////// 71 | //// Let's make a CollisionMap //// 72 | /////////////////////////////////// 73 | 74 | // We pick a reasonable default and out-of-bounds value 75 | // By initializing like this, the component value is automatically set to 0. 76 | const voxelized_geometry_tools::CollisionCell default_cell(0.0); 77 | // First, let's make the container 78 | voxelized_geometry_tools::CollisionMap collision_map( 79 | origin_transform, frame, grid_sizes, default_cell); 80 | 81 | // Let's set some values 82 | // This is how you should iterate through the 3D grid's cells 83 | for (int64_t x_index = 0; x_index < collision_map.GetNumXCells(); x_index++) 84 | { 85 | for (int64_t y_index = 0; y_index < collision_map.GetNumYCells(); y_index++) 86 | { 87 | for (int64_t z_index = 0; z_index < collision_map.GetNumZCells(); 88 | z_index++) 89 | { 90 | // Let's make the bottom corner (low x, low y, low z) an object 91 | if ((x_index < (collision_map.GetNumXCells() / 2)) && 92 | (y_index < (collision_map.GetNumYCells() / 2)) && 93 | (z_index < (collision_map.GetNumZCells() / 2))) 94 | { 95 | // Occupancy values > 0.5 are obstacles 96 | const voxelized_geometry_tools::CollisionCell obstacle_cell(1.0); 97 | collision_map.SetIndex(x_index, y_index, z_index, obstacle_cell); 98 | } 99 | } 100 | } 101 | } 102 | 103 | // We can also set by location - occupancy values > 0.5 are obstacles 104 | const voxelized_geometry_tools::CollisionCell obstacle_cell(1.0); 105 | collision_map.SetLocation(0.0, 0.0, 0.0, obstacle_cell); 106 | 107 | // Let's get some values 108 | // We can query by index 109 | int64_t x_index = 10; 110 | int64_t y_index = 10; 111 | int64_t z_index = 10; 112 | const auto index_query = 113 | collision_map.GetIndexImmutable(x_index, y_index, z_index); 114 | 115 | // Is it in the grid? 116 | if (index_query) 117 | { 118 | std::cout << "Index query result - stored value " 119 | << index_query.Value().Occupancy() << " (occupancy) " 120 | << index_query.Value().Component() << " (component)" << std::endl; 121 | } 122 | 123 | // Or we can query by location 124 | double x_location = 0.0; 125 | double y_location = 0.0; 126 | double z_location = 0.0; 127 | const auto location_query = 128 | collision_map.GetLocationImmutable(x_location, y_location, z_location); 129 | 130 | // Is it in the grid? 131 | if (location_query) 132 | { 133 | std::cout << "Location query result - stored value " 134 | << location_query.Value().Occupancy() << " (occupancy) " 135 | << location_query.Value().Component() << " (component)" 136 | << std::endl; 137 | } 138 | 139 | // Let's compute connected components 140 | const auto cc_start_time = std::chrono::steady_clock::now(); 141 | const uint32_t num_connected_components = 142 | collision_map.UpdateConnectedComponents(); 143 | const auto cc_end_time = std::chrono::steady_clock::now(); 144 | const double cc_elapsed = 145 | std::chrono::duration(cc_end_time - cc_start_time).count(); 146 | std::cout << "Connected components took " << cc_elapsed << " seconds" 147 | << std::endl; 148 | std::cout << "There are " << num_connected_components 149 | << " connected components in the grid" << std::endl; 150 | 151 | // Let's display the results to Rviz 152 | // First, the CollisionMap itself 153 | // We need to provide colors to use 154 | const float display_alpha = 1.0; 155 | ColorRGBA collision_color; 156 | collision_color.r = 1.0; 157 | collision_color.g = 0.0; 158 | collision_color.b = 0.0; 159 | collision_color.a = display_alpha; 160 | ColorRGBA free_color; 161 | free_color.r = 0.0; 162 | free_color.g = 1.0; 163 | free_color.b = 0.0; 164 | free_color.a = display_alpha; 165 | ColorRGBA unknown_color; 166 | unknown_color.r = 1.0; 167 | unknown_color.g = 1.0; 168 | unknown_color.b = 0.0; 169 | unknown_color.a = display_alpha; 170 | Marker collision_map_marker = 171 | voxelized_geometry_tools::ros_interface::ExportForDisplay( 172 | collision_map, collision_color, free_color, unknown_color); 173 | // To be safe, you'll need to set these yourself. The namespace (ns) value 174 | // should distinguish between different things being displayed while the id 175 | // value lets you have multiple versions of the same message at once. Always 176 | // set this to 1 if you only want one copy. 177 | collision_map_marker.ns = "collision_map"; 178 | collision_map_marker.id = 1; 179 | // Send it off for display 180 | display_fn(collision_map_marker); 181 | // Now, let's draw the connected components 182 | // Generally, you don't want a special color for unknown [P(occupancy) = 0.5] 183 | // components. 184 | Marker connected_components_marker = 185 | voxelized_geometry_tools::ros_interface 186 | ::ExportConnectedComponentsForDisplay(collision_map, false); 187 | connected_components_marker.ns = "connected_components"; 188 | connected_components_marker.id = 1; 189 | display_fn(connected_components_marker); 190 | 191 | /////////////////////////// 192 | //// Let's make an SDF //// 193 | /////////////////////////// 194 | 195 | // We pick a reasonable out-of-bounds value 196 | const float oob_value = std::numeric_limits::infinity(); 197 | // Enable parallelism in SDF generation 198 | const auto parallelism = 199 | common_robotics_utilities::parallelism::DegreeOfParallelism::FromOmp(); 200 | // Treat cells with unknown occupancy as if they were filled 201 | const bool unknown_is_filled = true; 202 | // Don't add a virtual border 203 | const bool add_virtual_border = false; 204 | const voxelized_geometry_tools::SignedDistanceFieldGenerationParameters 205 | sdf_parameters( 206 | oob_value, parallelism, unknown_is_filled, add_virtual_border); 207 | // We start by extracting the SDF from the CollisionMap 208 | const auto sdf_start_time = std::chrono::steady_clock::now(); 209 | const auto sdf = 210 | collision_map.ExtractSignedDistanceFieldFloat(sdf_parameters); 211 | const auto sdf_end_time = std::chrono::steady_clock::now(); 212 | const double sdf_elapsed = 213 | std::chrono::duration(sdf_end_time - sdf_start_time).count(); 214 | std::cout << "SDF generation took " << sdf_elapsed << " seconds" << std::endl; 215 | const auto sdf_extrema = sdf.GetMinimumMaximum(); 216 | std::cout << "Maximum distance in the SDF: " << sdf_extrema.Maximum() 217 | << ", minimum distance in the SDF: " << sdf_extrema.Minimum() 218 | << std::endl; 219 | 220 | // Let's get some values 221 | const auto index_sdf_query = sdf.GetIndexImmutable(x_index, y_index, z_index); 222 | // Is it in the grid? 223 | if (index_sdf_query) 224 | { 225 | std::cout << "Index query result - stored distance " 226 | << index_sdf_query.Value() << std::endl; 227 | } 228 | const auto location_sdf_query = 229 | sdf.GetLocationImmutable(x_location, y_location, z_location); 230 | if (location_sdf_query) 231 | { 232 | std::cout << "Location query result - stored distance " 233 | << location_sdf_query.Value() << std::endl; 234 | } 235 | 236 | // Let's get some gradients 237 | // Usually, you want to enable 'edge gradients' i.e. gradients for cells on 238 | // the edge of the grid that don't have 6 neighbors. 239 | const auto index_gradient_query = 240 | sdf.GetIndexCoarseGradient(x_index, y_index, z_index, true); 241 | if (index_gradient_query) 242 | { 243 | std::cout << "Index gradient query result - gradient " 244 | << common_robotics_utilities::print::Print( 245 | index_gradient_query.Value()) 246 | << std::endl; 247 | } 248 | const auto location_gradient_query = 249 | sdf.GetLocationCoarseGradient(x_location, y_location, z_location, true); 250 | if (location_gradient_query) 251 | { 252 | std::cout << "Location gradient query result - gradient " 253 | << common_robotics_utilities::print::Print( 254 | location_gradient_query.Value()) 255 | << std::endl; 256 | } 257 | 258 | // Let's display the results to Rviz 259 | Marker sdf_marker = 260 | voxelized_geometry_tools::ros_interface::ExportSDFForDisplay( 261 | sdf, display_alpha); 262 | sdf_marker.ns = "sdf"; 263 | sdf_marker.id = 1; 264 | display_fn(sdf_marker); 265 | std::cout << "...done" << std::endl; 266 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 267 | rclcpp::spin(node); 268 | rclcpp::shutdown(); 269 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 270 | ros::spin(); 271 | #endif 272 | return 0; 273 | } 274 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/collision_map.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace voxelized_geometry_tools 27 | { 28 | VGT_NAMESPACE_BEGIN 29 | class CollisionCell 30 | { 31 | private: 32 | using DeserializedCollisionCell 33 | = common_robotics_utilities::serialization::Deserialized; 34 | 35 | common_robotics_utilities::utility 36 | ::CopyableMoveableAtomic 37 | occupancy_{0.0f}; 38 | common_robotics_utilities::utility 39 | ::CopyableMoveableAtomic 40 | component_{0u}; 41 | 42 | public: 43 | static uint64_t Serialize( 44 | const CollisionCell& cell, std::vector& buffer); 45 | 46 | static DeserializedCollisionCell Deserialize( 47 | const std::vector& buffer, const uint64_t starting_offset); 48 | 49 | CollisionCell() : occupancy_(0.0f), component_(0u) {} 50 | 51 | CollisionCell(const float occupancy) 52 | : occupancy_(occupancy), component_(0u) {} 53 | 54 | CollisionCell(const float occupancy, const uint32_t component) 55 | : occupancy_(occupancy), component_(component) {} 56 | 57 | float Occupancy() const { return occupancy_.load(); } 58 | 59 | uint32_t Component() const { return component_.load(); } 60 | 61 | void SetOccupancy(const float occupancy) { occupancy_.store(occupancy); } 62 | 63 | void SetComponent(const uint32_t component) { component_.store(component); } 64 | }; 65 | 66 | // Enforce that despite its members being std::atomics, CollisionCell has the 67 | // same size as before. 68 | static_assert( 69 | sizeof(CollisionCell) == (sizeof(float) * 2), 70 | "CollisionCell is larger than expected."); 71 | 72 | using CollisionCellSerializer 73 | = common_robotics_utilities::serialization::Serializer; 74 | using CollisionCellDeserializer 75 | = common_robotics_utilities::serialization::Deserializer; 76 | 77 | class CollisionMap 78 | : public common_robotics_utilities::voxel_grid 79 | ::VoxelGridBase> 80 | { 81 | private: 82 | using DeserializedCollisionMap 83 | = common_robotics_utilities::serialization::Deserialized; 84 | 85 | uint32_t number_of_components_ = 0u; 86 | std::string frame_; 87 | common_robotics_utilities::utility::CopyableMoveableAtomic 88 | components_valid_{false}; 89 | 90 | /// Implement the VoxelGridBase interface. 91 | 92 | /// We need to implement cloning. 93 | std::unique_ptr>> 95 | DoClone() const override; 96 | 97 | /// We need to serialize the frame and locked flag. 98 | uint64_t DerivedSerializeSelf( 99 | std::vector& buffer, 100 | const CollisionCellSerializer& value_serializer) const override; 101 | 102 | /// We need to deserialize the frame and locked flag. 103 | uint64_t DerivedDeserializeSelf( 104 | const std::vector& buffer, const uint64_t starting_offset, 105 | const CollisionCellDeserializer& value_deserializer) override; 106 | 107 | /// Invalidate connected components on mutable access. 108 | bool OnMutableAccess(const int64_t x_index, 109 | const int64_t y_index, 110 | const int64_t z_index) override; 111 | 112 | /// Invalidate connected components on mutable raw access. 113 | bool OnMutableRawAccess() override; 114 | 115 | public: 116 | static uint64_t Serialize( 117 | const CollisionMap& map, std::vector& buffer); 118 | 119 | static DeserializedCollisionMap Deserialize( 120 | const std::vector& buffer, const uint64_t starting_offset); 121 | 122 | static void SaveToFile(const CollisionMap& map, 123 | const std::string& filepath, 124 | const bool compress); 125 | 126 | static CollisionMap LoadFromFile(const std::string& filepath); 127 | 128 | CollisionMap( 129 | const Eigen::Isometry3d& origin_transform, const std::string& frame, 130 | const common_robotics_utilities::voxel_grid::GridSizes& sizes, 131 | const CollisionCell& default_value) 132 | : CollisionMap( 133 | origin_transform, frame, sizes, default_value, default_value) {} 134 | 135 | CollisionMap( 136 | const std::string& frame, 137 | const common_robotics_utilities::voxel_grid::GridSizes& sizes, 138 | const CollisionCell& default_value) 139 | : CollisionMap(frame, sizes, default_value, default_value) {} 140 | 141 | CollisionMap( 142 | const Eigen::Isometry3d& origin_transform, const std::string& frame, 143 | const common_robotics_utilities::voxel_grid::GridSizes& sizes, 144 | const CollisionCell& default_value, const CollisionCell& oob_value) 145 | : common_robotics_utilities::voxel_grid 146 | ::VoxelGridBase>( 147 | origin_transform, sizes, default_value, oob_value), 148 | frame_(frame) 149 | { 150 | if (!HasUniformCellSize()) 151 | { 152 | throw std::invalid_argument( 153 | "Collision map cannot have non-uniform cell sizes"); 154 | } 155 | } 156 | 157 | CollisionMap( 158 | const std::string& frame, 159 | const common_robotics_utilities::voxel_grid::GridSizes& sizes, 160 | const CollisionCell& default_value, const CollisionCell& oob_value) 161 | : common_robotics_utilities::voxel_grid 162 | ::VoxelGridBase>( 163 | sizes, default_value, oob_value), 164 | frame_(frame) 165 | { 166 | if (!HasUniformCellSize()) 167 | { 168 | throw std::invalid_argument( 169 | "Collision map cannot have non-uniform cell sizes"); 170 | } 171 | } 172 | 173 | CollisionMap() 174 | : common_robotics_utilities::voxel_grid 175 | ::VoxelGridBase>() {} 176 | 177 | bool AreComponentsValid() const { return components_valid_.load(); } 178 | 179 | /// Use this with great care if you know the components are still/now valid. 180 | void ForceComponentsToBeValid() { components_valid_.store(true); } 181 | 182 | /// Use this to invalidate the current components. 183 | void ForceComponentsToBeInvalid() { components_valid_.store(false); } 184 | 185 | double GetResolution() const { return GetCellSizes().x(); } 186 | 187 | const std::string& GetFrame() const { return frame_; } 188 | 189 | void SetFrame(const std::string& frame) { frame_ = frame; } 190 | 191 | uint32_t UpdateConnectedComponents(); 192 | 193 | common_robotics_utilities::OwningMaybe 194 | GetNumConnectedComponents() const 195 | { 196 | if (components_valid_.load()) 197 | { 198 | return common_robotics_utilities::OwningMaybe( 199 | number_of_components_); 200 | } 201 | else 202 | { 203 | return common_robotics_utilities::OwningMaybe(); 204 | } 205 | } 206 | 207 | common_robotics_utilities::OwningMaybe IsSurfaceIndex( 208 | const common_robotics_utilities::voxel_grid::GridIndex& index) const; 209 | 210 | common_robotics_utilities::OwningMaybe IsSurfaceIndex( 211 | const int64_t x_index, const int64_t y_index, 212 | const int64_t z_index) const; 213 | 214 | common_robotics_utilities::OwningMaybe IsConnectedComponentSurfaceIndex( 215 | const common_robotics_utilities::voxel_grid::GridIndex& index) const; 216 | 217 | common_robotics_utilities::OwningMaybe IsConnectedComponentSurfaceIndex( 218 | const int64_t x_index, const int64_t y_index, 219 | const int64_t z_index) const; 220 | 221 | common_robotics_utilities::OwningMaybe CheckIfCandidateCorner( 222 | const double x, const double y, const double z) const; 223 | 224 | common_robotics_utilities::OwningMaybe CheckIfCandidateCorner3d( 225 | const Eigen::Vector3d& location) const; 226 | 227 | common_robotics_utilities::OwningMaybe CheckIfCandidateCorner4d( 228 | const Eigen::Vector4d& location) const; 229 | 230 | common_robotics_utilities::OwningMaybe CheckIfCandidateCorner( 231 | const common_robotics_utilities::voxel_grid::GridIndex& index) const; 232 | 233 | common_robotics_utilities::OwningMaybe CheckIfCandidateCorner( 234 | const int64_t x_index, const int64_t y_index, 235 | const int64_t z_index) const; 236 | 237 | enum COMPONENT_TYPES : uint8_t { FILLED_COMPONENTS=0x01, 238 | EMPTY_COMPONENTS=0x02, 239 | UNKNOWN_COMPONENTS=0x04 }; 240 | 241 | std::map> 243 | ExtractComponentSurfaces( 244 | const COMPONENT_TYPES component_types_to_extract) const; 245 | 246 | std::map> 248 | ExtractFilledComponentSurfaces() const; 249 | 250 | std::map> 252 | ExtractUnknownComponentSurfaces() const; 253 | 254 | std::map> 256 | ExtractEmptyComponentSurfaces() const; 257 | 258 | topology_computation::TopologicalInvariants ComputeComponentTopology( 259 | const COMPONENT_TYPES component_types_to_use, 260 | const common_robotics_utilities::utility::LoggingFunction& 261 | logging_fn = {}); 262 | 263 | template 264 | SignedDistanceField ExtractSignedDistanceField( 265 | const SignedDistanceFieldGenerationParameters& parameters) 266 | const 267 | { 268 | using common_robotics_utilities::voxel_grid::GridIndex; 269 | // Make the helper function 270 | const std::function 271 | is_filled_fn = [&] (const GridIndex& index) 272 | { 273 | const auto query = GetIndexImmutable(index); 274 | if (query) 275 | { 276 | const float occupancy = query.Value().Occupancy(); 277 | if (occupancy > 0.5) 278 | { 279 | // Mark as filled 280 | return true; 281 | } 282 | else if (parameters.UnknownIsFilled() && (occupancy == 0.5)) 283 | { 284 | // Mark as filled 285 | return true; 286 | } 287 | // Mark as free 288 | return false; 289 | } 290 | else 291 | { 292 | throw std::runtime_error("index out of grid bounds"); 293 | } 294 | }; 295 | return 296 | signed_distance_field_generation::internal::ExtractSignedDistanceField 297 | , ScalarType>( 298 | *this, is_filled_fn, GetFrame(), parameters); 299 | } 300 | 301 | SignedDistanceField ExtractSignedDistanceFieldDouble( 302 | const SignedDistanceFieldGenerationParameters& parameters) const; 303 | 304 | SignedDistanceField ExtractSignedDistanceFieldFloat( 305 | const SignedDistanceFieldGenerationParameters& parameters) const; 306 | }; 307 | VGT_NAMESPACE_END 308 | } // namespace voxelized_geometry_tools 309 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/cpu_pointcloud_voxelization.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace voxelized_geometry_tools 13 | { 14 | VGT_NAMESPACE_BEGIN 15 | namespace pointcloud_voxelization 16 | { 17 | /// CPU-based (OpenMP) implementation of pointcloud voxelizer. 18 | class CpuPointCloudVoxelizer : public PointCloudVoxelizationInterface { 19 | public: 20 | CpuPointCloudVoxelizer( 21 | const std::map& options, 22 | const LoggingFunction& logging_fn = {}); 23 | 24 | private: 25 | VoxelizerRuntime DoVoxelizePointClouds( 26 | const CollisionMap& static_environment, const double step_size_multiplier, 27 | const PointCloudVoxelizationFilterOptions& filter_options, 28 | const std::vector& pointclouds, 29 | CollisionMap& output_environment) const override; 30 | 31 | const common_robotics_utilities::parallelism::DegreeOfParallelism& 32 | Parallelism() const { return parallelism_; } 33 | 34 | common_robotics_utilities::parallelism::DegreeOfParallelism parallelism_; 35 | }; 36 | } // namespace pointcloud_voxelization 37 | VGT_NAMESPACE_END 38 | } // namespace voxelized_geometry_tools 39 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/cuda_voxelization_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace voxelized_geometry_tools 13 | { 14 | VGT_NAMESPACE_BEGIN 15 | namespace pointcloud_voxelization 16 | { 17 | namespace cuda_helpers 18 | { 19 | std::vector GetAvailableDevices(); 20 | 21 | std::unique_ptr 22 | MakeCudaVoxelizationHelper( 23 | const std::map& options, 24 | const LoggingFunction& logging_fn); 25 | } // namespace cuda_helpers 26 | } // namespace pointcloud_voxelization 27 | VGT_NAMESPACE_END 28 | } // namespace voxelized_geometry_tools 29 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/device_pointcloud_voxelization.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace voxelized_geometry_tools 19 | { 20 | VGT_NAMESPACE_BEGIN 21 | namespace pointcloud_voxelization 22 | { 23 | /// Base class for device-accelerated pointcloud voxelizers. 24 | class DevicePointCloudVoxelizer : public PointCloudVoxelizationInterface 25 | { 26 | public: 27 | virtual ~DevicePointCloudVoxelizer() {} 28 | 29 | protected: 30 | DevicePointCloudVoxelizer( 31 | const std::map& options, 32 | const LoggingFunction& logging_fn); 33 | 34 | void EnforceAvailable() const 35 | { 36 | if (!helper_interface_) 37 | { 38 | throw std::runtime_error( 39 | device_name_ + " is not available (feature was not built)"); 40 | } 41 | if (!helper_interface_->IsAvailable()) 42 | { 43 | throw std::runtime_error( 44 | device_name_ + " is not available (device cannot be used)"); 45 | } 46 | } 47 | 48 | std::unique_ptr helper_interface_; 49 | std::string device_name_; 50 | 51 | private: 52 | VoxelizerRuntime DoVoxelizePointClouds( 53 | const CollisionMap& static_environment, const double step_size_multiplier, 54 | const PointCloudVoxelizationFilterOptions& filter_options, 55 | const std::vector& pointclouds, 56 | CollisionMap& output_environment) const override; 57 | 58 | const common_robotics_utilities::parallelism::DegreeOfParallelism& 59 | DispatchParallelism() const { return dispatch_parallelism_; } 60 | 61 | common_robotics_utilities::parallelism::DegreeOfParallelism 62 | dispatch_parallelism_; 63 | }; 64 | 65 | /// Implementation of device-accelerated pointcloud voxelization for CUDA 66 | /// devices. See base classes for method documentation. 67 | class CudaPointCloudVoxelizer : public DevicePointCloudVoxelizer 68 | { 69 | public: 70 | CudaPointCloudVoxelizer( 71 | const std::map& options, 72 | const LoggingFunction& logging_fn = {}); 73 | }; 74 | 75 | /// Implementation of device-accelerated pointcloud voxelization for OpenCL 76 | /// devices. See base classes for method documentation. 77 | class OpenCLPointCloudVoxelizer : public DevicePointCloudVoxelizer 78 | { 79 | public: 80 | OpenCLPointCloudVoxelizer( 81 | const std::map& options, 82 | const LoggingFunction& logging_fn = {}); 83 | }; 84 | 85 | } // namespace pointcloud_voxelization 86 | VGT_NAMESPACE_END 87 | } // namespace voxelized_geometry_tools 88 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/device_voxelization_interface.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace voxelized_geometry_tools 13 | { 14 | VGT_NAMESPACE_BEGIN 15 | namespace pointcloud_voxelization 16 | { 17 | // Signature of basic text logging function. This is the same as CRU's logging 18 | // function type, but repeated here as some of our build systems are limited in 19 | // handling includes for device-specific compilers (e.g. NVCC). 20 | using LoggingFunction = std::function; 21 | 22 | // Wrapper for device name and the options necessary to retrieve it. 23 | class AvailableDevice 24 | { 25 | public: 26 | AvailableDevice( 27 | const std::string& device_name, 28 | const std::map& device_options) 29 | : device_name_(device_name), device_options_(device_options) {} 30 | 31 | const std::string& DeviceName() const { return device_name_; } 32 | 33 | const std::map& DeviceOptions() const 34 | { 35 | return device_options_; 36 | } 37 | 38 | private: 39 | std::string device_name_; 40 | std::map device_options_; 41 | }; 42 | 43 | // Helper for common option retrieval in device voxelizers. 44 | inline int32_t RetrieveOptionOrDefault( 45 | const std::map& options, const std::string& option, 46 | const int32_t default_value, const LoggingFunction& logging_fn) 47 | { 48 | auto found_itr = options.find(option); 49 | if (found_itr != options.end()) 50 | { 51 | const int32_t value = found_itr->second; 52 | if (logging_fn) 53 | { 54 | logging_fn( 55 | "Option [" + option + "] found, value [" + std::to_string(value) 56 | + "]"); 57 | } 58 | return value; 59 | } 60 | else 61 | { 62 | if (logging_fn) 63 | { 64 | logging_fn( 65 | "Option [" + option + "] not found, default [" 66 | + std::to_string(default_value) + "]"); 67 | } 68 | return default_value; 69 | } 70 | } 71 | 72 | // Opaque handle type to tracking grids. 73 | class TrackingGridsHandle 74 | { 75 | public: 76 | // Delete copy and move constructors and assignment operators. 77 | explicit TrackingGridsHandle(const TrackingGridsHandle&) = delete; 78 | explicit TrackingGridsHandle(TrackingGridsHandle&&) = delete; 79 | TrackingGridsHandle& operator=(const TrackingGridsHandle&) = delete; 80 | TrackingGridsHandle& operator=(TrackingGridsHandle&&) = delete; 81 | 82 | virtual ~TrackingGridsHandle() {} 83 | 84 | int64_t GetTrackingGridStartingOffset(const size_t index) const 85 | { 86 | return tracking_grid_starting_offsets_.at(index); 87 | } 88 | 89 | size_t GetNumTrackingGrids() const 90 | { 91 | return tracking_grid_starting_offsets_.size(); 92 | } 93 | 94 | int64_t NumCellsPerGrid() const { return num_cells_per_grid_; } 95 | 96 | protected: 97 | TrackingGridsHandle( 98 | const std::vector& tracking_grid_starting_offsets, 99 | const int64_t num_cells_per_grid) 100 | : tracking_grid_starting_offsets_(tracking_grid_starting_offsets), 101 | num_cells_per_grid_(num_cells_per_grid) {} 102 | 103 | private: 104 | std::vector tracking_grid_starting_offsets_; 105 | int64_t num_cells_per_grid_ = 0; 106 | }; 107 | 108 | // Opaque handle type to filter grid. 109 | class FilterGridHandle 110 | { 111 | public: 112 | // Delete copy and move constructors and assignment operators. 113 | explicit FilterGridHandle(const FilterGridHandle&) = delete; 114 | explicit FilterGridHandle(FilterGridHandle&&) = delete; 115 | FilterGridHandle& operator=(const FilterGridHandle&) = delete; 116 | FilterGridHandle& operator=(FilterGridHandle&&) = delete; 117 | 118 | virtual ~FilterGridHandle() {} 119 | 120 | int64_t NumCells() const { return num_cells_; } 121 | 122 | protected: 123 | FilterGridHandle(const int64_t num_cells) : num_cells_(num_cells) {} 124 | 125 | private: 126 | int64_t num_cells_ = 0; 127 | }; 128 | 129 | class DeviceVoxelizationHelperInterface 130 | { 131 | public: 132 | // Delete copy and move constructors and assignment operators. 133 | explicit DeviceVoxelizationHelperInterface( 134 | const DeviceVoxelizationHelperInterface&) = delete; 135 | explicit DeviceVoxelizationHelperInterface( 136 | DeviceVoxelizationHelperInterface&&) = delete; 137 | DeviceVoxelizationHelperInterface& operator=( 138 | const DeviceVoxelizationHelperInterface&) = delete; 139 | DeviceVoxelizationHelperInterface& operator=( 140 | DeviceVoxelizationHelperInterface&&) = delete; 141 | 142 | virtual ~DeviceVoxelizationHelperInterface() {} 143 | 144 | virtual bool IsAvailable() const = 0; 145 | 146 | virtual std::unique_ptr PrepareTrackingGrids( 147 | const int64_t num_cells, const int32_t num_grids) = 0; 148 | 149 | virtual void RaycastPoints( 150 | const std::vector& raw_points, const float max_range, 151 | const float* const grid_pointcloud_transform, 152 | const float inverse_step_size, const float inverse_cell_size, 153 | const int32_t num_x_cells, const int32_t num_y_cells, 154 | const int32_t num_z_cells, TrackingGridsHandle& tracking_grids, 155 | const size_t tracking_grid_index) = 0; 156 | 157 | virtual std::unique_ptr PrepareFilterGrid( 158 | const int64_t num_cells, const void* host_data_ptr) = 0; 159 | 160 | virtual void FilterTrackingGrids( 161 | const TrackingGridsHandle& tracking_grids, const float percent_seen_free, 162 | const int32_t outlier_points_threshold, 163 | const int32_t num_cameras_seen_free, FilterGridHandle& filter_grid) = 0; 164 | 165 | virtual void RetrieveTrackingGrid( 166 | const TrackingGridsHandle& tracking_grids, 167 | const size_t tracking_grid_index, void* host_data_ptr) = 0; 168 | 169 | virtual void RetrieveFilteredGrid( 170 | const FilterGridHandle& filter_grid, void* host_data_ptr) = 0; 171 | 172 | protected: 173 | DeviceVoxelizationHelperInterface() = default; 174 | }; 175 | } // namespace pointcloud_voxelization 176 | VGT_NAMESPACE_END 177 | } // namespace voxelized_geometry_tools 178 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/dynamic_spatial_hashed_collision_map.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace voxelized_geometry_tools 16 | { 17 | VGT_NAMESPACE_BEGIN 18 | class DynamicSpatialHashedCollisionMap final 19 | : public common_robotics_utilities::voxel_grid 20 | ::DynamicSpatialHashedVoxelGridBase< 21 | CollisionCell, std::vector> 22 | { 23 | private: 24 | using DeserializedDynamicSpatialHashedCollisionMap 25 | = common_robotics_utilities::serialization 26 | ::Deserialized; 27 | 28 | std::string frame_; 29 | 30 | /// Implement the DynamicSpatialHashedVoxelGridBase interface. 31 | 32 | /// We need to implement cloning. 33 | std::unique_ptr>> 36 | DoClone() const override; 37 | 38 | /// We need to serialize the frame and locked flag. 39 | uint64_t DerivedSerializeSelf( 40 | std::vector& buffer, 41 | const CollisionCellSerializer& value_serializer) const override; 42 | 43 | /// We need to deserialize the frame and locked flag. 44 | uint64_t DerivedDeserializeSelf( 45 | const std::vector& buffer, const uint64_t starting_offset, 46 | const CollisionCellDeserializer& value_deserializer) override; 47 | 48 | bool OnMutableAccess(const Eigen::Vector4d& location) override; 49 | 50 | bool OnMutableRawAccess() override; 51 | 52 | public: 53 | static uint64_t Serialize( 54 | const DynamicSpatialHashedCollisionMap& grid, 55 | std::vector& buffer); 56 | 57 | static DeserializedDynamicSpatialHashedCollisionMap Deserialize( 58 | const std::vector& buffer, const uint64_t starting_offset); 59 | 60 | static void SaveToFile(const DynamicSpatialHashedCollisionMap& map, 61 | const std::string& filepath, 62 | const bool compress); 63 | 64 | static DynamicSpatialHashedCollisionMap LoadFromFile( 65 | const std::string& filepath); 66 | 67 | DynamicSpatialHashedCollisionMap( 68 | const common_robotics_utilities::voxel_grid::GridSizes& chunk_sizes, 69 | const CollisionCell& default_value, const size_t expected_chunks, 70 | const std::string& frame) 71 | : DynamicSpatialHashedVoxelGridBase< 72 | CollisionCell, std::vector>( 73 | Eigen::Isometry3d::Identity(), chunk_sizes, default_value, 74 | expected_chunks), 75 | frame_(frame) 76 | { 77 | if (!HasUniformCellSize()) 78 | { 79 | throw std::invalid_argument( 80 | "DSH collision map cannot have non-uniform cell sizes"); 81 | } 82 | } 83 | 84 | DynamicSpatialHashedCollisionMap( 85 | const Eigen::Isometry3d& origin_transform, 86 | const common_robotics_utilities::voxel_grid::GridSizes& chunk_sizes, 87 | const CollisionCell& default_value, const size_t expected_chunks, 88 | const std::string& frame) 89 | : DynamicSpatialHashedVoxelGridBase< 90 | CollisionCell, std::vector>( 91 | origin_transform, chunk_sizes, default_value, expected_chunks), 92 | frame_(frame) 93 | { 94 | if (!HasUniformCellSize()) 95 | { 96 | throw std::invalid_argument( 97 | "DSH collision map cannot have non-uniform cell sizes"); 98 | } 99 | } 100 | 101 | DynamicSpatialHashedCollisionMap() 102 | : DynamicSpatialHashedVoxelGridBase< 103 | CollisionCell, std::vector>() {} 104 | 105 | double GetResolution() const { return GetCellSizes().x(); } 106 | 107 | const std::string& GetFrame() const { return frame_; } 108 | 109 | void SetFrame(const std::string& frame) { frame_ = frame; } 110 | }; 111 | VGT_NAMESPACE_END 112 | } // namespace voxelized_geometry_tools 113 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/mesh_rasterizer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // TODO(calderpg) Factor this out to support different voxel grid types. 12 | namespace voxelized_geometry_tools 13 | { 14 | VGT_NAMESPACE_BEGIN 15 | namespace mesh_rasterizer 16 | { 17 | /// This sets all voxels intersected by the provided triangle to filled. Note 18 | /// that it sets voxels filled regardless of whether or not the voxel cell 19 | /// center is "inside" or "outside" the triangle. 20 | /// Note: this uses a conservative approximation of triangle-box collision that 21 | /// will identify some extraneous voxels as colliding. For its use so far, this 22 | /// has better than adding too few, since we generally want a watertight layer 23 | /// of surface voxels. 24 | void RasterizeTriangle( 25 | const std::vector& vertices, 26 | const std::vector& triangles, 27 | const size_t triangle_index, 28 | voxelized_geometry_tools::CollisionMap& collision_map, 29 | const bool enforce_collision_map_contains_triangle); 30 | 31 | /// This sets all voxels intersected by the provided mesh to filled. Note 32 | /// that it sets voxels filled regardless of whether or not the voxel cell 33 | /// center is "inside" or "outside" the intersecting triangle(s) of the mesh. 34 | void RasterizeMesh( 35 | const std::vector& vertices, 36 | const std::vector& triangles, 37 | voxelized_geometry_tools::CollisionMap& collision_map, 38 | const bool enforce_collision_map_contains_mesh, 39 | const common_robotics_utilities::parallelism::DegreeOfParallelism& 40 | parallelism); 41 | 42 | /// This sets all voxels intersected by the provided mesh to filled. Note 43 | /// that it sets voxels filled regardless of whether or not the voxel cell 44 | /// center is "inside" or "outside" the intersecting triangle(s) of the mesh. 45 | /// The generated CollisionMap will fully contain the provided mesh. 46 | voxelized_geometry_tools::CollisionMap RasterizeMeshIntoCollisionMap( 47 | const std::vector& vertices, 48 | const std::vector& triangles, 49 | const double resolution, 50 | const common_robotics_utilities::parallelism::DegreeOfParallelism& 51 | parallelism); 52 | } // namespace mesh_rasterizer 53 | VGT_NAMESPACE_END 54 | } // namespace voxelized_geometry_tools 55 | 56 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/opencl_voxelization_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace voxelized_geometry_tools 13 | { 14 | VGT_NAMESPACE_BEGIN 15 | namespace pointcloud_voxelization 16 | { 17 | namespace opencl_helpers 18 | { 19 | std::vector GetAvailableDevices(); 20 | 21 | std::unique_ptr 22 | MakeOpenCLVoxelizationHelper( 23 | const std::map& options, 24 | const LoggingFunction& logging_fn); 25 | } // namespace opencl_helpers 26 | } // namespace pointcloud_voxelization 27 | VGT_NAMESPACE_END 28 | } // namespace voxelized_geometry_tools 29 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/pointcloud_voxelization.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace voxelized_geometry_tools 14 | { 15 | VGT_NAMESPACE_BEGIN 16 | namespace pointcloud_voxelization 17 | { 18 | enum class BackendOptions : uint8_t { BEST_AVAILABLE = 0x00, 19 | CPU = 0x01, 20 | OPENCL = 0x02, 21 | CUDA = 0x03 }; 22 | 23 | // Wrapper for an available voxelizer backend. 24 | class AvailableBackend 25 | { 26 | public: 27 | AvailableBackend( 28 | const std::string& device_name, 29 | const std::map& device_options, 30 | const BackendOptions backend_option) 31 | : device_name_(device_name), device_options_(device_options), 32 | backend_option_(backend_option) {} 33 | 34 | AvailableBackend( 35 | const AvailableDevice& device, const BackendOptions backend_option) 36 | : AvailableBackend( 37 | device.DeviceName(), device.DeviceOptions(), backend_option) {} 38 | 39 | const std::string& DeviceName() const { return device_name_; } 40 | 41 | const std::map& DeviceOptions() const 42 | { 43 | return device_options_; 44 | } 45 | 46 | BackendOptions BackendOption() const { return backend_option_; } 47 | 48 | private: 49 | std::string device_name_; 50 | std::map device_options_; 51 | BackendOptions backend_option_{}; 52 | }; 53 | 54 | std::vector GetAvailableBackends(); 55 | 56 | std::unique_ptr MakePointCloudVoxelizer( 57 | const BackendOptions backend_option, 58 | const std::map& device_options, 59 | const LoggingFunction& logging_fn = {}); 60 | 61 | std::unique_ptr MakePointCloudVoxelizer( 62 | const AvailableBackend& backend, 63 | const LoggingFunction& logging_fn = {}); 64 | 65 | std::unique_ptr 66 | MakeBestAvailablePointCloudVoxelizer( 67 | const std::map& device_options, 68 | const LoggingFunction& logging_fn = {}); 69 | } // namespace pointcloud_voxelization 70 | VGT_NAMESPACE_END 71 | } // namespace voxelized_geometry_tools 72 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/pointcloud_voxelization_interface.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace voxelized_geometry_tools 14 | { 15 | VGT_NAMESPACE_BEGIN 16 | namespace pointcloud_voxelization 17 | { 18 | enum class SeenAs : uint8_t {UNKNOWN = 0x00, FILLED = 0x01, FREE = 0x02}; 19 | 20 | class PointCloudVoxelizationFilterOptions 21 | { 22 | public: 23 | PointCloudVoxelizationFilterOptions( 24 | const double percent_seen_free, const int32_t outlier_points_threshold, 25 | const int32_t num_cameras_seen_free) 26 | : percent_seen_free_(percent_seen_free), 27 | outlier_points_threshold_(outlier_points_threshold), 28 | num_cameras_seen_free_(num_cameras_seen_free) 29 | { 30 | if (percent_seen_free_ <= 0.0 || percent_seen_free_ > 1.0) 31 | { 32 | throw std::invalid_argument("0 < percent_seen_free_ <= 1 must be true"); 33 | } 34 | if (outlier_points_threshold_ <= 0) 35 | { 36 | throw std::invalid_argument("outlier_points_threshold_ <= 0"); 37 | } 38 | if (num_cameras_seen_free_ <= 0) 39 | { 40 | throw std::invalid_argument("num_cameras_seen_free_ <= 0"); 41 | } 42 | } 43 | 44 | PointCloudVoxelizationFilterOptions() 45 | : percent_seen_free_(1.0), 46 | outlier_points_threshold_(1), 47 | num_cameras_seen_free_(1) {} 48 | 49 | double PercentSeenFree() const { return percent_seen_free_; } 50 | 51 | int32_t OutlierPointsThreshold() const { return outlier_points_threshold_; } 52 | 53 | int32_t NumCamerasSeenFree() const { return num_cameras_seen_free_; } 54 | 55 | SeenAs CountsSeenAs( 56 | const int32_t seen_free_count, const int32_t seen_filled_count) const 57 | { 58 | const int32_t filtered_seen_filled_count = 59 | (seen_filled_count >= OutlierPointsThreshold()) ? seen_filled_count : 0; 60 | if (seen_free_count > 0 && filtered_seen_filled_count > 0) 61 | { 62 | const double percent_seen_free = 63 | static_cast(seen_free_count) 64 | / static_cast(seen_free_count + filtered_seen_filled_count); 65 | if (percent_seen_free >= PercentSeenFree()) 66 | { 67 | return SeenAs::FREE; 68 | } 69 | else 70 | { 71 | return SeenAs::FILLED; 72 | } 73 | } 74 | else if (seen_free_count > 0) 75 | { 76 | return SeenAs::FREE; 77 | } 78 | else if (filtered_seen_filled_count > 0) 79 | { 80 | return SeenAs::FILLED; 81 | } 82 | else 83 | { 84 | return SeenAs::UNKNOWN; 85 | } 86 | } 87 | 88 | private: 89 | double percent_seen_free_ = 1.0; 90 | int32_t outlier_points_threshold_ = 1; 91 | int32_t num_cameras_seen_free_ = 1; 92 | }; 93 | 94 | class PointCloudWrapper 95 | { 96 | public: 97 | explicit PointCloudWrapper(const PointCloudWrapper&) = delete; 98 | explicit PointCloudWrapper(PointCloudWrapper&&) = delete; 99 | PointCloudWrapper& operator=(const PointCloudWrapper&) = delete; 100 | PointCloudWrapper& operator=(PointCloudWrapper&&) = delete; 101 | 102 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 103 | 104 | virtual ~PointCloudWrapper() {} 105 | 106 | virtual double MaxRange() const = 0; 107 | 108 | virtual int64_t Size() const = 0; 109 | 110 | virtual const Eigen::Isometry3d& GetPointCloudOriginTransform() const = 0; 111 | 112 | virtual void SetPointCloudOriginTransform( 113 | const Eigen::Isometry3d& origin_transform) = 0; 114 | 115 | Eigen::Vector4d GetPointLocationVector4d(const int64_t point_index) const 116 | { 117 | Eigen::Vector4d point(0.0, 0.0, 0.0, 1.0); 118 | CopyPointLocationIntoVector4d(point_index, point); 119 | return point; 120 | } 121 | 122 | Eigen::Vector4f GetPointLocationVector4f(const int64_t point_index) const 123 | { 124 | Eigen::Vector4f point(0.0f, 0.0f, 0.0f, 1.0f); 125 | CopyPointLocationIntoVector4f(point_index, point); 126 | return point; 127 | } 128 | 129 | void CopyPointLocationIntoVector4d( 130 | const int64_t point_index, Eigen::Vector4d& point_location) const 131 | { 132 | CopyPointLocationIntoDoublePtr(point_index, point_location.data()); 133 | point_location(3) = 1.0; 134 | } 135 | 136 | void CopyPointLocationIntoVector4f( 137 | const int64_t point_index, Eigen::Vector4f& point_location) const 138 | { 139 | CopyPointLocationIntoFloatPtr(point_index, point_location.data()); 140 | point_location(3) = 1.0f; 141 | } 142 | 143 | void CopyPointLocationIntoVectorDouble( 144 | const int64_t point_index, std::vector& vector, 145 | const int64_t vector_index) const 146 | { 147 | EnforceVectorIndexInRange(vector_index, vector); 148 | CopyPointLocationIntoDoublePtr(point_index, vector.data() + vector_index); 149 | } 150 | 151 | void CopyPointLocationIntoVectorFloat( 152 | const int64_t point_index, std::vector& vector, 153 | const int64_t vector_index) const 154 | { 155 | EnforceVectorIndexInRange(vector_index, vector); 156 | CopyPointLocationIntoFloatPtr(point_index, vector.data() + vector_index); 157 | } 158 | 159 | void CopyPointLocationIntoDoublePtr( 160 | const int64_t point_index, double* destination) const 161 | { 162 | EnforcePointIndexInRange(point_index); 163 | CopyPointLocationIntoDoublePtrImpl(point_index, destination); 164 | } 165 | 166 | void CopyPointLocationIntoFloatPtr( 167 | const int64_t point_index, float* destination) const 168 | { 169 | EnforcePointIndexInRange(point_index); 170 | CopyPointLocationIntoFloatPtrImpl(point_index, destination); 171 | } 172 | 173 | void EnforcePointIndexInRange(const int64_t point_index) const 174 | { 175 | if (point_index < 0 || point_index >= Size()) 176 | { 177 | throw std::out_of_range("point_index out of range"); 178 | } 179 | } 180 | 181 | template 182 | static void EnforceVectorIndexInRange( 183 | const int64_t vector_index, const std::vector& destination) 184 | { 185 | if (vector_index < 0 || 186 | (vector_index + 3) > static_cast(destination.size())) 187 | { 188 | throw std::out_of_range("vector_index out of range"); 189 | } 190 | } 191 | 192 | protected: 193 | virtual void CopyPointLocationIntoDoublePtrImpl( 194 | const int64_t point_index, double* destination) const = 0; 195 | 196 | virtual void CopyPointLocationIntoFloatPtrImpl( 197 | const int64_t point_index, float* destination) const = 0; 198 | 199 | PointCloudWrapper() = default; 200 | }; 201 | 202 | using PointCloudWrapperSharedPtr = std::shared_ptr; 203 | 204 | class VoxelizerRuntime 205 | { 206 | public: 207 | VoxelizerRuntime(const double raycasting_time, const double filtering_time) 208 | : raycasting_time_(raycasting_time), filtering_time_(filtering_time) 209 | { 210 | if (raycasting_time_ < 0.0) 211 | { 212 | throw std::invalid_argument("raycasting_time < 0.0"); 213 | } 214 | if (filtering_time_ < 0.0) 215 | { 216 | throw std::invalid_argument("filtering_time < 0.0"); 217 | } 218 | } 219 | 220 | double RaycastingTime() const { return raycasting_time_; } 221 | 222 | double FilteringTime() const { return filtering_time_; } 223 | 224 | private: 225 | double raycasting_time_ = 0.0; 226 | double filtering_time_ = 0.0; 227 | }; 228 | 229 | class PointCloudVoxelizationInterface 230 | { 231 | public: 232 | // Delete copy and move constructors and assignment operators. 233 | explicit PointCloudVoxelizationInterface( 234 | const PointCloudVoxelizationInterface&) = delete; 235 | explicit PointCloudVoxelizationInterface( 236 | PointCloudVoxelizationInterface&&) = delete; 237 | PointCloudVoxelizationInterface& operator=( 238 | const PointCloudVoxelizationInterface&) = delete; 239 | PointCloudVoxelizationInterface& operator=( 240 | PointCloudVoxelizationInterface&&) = delete; 241 | 242 | virtual ~PointCloudVoxelizationInterface() {} 243 | 244 | CollisionMap VoxelizePointClouds( 245 | const CollisionMap& static_environment, const double step_size_multiplier, 246 | const PointCloudVoxelizationFilterOptions& filter_options, 247 | const std::vector& pointclouds, 248 | const std::function& 249 | runtime_log_fn = {}) const { 250 | CollisionMap output_environment = static_environment; 251 | const auto voxelizer_runtime = VoxelizePointClouds( 252 | static_environment, step_size_multiplier, filter_options, pointclouds, 253 | output_environment); 254 | if (runtime_log_fn) 255 | { 256 | runtime_log_fn(voxelizer_runtime); 257 | } 258 | return output_environment; 259 | } 260 | 261 | VoxelizerRuntime VoxelizePointClouds( 262 | const CollisionMap& static_environment, const double step_size_multiplier, 263 | const PointCloudVoxelizationFilterOptions& filter_options, 264 | const std::vector& pointclouds, 265 | CollisionMap& output_environment) const { 266 | if (!static_environment.IsInitialized()) 267 | { 268 | throw std::invalid_argument("!static_environment.IsInitialized()"); 269 | } 270 | if (step_size_multiplier > 1.0 || step_size_multiplier <= 0.0) 271 | { 272 | throw std::invalid_argument("step_size_multiplier is not in (0, 1]"); 273 | } 274 | if (!output_environment.IsInitialized()) 275 | { 276 | throw std::invalid_argument("!output_environment.IsInitialized()"); 277 | } 278 | if (static_environment.GetGridSizes() != output_environment.GetGridSizes()) 279 | { 280 | throw std::invalid_argument( 281 | "static_environment.GetGridSizes() != " 282 | "output_environment.GetGridSizes()"); 283 | } 284 | for (size_t idx = 0; idx < pointclouds.size(); idx++) 285 | { 286 | const PointCloudWrapperSharedPtr& cloud_ptr = pointclouds.at(idx); 287 | if (!cloud_ptr) 288 | { 289 | throw std::invalid_argument( 290 | "pointclouds[" + std::to_string(idx) + "] is null"); 291 | } 292 | } 293 | return DoVoxelizePointClouds( 294 | static_environment, step_size_multiplier, filter_options, pointclouds, 295 | output_environment); 296 | } 297 | 298 | protected: 299 | virtual VoxelizerRuntime DoVoxelizePointClouds( 300 | const CollisionMap& static_environment, const double step_size_multiplier, 301 | const PointCloudVoxelizationFilterOptions& filter_options, 302 | const std::vector& pointclouds, 303 | CollisionMap& output_environment) const = 0; 304 | 305 | PointCloudVoxelizationInterface() = default; 306 | }; 307 | } // namespace pointcloud_voxelization 308 | VGT_NAMESPACE_END 309 | } // namespace voxelized_geometry_tools 310 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/pointcloud_voxelization_ros_interface.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 12 | #include 13 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 14 | #include 15 | #else 16 | #error "Undefined or unknown VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION" 17 | #endif 18 | #include 19 | #include 20 | 21 | namespace voxelized_geometry_tools 22 | { 23 | VGT_NAMESPACE_BEGIN 24 | namespace pointcloud_voxelization 25 | { 26 | 27 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 28 | using PointCloud2 = sensor_msgs::msg::PointCloud2; 29 | using PointCloud2ConstSharedPtr = std::shared_ptr; 30 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 31 | using PointCloud2 = sensor_msgs::PointCloud2; 32 | using PointCloud2ConstSharedPtr = sensor_msgs::PointCloud2ConstPtr; 33 | #endif 34 | 35 | class PointCloud2Wrapper : public PointCloudWrapper 36 | { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | double MaxRange() const override { return max_range_; } 41 | 42 | int64_t Size() const override 43 | { 44 | return static_cast(cloud_ptr_->width * cloud_ptr_->height); 45 | } 46 | 47 | const Eigen::Isometry3d& GetPointCloudOriginTransform() const override 48 | { 49 | return origin_transform_; 50 | } 51 | 52 | void SetPointCloudOriginTransform( 53 | const Eigen::Isometry3d& origin_transform) override 54 | { 55 | origin_transform_ = origin_transform; 56 | } 57 | 58 | const PointCloud2& Cloud() const { return *cloud_ptr_; } 59 | 60 | protected: 61 | PointCloud2Wrapper( 62 | const PointCloud2* const cloud_ptr, 63 | const Eigen::Isometry3d& origin_transform, const double max_range); 64 | 65 | private: 66 | void CopyPointLocationIntoDoublePtrImpl( 67 | const int64_t point_index, double* destination) const override 68 | { 69 | const Eigen::Vector4d point = 70 | GetPointLocationVector4f(point_index).cast(); 71 | std::memcpy(destination, point.data(), sizeof(double) * 3); 72 | } 73 | 74 | void CopyPointLocationIntoFloatPtrImpl( 75 | const int64_t point_index, float* destination) const override 76 | { 77 | const size_t starting_offset = GetStartingOffsetForPointXYZ(point_index); 78 | std::memcpy(destination, &(cloud_ptr_->data.at(starting_offset)), 79 | sizeof(float) * 3); 80 | } 81 | 82 | size_t GetStartingOffsetForPointXYZ(const int64_t point_index) const 83 | { 84 | const size_t starting_offset = 85 | (static_cast(point_index) 86 | * static_cast(cloud_ptr_->point_step)) 87 | + xyz_offset_from_point_start_; 88 | return starting_offset; 89 | } 90 | 91 | const PointCloud2* const cloud_ptr_ = nullptr; 92 | size_t xyz_offset_from_point_start_ = 0; 93 | Eigen::Isometry3d origin_transform_ = Eigen::Isometry3d::Identity(); 94 | double max_range_ = std::numeric_limits::infinity(); 95 | }; 96 | 97 | class NonOwningPointCloud2Wrapper : public PointCloud2Wrapper 98 | { 99 | public: 100 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 101 | 102 | NonOwningPointCloud2Wrapper( 103 | const PointCloud2* const cloud_ptr, 104 | const Eigen::Isometry3d& origin_transform, 105 | const double max_range = std::numeric_limits::infinity()) 106 | : PointCloud2Wrapper(cloud_ptr, origin_transform, max_range) {} 107 | }; 108 | 109 | class OwningPointCloud2Wrapper : public PointCloud2Wrapper 110 | { 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | 114 | OwningPointCloud2Wrapper( 115 | const PointCloud2ConstSharedPtr& cloud_ptr, 116 | const Eigen::Isometry3d& origin_transform, 117 | const double max_range = std::numeric_limits::infinity()) 118 | : PointCloud2Wrapper(cloud_ptr.get(), origin_transform, max_range), 119 | owned_cloud_ptr_(cloud_ptr) {} 120 | 121 | private: 122 | PointCloud2ConstSharedPtr owned_cloud_ptr_; 123 | }; 124 | } // namespace pointcloud_voxelization 125 | VGT_NAMESPACE_END 126 | } // namespace voxelized_geometry_tools 127 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/signed_distance_field_generation.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace voxelized_geometry_tools 18 | { 19 | VGT_NAMESPACE_BEGIN 20 | namespace signed_distance_field_generation 21 | { 22 | // To allow for faster development/improvements, the SignedDistanceField 23 | // generation API has no stability guarantees and lives in an internal 24 | // namespace. Use the stable API exposed by CollisionMap and 25 | // TaggedObjectCollisionMap instead. 26 | namespace internal 27 | { 28 | using common_robotics_utilities::voxel_grid::GridIndex; 29 | using common_robotics_utilities::voxel_grid::GridSizes; 30 | 31 | using EDTDistanceField = 32 | common_robotics_utilities::voxel_grid::VoxelGrid; 33 | 34 | void ComputeDistanceFieldTransformInPlace( 35 | const common_robotics_utilities::parallelism::DegreeOfParallelism& 36 | parallelism, 37 | EDTDistanceField& distance_field); 38 | 39 | template 40 | inline SignedDistanceField ExtractSignedDistanceField( 41 | const Eigen::Isometry3d& grid_origin_transform, const GridSizes& grid_sizes, 42 | const std::function& is_filled_fn, 43 | const std::string& frame, 44 | const SignedDistanceFieldGenerationParameters& parameters) 45 | { 46 | const double marked_cell = 0.0; 47 | const double unmarked_cell = std::numeric_limits::infinity(); 48 | 49 | // Make two distance fields, one for distance to filled voxels, one for 50 | // distance to free voxels. 51 | EDTDistanceField filled_distance_field( 52 | grid_origin_transform, grid_sizes, unmarked_cell); 53 | EDTDistanceField free_distance_field( 54 | grid_origin_transform, grid_sizes, unmarked_cell); 55 | 56 | for (int64_t x_index = 0; x_index < grid_sizes.NumXCells(); x_index++) 57 | { 58 | for (int64_t y_index = 0; y_index < grid_sizes.NumYCells(); y_index++) 59 | { 60 | for (int64_t z_index = 0; z_index < grid_sizes.NumZCells(); z_index++) 61 | { 62 | const GridIndex current_index(x_index, y_index, z_index); 63 | if (is_filled_fn(current_index)) 64 | { 65 | filled_distance_field.SetIndex(current_index, marked_cell); 66 | } 67 | else 68 | { 69 | free_distance_field.SetIndex(current_index, marked_cell); 70 | } 71 | } 72 | } 73 | } 74 | 75 | // Compute the distance transforms in place 76 | ComputeDistanceFieldTransformInPlace( 77 | parameters.Parallelism(), filled_distance_field); 78 | ComputeDistanceFieldTransformInPlace( 79 | parameters.Parallelism(), free_distance_field); 80 | 81 | // Generate the SDF 82 | SignedDistanceField new_sdf( 83 | grid_origin_transform, frame, grid_sizes, parameters.OOBValue()); 84 | for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++) 85 | { 86 | for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++) 87 | { 88 | for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++) 89 | { 90 | const double filled_distance_squared = 91 | filled_distance_field.GetIndexImmutable( 92 | x_index, y_index, z_index).Value(); 93 | const double free_distance_squared = 94 | free_distance_field.GetIndexImmutable( 95 | x_index, y_index, z_index).Value(); 96 | 97 | const double distance1 = 98 | std::sqrt(filled_distance_squared) * new_sdf.GetResolution(); 99 | const double distance2 = 100 | std::sqrt(free_distance_squared) * new_sdf.GetResolution(); 101 | const double distance = distance1 - distance2; 102 | 103 | new_sdf.SetIndex( 104 | x_index, y_index, z_index, static_cast(distance)); 105 | } 106 | } 107 | } 108 | 109 | // Lock & update min/max values. 110 | new_sdf.Lock(); 111 | return new_sdf; 112 | } 113 | 114 | template 115 | inline SignedDistanceField ExtractSignedDistanceField( 116 | const common_robotics_utilities::voxel_grid 117 | ::VoxelGridBase& grid, 118 | const std::function& is_filled_fn, 119 | const std::string& frame, 120 | const SignedDistanceFieldGenerationParameters& parameters) 121 | { 122 | if (!grid.HasUniformCellSize()) 123 | { 124 | throw std::invalid_argument("Grid must have uniform resolution"); 125 | } 126 | if (!parameters.AddVirtualBorder()) 127 | { 128 | // This is the conventional single-pass result 129 | return ExtractSignedDistanceField( 130 | grid.GetOriginTransform(), grid.GetGridSizes(), is_filled_fn, frame, 131 | parameters); 132 | } 133 | else 134 | { 135 | const int64_t x_axis_size_offset = 136 | (grid.GetNumXCells() > 1) ? INT64_C(2) : INT64_C(0); 137 | const int64_t x_axis_query_offset = 138 | (grid.GetNumXCells() > 1) ? INT64_C(1) : INT64_C(0); 139 | const int64_t y_axis_size_offset = 140 | (grid.GetNumYCells() > 1) ? INT64_C(2) : INT64_C(0); 141 | const int64_t y_axis_query_offset = 142 | (grid.GetNumYCells() > 1) ? INT64_C(1) : INT64_C(0); 143 | const int64_t z_axis_size_offset = 144 | (grid.GetNumZCells() > 1) ? INT64_C(2) : INT64_C(0); 145 | const int64_t z_axis_query_offset = 146 | (grid.GetNumZCells() > 1) ? INT64_C(1) : INT64_C(0); 147 | 148 | // We need to lie about the size of the grid to add a virtual border 149 | const int64_t num_x_cells = grid.GetNumXCells() + x_axis_size_offset; 150 | const int64_t num_y_cells = grid.GetNumYCells() + y_axis_size_offset; 151 | const int64_t num_z_cells = grid.GetNumZCells() + z_axis_size_offset; 152 | 153 | // Make some deceitful helper functions that hide our lies about size 154 | // For the free space SDF, we lie and say the virtual border is filled 155 | const std::function free_is_filled_fn 156 | = [&] (const GridIndex& virtual_border_grid_index) 157 | { 158 | // Is there a virtual border on our axis? 159 | if (x_axis_size_offset > 0) 160 | { 161 | // Are we a virtual border cell? 162 | if ((virtual_border_grid_index.X() == 0) 163 | || (virtual_border_grid_index.X() == (num_x_cells - 1))) 164 | { 165 | return true; 166 | } 167 | } 168 | // Is there a virtual border on our axis? 169 | if (y_axis_size_offset > 0) 170 | { 171 | // Are we a virtual border cell? 172 | if ((virtual_border_grid_index.Y() == 0) 173 | || (virtual_border_grid_index.Y() == (num_y_cells - 1))) 174 | { 175 | return true; 176 | } 177 | } 178 | // Is there a virtual border on our axis? 179 | if (z_axis_size_offset > 0) 180 | { 181 | // Are we a virtual border cell? 182 | if ((virtual_border_grid_index.Z() == 0) 183 | || (virtual_border_grid_index.Z() == (num_z_cells - 1))) 184 | { 185 | return true; 186 | } 187 | } 188 | const GridIndex real_grid_index( 189 | virtual_border_grid_index.X() - x_axis_query_offset, 190 | virtual_border_grid_index.Y() - y_axis_query_offset, 191 | virtual_border_grid_index.Z() - z_axis_query_offset); 192 | return is_filled_fn(real_grid_index); 193 | }; 194 | 195 | // For the filled space SDF, we lie and say the virtual border is empty 196 | const std::function filled_is_filled_fn 197 | = [&] (const GridIndex& virtual_border_grid_index) 198 | { 199 | // Is there a virtual border on our axis? 200 | if (x_axis_size_offset > 0) 201 | { 202 | // Are we a virtual border cell? 203 | if ((virtual_border_grid_index.X() == 0) 204 | || (virtual_border_grid_index.X() == (num_x_cells - 1))) 205 | { 206 | return false; 207 | } 208 | } 209 | // Is there a virtual border on our axis? 210 | if (y_axis_size_offset > 0) 211 | { 212 | // Are we a virtual border cell? 213 | if ((virtual_border_grid_index.Y() == 0) 214 | || (virtual_border_grid_index.Y() == (num_y_cells - 1))) 215 | { 216 | return false; 217 | } 218 | } 219 | // Is there a virtual border on our axis? 220 | if (z_axis_size_offset > 0) 221 | { 222 | // Are we a virtual border cell? 223 | if ((virtual_border_grid_index.Z() == 0) 224 | || (virtual_border_grid_index.Z() == (num_z_cells - 1))) 225 | { 226 | return false; 227 | } 228 | } 229 | const GridIndex real_grid_index( 230 | virtual_border_grid_index.X() - x_axis_query_offset, 231 | virtual_border_grid_index.Y() - y_axis_query_offset, 232 | virtual_border_grid_index.Z() - z_axis_query_offset); 233 | return is_filled_fn(real_grid_index); 234 | }; 235 | 236 | // Make both SDFs 237 | const common_robotics_utilities::voxel_grid::GridSizes enlarged_sizes( 238 | grid.GetCellSizes().x(), num_x_cells, num_y_cells, num_z_cells); 239 | const auto free_sdf = ExtractSignedDistanceField( 240 | grid.GetOriginTransform(), enlarged_sizes, free_is_filled_fn, frame, 241 | parameters); 242 | const auto filled_sdf = ExtractSignedDistanceField( 243 | grid.GetOriginTransform(), enlarged_sizes, filled_is_filled_fn, frame, 244 | parameters); 245 | 246 | // Combine to make a single SDF 247 | SignedDistanceField combined_sdf( 248 | grid.GetOriginTransform(), frame, grid.GetGridSizes(), 249 | parameters.OOBValue()); 250 | for (int64_t x_idx = 0; x_idx < combined_sdf.GetNumXCells(); x_idx++) 251 | { 252 | for (int64_t y_idx = 0; y_idx < combined_sdf.GetNumYCells(); y_idx++) 253 | { 254 | for (int64_t z_idx = 0; z_idx < combined_sdf.GetNumZCells(); z_idx++) 255 | { 256 | const int64_t query_x_idx = x_idx + x_axis_query_offset; 257 | const int64_t query_y_idx = y_idx + y_axis_query_offset; 258 | const int64_t query_z_idx = z_idx + z_axis_query_offset; 259 | const SDFScalarType free_sdf_value = free_sdf.GetIndexImmutable( 260 | query_x_idx, query_y_idx, query_z_idx).Value(); 261 | const SDFScalarType filled_sdf_value = filled_sdf.GetIndexImmutable( 262 | query_x_idx, query_y_idx, query_z_idx).Value(); 263 | 264 | if (free_sdf_value >= 0.0) 265 | { 266 | combined_sdf.SetIndex(x_idx, y_idx, z_idx, free_sdf_value); 267 | } 268 | else if (filled_sdf_value <= -0.0) 269 | { 270 | combined_sdf.SetIndex(x_idx, y_idx, z_idx, filled_sdf_value); 271 | } 272 | else 273 | { 274 | combined_sdf.SetIndex(x_idx, y_idx, z_idx, 0.0); 275 | } 276 | } 277 | } 278 | } 279 | 280 | // Lock & update min/max values. 281 | combined_sdf.Lock(); 282 | return combined_sdf; 283 | } 284 | } 285 | } // namespace internal 286 | } // namespace signed_distance_field_generation 287 | VGT_NAMESPACE_END 288 | } // namespace voxelized_geometry_tools 289 | -------------------------------------------------------------------------------- /include/voxelized_geometry_tools/vgt_namespace.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** Downstream projects can adjust these macros to tweak the project namespace. 4 | 5 | When set, they should refer to a C++ inline namespace: 6 | https://en.cppreference.com/w/cpp/language/namespace#Inline_namespaces 7 | 8 | The inline namespace provides symbol versioning to allow multiple copies of 9 | common_robotics_utilities to be linked into the same image. In many cases, 10 | the namespace will also be marked hidden so that linker symbols are private. 11 | 12 | Example: 13 | #define VGT_NAMESPACE_BEGIN \ 14 | inline namespace v1 __attribute__ ((visibility ("hidden"))) { 15 | #define VGT_NAMESPACE_END } 16 | */ 17 | 18 | #ifndef VGT_NAMESPACE_BEGIN 19 | # define VGT_NAMESPACE_BEGIN inline namespace v1 { 20 | #endif 21 | 22 | #ifndef VGT_NAMESPACE_END 23 | # define VGT_NAMESPACE_END } 24 | #endif 25 | -------------------------------------------------------------------------------- /msg/CollisionMapMessage.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8[] serialized_map 3 | bool is_compressed 4 | -------------------------------------------------------------------------------- /msg/DynamicSpatialHashedCollisionMapMessage.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8[] serialized_map 3 | bool is_compressed 4 | -------------------------------------------------------------------------------- /msg/SignedDistanceFieldMessage.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8[] serialized_sdf 3 | bool is_compressed 4 | -------------------------------------------------------------------------------- /msg/TaggedObjectCollisionMapMessage.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8[] serialized_map 3 | bool is_compressed 4 | -------------------------------------------------------------------------------- /package.xml.ros1: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxelized_geometry_tools 4 | 0.0.1 5 | Voxelized geometry tools (Voxel-based collision/occupancy maps, signed-distance fields, discrete geometry tools) 6 | Calder Phillips-Grafflin 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | std_msgs 13 | geometry_msgs 14 | visualization_msgs 15 | common_robotics_utilities 16 | message_generation 17 | message_runtime 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /package.xml.ros2: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxelized_geometry_tools 4 | 0.1.0 5 | Voxelized geometry tools (Voxel-based collision/occupancy maps, signed-distance fields, discrete geometry tools) 6 | Calder Phillips-Grafflin 7 | BSD 8 | 9 | ament_cmake_ros 10 | rosidl_default_generators 11 | 12 | common_robotics_utilities 13 | geometry_msgs 14 | rclcpp 15 | sensor_msgs 16 | std_msgs 17 | visualization_msgs 18 | 19 | rosidl_default_runtime 20 | 21 | rosidl_interface_packages 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/cpu_pointcloud_voxelization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 20 | using common_robotics_utilities::parallelism::ParallelForBackend; 21 | using common_robotics_utilities::parallelism::StaticParallelForIndexLoop; 22 | 23 | namespace voxelized_geometry_tools 24 | { 25 | VGT_NAMESPACE_BEGIN 26 | namespace pointcloud_voxelization 27 | { 28 | namespace 29 | { 30 | /// Struct to store free/filled counts in a voxel grid. Counts are stored in 31 | /// std::atomic since a given cell may be manipulated by multiple 32 | /// threads simultaneously and this gives us atomic fetch_add() to increment the 33 | /// stored values. 34 | struct CpuVoxelizationTrackingCell 35 | { 36 | common_robotics_utilities::utility 37 | ::CopyableMoveableAtomic 38 | seen_free_count{0}; 39 | common_robotics_utilities::utility 40 | ::CopyableMoveableAtomic 41 | seen_filled_count{0}; 42 | }; 43 | 44 | using CpuVoxelizationTrackingGrid = common_robotics_utilities::voxel_grid 45 | ::VoxelGrid; 46 | using CpuVoxelizationTrackingGridAllocator = 47 | Eigen::aligned_allocator; 48 | using VectorCpuVoxelizationTrackingGrid = 49 | std::vector; 51 | 52 | void RaycastPointCloud( 53 | const PointCloudWrapper& cloud, const double step_size, 54 | const DegreeOfParallelism& parallelism, 55 | CpuVoxelizationTrackingGrid& tracking_grid) 56 | { 57 | // Get X_GW, the transform from grid origin to world 58 | const Eigen::Isometry3d& X_GW = tracking_grid.GetInverseOriginTransform(); 59 | // Get X_WC, the transform from world to the origin of the pointcloud 60 | const Eigen::Isometry3d& X_WC = cloud.GetPointCloudOriginTransform(); 61 | // Transform X_GC, transform from grid origin to the origin of the pointcloud 62 | const Eigen::Isometry3d X_GC = X_GW * X_WC; 63 | // Get the pointcloud origin in grid frame 64 | const Eigen::Vector4d p_GCo = X_GC * Eigen::Vector4d(0.0, 0.0, 0.0, 1.0); 65 | // Get the max range 66 | const double max_range = cloud.MaxRange(); 67 | 68 | // Helper lambda for raycasting a single point 69 | const auto per_item_work = [&](const int32_t, const int64_t point_index) 70 | { 71 | // Location of point P in frame of camera C 72 | const Eigen::Vector4d p_CP = cloud.GetPointLocationVector4d(point_index); 73 | // Skip invalid points marked with NaN or infinity 74 | if (std::isfinite(p_CP(0)) && std::isfinite(p_CP(1)) && 75 | std::isfinite(p_CP(2))) 76 | { 77 | // Location of point P in grid G 78 | const Eigen::Vector4d p_GP = X_GC * p_CP; 79 | // Ray from camera to point 80 | const Eigen::Vector4d ray = p_GP - p_GCo; 81 | const double ray_length = ray.norm(); 82 | // Step along ray 83 | const int32_t num_steps = 84 | std::max(1, static_cast(std::floor(ray_length / step_size))); 85 | common_robotics_utilities::voxel_grid::GridIndex last_index(-1, -1, -1); 86 | bool ray_crossed_grid = false; 87 | for (int32_t step = 0; step < num_steps; step++) 88 | { 89 | const double ratio = 90 | static_cast(step) / static_cast(num_steps); 91 | if ((ratio * ray_length) > max_range) 92 | { 93 | // We've gone beyond max range of the sensor 94 | break; 95 | } 96 | const Eigen::Vector4d p_GQ = p_GCo + (ray * ratio); 97 | const common_robotics_utilities::voxel_grid::GridIndex index = 98 | tracking_grid.LocationInGridFrameToGridIndex4d(p_GQ); 99 | // We don't want to double count in the same cell multiple times 100 | if (!(index == last_index)) 101 | { 102 | auto query = tracking_grid.GetIndexMutable(index); 103 | // We must check to see if the query is within bounds. 104 | if (query) 105 | { 106 | ray_crossed_grid = true; 107 | query.Value().seen_free_count.fetch_add(1); 108 | } 109 | else if (ray_crossed_grid) 110 | { 111 | // We've left the grid and there's no reason to keep going. 112 | break; 113 | } 114 | } 115 | last_index = index; 116 | } 117 | // Set the point itself as filled, if it is in range 118 | if (ray_length <= max_range) 119 | { 120 | const common_robotics_utilities::voxel_grid::GridIndex index = 121 | tracking_grid.LocationInGridFrameToGridIndex4d(p_GP); 122 | auto query = tracking_grid.GetIndexMutable(index); 123 | // We must check to see if the query is within bounds. 124 | if (query) 125 | { 126 | query.Value().seen_filled_count.fetch_add(1); 127 | } 128 | } 129 | } 130 | }; 131 | 132 | // Raycast all points in the pointcloud. Use OpenMP if available, if not fall 133 | // back to manual dispatch via std::async. 134 | StaticParallelForIndexLoop( 135 | parallelism, 0, cloud.Size(), per_item_work, 136 | ParallelForBackend::BEST_AVAILABLE); 137 | } 138 | 139 | void CombineAndFilterGrids( 140 | const PointCloudVoxelizationFilterOptions& filter_options, 141 | const VectorCpuVoxelizationTrackingGrid& tracking_grids, 142 | const DegreeOfParallelism& parallelism, 143 | CollisionMap& filtered_grid) 144 | { 145 | // Because we want to improve performance and don't need to know where in the 146 | // grid we are, we can take advantage of the dense backing vector to iterate 147 | // through the grid data, rather than the grid cells. 148 | 149 | // Helper lambda for each item's work 150 | const auto per_item_work = [&](const int32_t, const int64_t voxel_index) 151 | { 152 | auto& current_cell = filtered_grid.GetDataIndexMutable(voxel_index); 153 | // Filled cells stay filled, we don't work with them. 154 | // We only change cells that are unknown or empty. 155 | if (current_cell.Occupancy() <= 0.5) 156 | { 157 | int32_t seen_filled = 0; 158 | int32_t seen_free = 0; 159 | for (size_t idx = 0; idx < tracking_grids.size(); idx++) 160 | { 161 | const CpuVoxelizationTrackingCell& grid_cell = 162 | tracking_grids.at(idx).GetDataIndexImmutable(voxel_index); 163 | const int32_t free_count = grid_cell.seen_free_count.load(); 164 | const int32_t filled_count = grid_cell.seen_filled_count.load(); 165 | const SeenAs seen_as = 166 | filter_options.CountsSeenAs(free_count, filled_count); 167 | if (seen_as == SeenAs::FREE) 168 | { 169 | seen_free += 1; 170 | } 171 | else if (seen_as == SeenAs::FILLED) 172 | { 173 | seen_filled += 1; 174 | } 175 | } 176 | if (seen_filled > 0) 177 | { 178 | // If any camera saw something here, it is filled. 179 | current_cell.SetOccupancy(1.0); 180 | } 181 | else if (seen_free >= filter_options.NumCamerasSeenFree()) 182 | { 183 | // Did enough cameras see this empty? 184 | current_cell.SetOccupancy(0.0); 185 | } 186 | else 187 | { 188 | // Otherwise, it is unknown. 189 | current_cell.SetOccupancy(0.5); 190 | } 191 | } 192 | }; 193 | 194 | // Use OpenMP if available, if not fall back to manual dispatch via 195 | // std::async. 196 | StaticParallelForIndexLoop( 197 | parallelism, 0, filtered_grid.GetTotalCells(), per_item_work, 198 | ParallelForBackend::BEST_AVAILABLE); 199 | } 200 | } // namespace 201 | 202 | CpuPointCloudVoxelizer::CpuPointCloudVoxelizer( 203 | const std::map& options, 204 | const LoggingFunction& logging_fn) 205 | { 206 | const int32_t cpu_parallelize = 207 | RetrieveOptionOrDefault(options, "CPU_PARALLELIZE", 1, logging_fn); 208 | const int32_t cpu_num_threads = 209 | RetrieveOptionOrDefault(options, "CPU_NUM_THREADS", -1, logging_fn); 210 | if (cpu_parallelize > 0 && cpu_num_threads >= 1) 211 | { 212 | parallelism_ = DegreeOfParallelism(cpu_num_threads); 213 | if (logging_fn) 214 | { 215 | logging_fn( 216 | "Configured parallelism using provided number of threads " 217 | + std::to_string(cpu_num_threads)); 218 | } 219 | } 220 | else if (cpu_parallelize > 0) 221 | { 222 | parallelism_ = DegreeOfParallelism::FromOmp(); 223 | if (logging_fn) 224 | { 225 | logging_fn( 226 | "Configured parallelism using OpenMP num threads " 227 | + std::to_string(Parallelism().GetNumThreads())); 228 | } 229 | } 230 | else 231 | { 232 | parallelism_ = DegreeOfParallelism::None(); 233 | if (logging_fn) 234 | { 235 | logging_fn("Parallelism disabled"); 236 | } 237 | } 238 | } 239 | 240 | VoxelizerRuntime CpuPointCloudVoxelizer::DoVoxelizePointClouds( 241 | const CollisionMap& static_environment, const double step_size_multiplier, 242 | const PointCloudVoxelizationFilterOptions& filter_options, 243 | const std::vector& pointclouds, 244 | CollisionMap& output_environment) const 245 | { 246 | const std::chrono::time_point start_time = 247 | std::chrono::steady_clock::now(); 248 | // Pose of grid G in world W. 249 | const Eigen::Isometry3d& X_WG = static_environment.GetOriginTransform(); 250 | // Get grid resolution and size parameters. 251 | const auto& grid_size = static_environment.GetGridSizes(); 252 | const double cell_size = static_environment.GetResolution(); 253 | const double step_size = cell_size * step_size_multiplier; 254 | // For each cloud, raycast it into its own "tracking grid" 255 | VectorCpuVoxelizationTrackingGrid tracking_grids( 256 | pointclouds.size(), 257 | CpuVoxelizationTrackingGrid( 258 | X_WG, grid_size, CpuVoxelizationTrackingCell())); 259 | for (size_t idx = 0; idx < pointclouds.size(); idx++) 260 | { 261 | const PointCloudWrapperSharedPtr& cloud_ptr = pointclouds.at(idx); 262 | CpuVoxelizationTrackingGrid& tracking_grid = tracking_grids.at(idx); 263 | RaycastPointCloud(*cloud_ptr, step_size, Parallelism(), tracking_grid); 264 | } 265 | const std::chrono::time_point raycasted_time = 266 | std::chrono::steady_clock::now(); 267 | // Combine & filter 268 | CombineAndFilterGrids( 269 | filter_options, tracking_grids, Parallelism(), output_environment); 270 | const std::chrono::time_point done_time = 271 | std::chrono::steady_clock::now(); 272 | return VoxelizerRuntime( 273 | std::chrono::duration(raycasted_time - start_time).count(), 274 | std::chrono::duration(done_time - raycasted_time).count()); 275 | } 276 | } // namespace pointcloud_voxelization 277 | VGT_NAMESPACE_END 278 | } // namespace voxelized_geometry_tools 279 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/device_pointcloud_voxelization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 19 | using common_robotics_utilities::parallelism::DynamicParallelForIndexLoop; 20 | using common_robotics_utilities::parallelism::ParallelForBackend; 21 | 22 | namespace voxelized_geometry_tools 23 | { 24 | VGT_NAMESPACE_BEGIN 25 | namespace pointcloud_voxelization 26 | { 27 | DevicePointCloudVoxelizer::DevicePointCloudVoxelizer( 28 | const std::map& options, 29 | const LoggingFunction& logging_fn) 30 | { 31 | const int32_t dispatch_parallelize = 32 | RetrieveOptionOrDefault(options, "DISPATCH_PARALLELIZE", 1, logging_fn); 33 | const int32_t dispatch_num_threads = 34 | RetrieveOptionOrDefault(options, "DISPATCH_NUM_THREADS", -1, logging_fn); 35 | if (dispatch_parallelize > 0 && dispatch_num_threads >= 1) 36 | { 37 | dispatch_parallelism_ = DegreeOfParallelism(dispatch_num_threads); 38 | if (logging_fn) 39 | { 40 | logging_fn( 41 | "Configured dispatch parallelism using provided number of threads " 42 | + std::to_string(dispatch_num_threads)); 43 | } 44 | } 45 | else if (dispatch_parallelize > 0) 46 | { 47 | dispatch_parallelism_ = DegreeOfParallelism::FromOmp(); 48 | if (logging_fn) 49 | { 50 | logging_fn( 51 | "Configured dispatch parallelism using OpenMP num threads " 52 | + std::to_string(DispatchParallelism().GetNumThreads())); 53 | } 54 | } 55 | else 56 | { 57 | dispatch_parallelism_ = DegreeOfParallelism::None(); 58 | if (logging_fn) 59 | { 60 | logging_fn("Dispatch parallelism disabled"); 61 | } 62 | } 63 | } 64 | 65 | VoxelizerRuntime DevicePointCloudVoxelizer::DoVoxelizePointClouds( 66 | const CollisionMap& static_environment, const double step_size_multiplier, 67 | const PointCloudVoxelizationFilterOptions& filter_options, 68 | const std::vector& pointclouds, 69 | CollisionMap& output_environment) const 70 | { 71 | EnforceAvailable(); 72 | 73 | const std::chrono::time_point start_time = 74 | std::chrono::steady_clock::now(); 75 | 76 | // Allocate device-side memory for tracking grids. Note that at least one grid 77 | // is always allocated so that filtering is consistent, even if no points are 78 | // raycast. 79 | const size_t num_tracking_grids = 80 | std::max(pointclouds.size(), static_cast(1)); 81 | 82 | std::unique_ptr tracking_grids = 83 | helper_interface_->PrepareTrackingGrids( 84 | static_environment.GetTotalCells(), 85 | static_cast(num_tracking_grids)); 86 | if (tracking_grids->GetNumTrackingGrids() != num_tracking_grids) 87 | { 88 | throw std::runtime_error("Failed to allocate device tracking grid"); 89 | } 90 | 91 | // Get X_GW, the transform from grid origin to world 92 | const Eigen::Isometry3d& X_GW = 93 | static_environment.GetInverseOriginTransform(); 94 | 95 | // Prepare grid data 96 | const float inverse_step_size = 97 | static_cast(1.0 / 98 | (static_environment.GetResolution() * step_size_multiplier)); 99 | const float inverse_cell_size = 100 | static_cast(static_environment.GetGridSizes().InvCellXSize()); 101 | const int32_t num_x_cells = 102 | static_cast(static_environment.GetNumXCells()); 103 | const int32_t num_y_cells = 104 | static_cast(static_environment.GetNumYCells()); 105 | const int32_t num_z_cells = 106 | static_cast(static_environment.GetNumZCells()); 107 | 108 | // Lambda for the raycasting of a single pointcloud. 109 | const auto per_item_work = [&](const int32_t, const int64_t pointcloud_index) 110 | { 111 | const PointCloudWrapperSharedPtr& pointcloud = 112 | pointclouds.at(static_cast(pointcloud_index)); 113 | 114 | // Only do work if the pointcloud is non-empty, to avoid passing empty 115 | // arrays into the device interface. 116 | if (pointcloud->Size() > 0) 117 | { 118 | // Get X_WC, the transform from world to the origin of the pointcloud 119 | const Eigen::Isometry3d& X_WC = 120 | pointcloud->GetPointCloudOriginTransform(); 121 | // X_GC, transform from grid origin to the origin of the pointcloud 122 | const Eigen::Isometry3f grid_pointcloud_transform_float = 123 | (X_GW * X_WC).cast(); 124 | 125 | const float max_range = static_cast(pointcloud->MaxRange()); 126 | 127 | // Copy pointcloud 128 | std::vector raw_points( 129 | static_cast(pointcloud->Size()) * 3, 0.0); 130 | for (int64_t point = 0; point < pointcloud->Size(); point++) 131 | { 132 | pointcloud->CopyPointLocationIntoVectorFloat( 133 | point, raw_points, point * 3); 134 | } 135 | 136 | // Raycast 137 | helper_interface_->RaycastPoints( 138 | raw_points, max_range, grid_pointcloud_transform_float.data(), 139 | inverse_step_size, inverse_cell_size, num_x_cells, num_y_cells, 140 | num_z_cells, *tracking_grids, static_cast(pointcloud_index)); 141 | } 142 | }; 143 | 144 | DynamicParallelForIndexLoop( 145 | DispatchParallelism(), 0, static_cast(pointclouds.size()), 146 | per_item_work, ParallelForBackend::BEST_AVAILABLE); 147 | 148 | const std::chrono::time_point raycasted_time = 149 | std::chrono::steady_clock::now(); 150 | 151 | // Filter 152 | const float percent_seen_free = 153 | static_cast(filter_options.PercentSeenFree()); 154 | const int32_t outlier_points_threshold = 155 | filter_options.OutlierPointsThreshold(); 156 | const int32_t num_cameras_seen_free = 157 | filter_options.NumCamerasSeenFree(); 158 | 159 | std::unique_ptr filter_grid = 160 | helper_interface_->PrepareFilterGrid( 161 | static_environment.GetTotalCells(), 162 | static_environment.GetImmutableRawData().data()); 163 | 164 | helper_interface_->FilterTrackingGrids( 165 | *tracking_grids, percent_seen_free, outlier_points_threshold, 166 | num_cameras_seen_free, *filter_grid); 167 | 168 | // Retrieve & return 169 | helper_interface_->RetrieveFilteredGrid( 170 | *filter_grid, output_environment.GetMutableRawData().data()); 171 | 172 | const std::chrono::time_point done_time = 173 | std::chrono::steady_clock::now(); 174 | 175 | return VoxelizerRuntime( 176 | std::chrono::duration(raycasted_time - start_time).count(), 177 | std::chrono::duration(done_time - raycasted_time).count()); 178 | } 179 | 180 | CudaPointCloudVoxelizer::CudaPointCloudVoxelizer( 181 | const std::map& options, 182 | const LoggingFunction& logging_fn) 183 | : DevicePointCloudVoxelizer(options, logging_fn) 184 | { 185 | device_name_ = "CudaPointCloudVoxelizer"; 186 | helper_interface_ = std::unique_ptr( 187 | cuda_helpers::MakeCudaVoxelizationHelper(options, logging_fn)); 188 | EnforceAvailable(); 189 | } 190 | 191 | OpenCLPointCloudVoxelizer::OpenCLPointCloudVoxelizer( 192 | const std::map& options, 193 | const LoggingFunction& logging_fn) 194 | : DevicePointCloudVoxelizer(options, logging_fn) 195 | { 196 | device_name_ = "OpenCLPointCloudVoxelizer"; 197 | helper_interface_ = std::unique_ptr( 198 | opencl_helpers::MakeOpenCLVoxelizationHelper(options, logging_fn)); 199 | EnforceAvailable(); 200 | } 201 | } // namespace pointcloud_voxelization 202 | VGT_NAMESPACE_END 203 | } // namespace voxelized_geometry_tools 204 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/dummy_cuda_voxelization_helpers.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace voxelized_geometry_tools 9 | { 10 | VGT_NAMESPACE_BEGIN 11 | namespace pointcloud_voxelization 12 | { 13 | namespace cuda_helpers 14 | { 15 | std::vector GetAvailableDevices() { return {}; } 16 | 17 | std::unique_ptr 18 | MakeCudaVoxelizationHelper( 19 | const std::map&, const LoggingFunction&) 20 | { 21 | return std::unique_ptr(); 22 | } 23 | } // namespace cuda_helpers 24 | } // namespace pointcloud_voxelization 25 | VGT_NAMESPACE_END 26 | } // namespace voxelized_geometry_tools 27 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/dummy_opencl_voxelization_helpers.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace voxelized_geometry_tools 9 | { 10 | VGT_NAMESPACE_BEGIN 11 | namespace pointcloud_voxelization 12 | { 13 | namespace opencl_helpers 14 | { 15 | std::vector GetAvailableDevices() { return {}; } 16 | 17 | std::unique_ptr 18 | MakeOpenCLVoxelizationHelper( 19 | const std::map&, const LoggingFunction&) 20 | { 21 | return std::unique_ptr(); 22 | } 23 | } // namespace opencl_helpers 24 | } // namespace pointcloud_voxelization 25 | VGT_NAMESPACE_END 26 | } // namespace voxelized_geometry_tools 27 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/dynamic_spatial_hashed_collision_map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace voxelized_geometry_tools 19 | { 20 | VGT_NAMESPACE_BEGIN 21 | /// We need to implement cloning. 22 | std::unique_ptr>> 25 | DynamicSpatialHashedCollisionMap::DoClone() const 26 | { 27 | return std::unique_ptr( 28 | new DynamicSpatialHashedCollisionMap(*this)); 29 | } 30 | 31 | /// We need to serialize the frame and locked flag. 32 | uint64_t DynamicSpatialHashedCollisionMap::DerivedSerializeSelf( 33 | std::vector& buffer, 34 | const CollisionCellSerializer& value_serializer) const 35 | { 36 | CRU_UNUSED(value_serializer); 37 | const uint64_t start_size = buffer.size(); 38 | common_robotics_utilities::serialization::SerializeString(frame_, buffer); 39 | const uint64_t bytes_written = buffer.size() - start_size; 40 | return bytes_written; 41 | } 42 | 43 | /// We need to deserialize the frame and locked flag. 44 | uint64_t DynamicSpatialHashedCollisionMap::DerivedDeserializeSelf( 45 | const std::vector& buffer, const uint64_t starting_offset, 46 | const CollisionCellDeserializer& value_deserializer) 47 | { 48 | CRU_UNUSED(value_deserializer); 49 | uint64_t current_position = starting_offset; 50 | const auto frame_deserialized 51 | = common_robotics_utilities::serialization::DeserializeString( 52 | buffer, current_position); 53 | frame_ = frame_deserialized.Value(); 54 | current_position += frame_deserialized.BytesRead(); 55 | // Figure out how many bytes were read 56 | const uint64_t bytes_read = current_position - starting_offset; 57 | return bytes_read; 58 | } 59 | 60 | bool DynamicSpatialHashedCollisionMap::OnMutableAccess( 61 | const Eigen::Vector4d& location) 62 | { 63 | CRU_UNUSED(location); 64 | return true; 65 | } 66 | 67 | bool DynamicSpatialHashedCollisionMap::OnMutableRawAccess() 68 | { 69 | return true; 70 | } 71 | 72 | uint64_t DynamicSpatialHashedCollisionMap::Serialize( 73 | const DynamicSpatialHashedCollisionMap& map, std::vector& buffer) 74 | { 75 | return map.SerializeSelf(buffer, CollisionCell::Serialize); 76 | } 77 | 78 | DynamicSpatialHashedCollisionMap::DeserializedDynamicSpatialHashedCollisionMap 79 | DynamicSpatialHashedCollisionMap::Deserialize( 80 | const std::vector& buffer, const uint64_t starting_offset) 81 | { 82 | DynamicSpatialHashedCollisionMap temp_map; 83 | const uint64_t bytes_read 84 | = temp_map.DeserializeSelf( 85 | buffer, starting_offset, CollisionCell::Deserialize); 86 | return common_robotics_utilities::serialization::MakeDeserialized( 87 | temp_map, bytes_read); 88 | } 89 | 90 | void DynamicSpatialHashedCollisionMap::SaveToFile( 91 | const DynamicSpatialHashedCollisionMap& map, 92 | const std::string& filepath, 93 | const bool compress) 94 | { 95 | std::vector buffer; 96 | DynamicSpatialHashedCollisionMap::Serialize(map, buffer); 97 | std::ofstream output_file(filepath, std::ios::out|std::ios::binary); 98 | if (compress) 99 | { 100 | output_file.write("DMGZ", 4); 101 | const std::vector compressed 102 | = common_robotics_utilities::zlib_helpers::CompressBytes(buffer); 103 | const size_t serialized_size = compressed.size(); 104 | output_file.write( 105 | reinterpret_cast(compressed.data()), 106 | static_cast(serialized_size)); 107 | } 108 | else 109 | { 110 | output_file.write("DMGR", 4); 111 | const size_t serialized_size = buffer.size(); 112 | output_file.write( 113 | reinterpret_cast(buffer.data()), 114 | static_cast(serialized_size)); 115 | } 116 | output_file.close(); 117 | } 118 | 119 | DynamicSpatialHashedCollisionMap 120 | DynamicSpatialHashedCollisionMap::LoadFromFile(const std::string& filepath) 121 | { 122 | std::ifstream input_file( 123 | filepath, std::ios::in | std::ios::binary | std::ios::ate); 124 | if (input_file.good() == false) 125 | { 126 | throw std::invalid_argument("File does not exist"); 127 | } 128 | const std::streampos end = input_file.tellg(); 129 | input_file.seekg(0, std::ios::beg); 130 | const std::streampos begin = input_file.tellg(); 131 | const std::streamsize serialized_size = end - begin; 132 | const std::streamsize header_size = 4; 133 | if (serialized_size >= header_size) 134 | { 135 | // Load the header 136 | std::vector file_header(header_size + 1, 0x00); 137 | input_file.read(reinterpret_cast(file_header.data()), 138 | header_size); 139 | const std::string header_string( 140 | reinterpret_cast(file_header.data())); 141 | // Load the rest of the file 142 | std::vector file_buffer( 143 | static_cast(serialized_size - header_size), 0x00); 144 | input_file.read(reinterpret_cast(file_buffer.data()), 145 | serialized_size - header_size); 146 | // Deserialize 147 | if (header_string == "DMGZ") 148 | { 149 | const std::vector decompressed 150 | = common_robotics_utilities::zlib_helpers 151 | ::DecompressBytes(file_buffer); 152 | return DynamicSpatialHashedCollisionMap::Deserialize( 153 | decompressed, 0).Value(); 154 | } 155 | else if (header_string == "DMGR") 156 | { 157 | return DynamicSpatialHashedCollisionMap::Deserialize( 158 | file_buffer, 0).Value(); 159 | } 160 | else 161 | { 162 | throw std::invalid_argument( 163 | "File has invalid header [" + header_string + "]"); 164 | } 165 | } 166 | else 167 | { 168 | throw std::invalid_argument("File is too small"); 169 | } 170 | } 171 | VGT_NAMESPACE_END 172 | } // namespace voxelized_geometry_tools 173 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/mesh_rasterizer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 15 | using common_robotics_utilities::parallelism::ParallelForBackend; 16 | using common_robotics_utilities::parallelism::StaticParallelForIndexLoop; 17 | 18 | namespace voxelized_geometry_tools 19 | { 20 | VGT_NAMESPACE_BEGIN 21 | namespace mesh_rasterizer 22 | { 23 | namespace 24 | { 25 | bool PointProjectsInsideTriangle( 26 | const Eigen::Vector3d& p_Mv1, const Eigen::Vector3d& p_Mv2, 27 | const Eigen::Vector3d& p_Mv3, const Eigen::Vector3d& p_MQ) 28 | { 29 | const auto same_side = [] ( 30 | const Eigen::Vector3d& p_MA, const Eigen::Vector3d& p_MB, 31 | const Eigen::Vector3d& p_Mp1, const Eigen::Vector3d& p_Mp2) -> bool 32 | { 33 | const Eigen::Vector3d v_AB = p_MB - p_MA; 34 | const Eigen::Vector3d cross1 = v_AB.cross(p_Mp1 - p_MA); 35 | const Eigen::Vector3d cross2 = v_AB.cross(p_Mp2 - p_MA); 36 | return (cross1.dot(cross2) >= 0.0); 37 | }; 38 | 39 | return (same_side(p_Mv1, p_Mv2, p_Mv3, p_MQ) && 40 | same_side(p_Mv2, p_Mv3, p_Mv1, p_MQ) && 41 | same_side(p_Mv3, p_Mv1, p_Mv2, p_MQ)); 42 | } 43 | 44 | Eigen::Vector3d ClosestPointOnLineSegment( 45 | const Eigen::Vector3d& p_MA, const Eigen::Vector3d& p_MB, 46 | const Eigen::Vector3d& p_MQ) 47 | { 48 | const Eigen::Vector3d v_AB = p_MB - p_MA; 49 | const Eigen::Vector3d v_AQ = p_MQ - p_MA; 50 | 51 | const double ratio = v_AB.dot(v_AQ) / v_AB.squaredNorm(); 52 | const double clamped = 53 | common_robotics_utilities::utility::ClampValue(ratio, 0.0, 1.0); 54 | 55 | return p_MA + (v_AB * clamped); 56 | } 57 | 58 | Eigen::Vector3d CalcClosestPointOnTriangle( 59 | const Eigen::Vector3d& p_Mv1, const Eigen::Vector3d& p_Mv2, 60 | const Eigen::Vector3d& p_Mv3, const Eigen::Vector3d& normal, 61 | const Eigen::Vector3d& p_MQ) 62 | { 63 | Eigen::Vector3d p_MQclosest; 64 | 65 | if (PointProjectsInsideTriangle(p_Mv1, p_Mv2, p_Mv3, p_MQ)) 66 | { 67 | const Eigen::Vector3d v_v1Q = p_MQ - p_Mv1; 68 | // Project query point to triangle plane. 69 | const Eigen::Vector3d p_MQprojected = 70 | p_Mv1 + common_robotics_utilities::math::VectorRejection(normal, v_v1Q); 71 | p_MQclosest = p_MQprojected; 72 | } 73 | else 74 | { 75 | const Eigen::Vector3d p_MQclosest12 = 76 | ClosestPointOnLineSegment(p_Mv1, p_Mv2, p_MQ); 77 | const Eigen::Vector3d p_MQclosest23 = 78 | ClosestPointOnLineSegment(p_Mv2, p_Mv3, p_MQ); 79 | const Eigen::Vector3d p_MQclosest31 = 80 | ClosestPointOnLineSegment(p_Mv3, p_Mv1, p_MQ); 81 | const double d_closest12_squared = p_MQclosest12.squaredNorm(); 82 | const double d_closest23_squared = p_MQclosest23.squaredNorm(); 83 | const double d_closest31_squared = p_MQclosest31.squaredNorm(); 84 | if (d_closest12_squared <= d_closest23_squared && 85 | d_closest12_squared <= d_closest31_squared) 86 | { 87 | p_MQclosest = p_MQclosest12; 88 | } 89 | else if (d_closest23_squared <= d_closest12_squared && 90 | d_closest23_squared <= d_closest31_squared) 91 | { 92 | p_MQclosest = p_MQclosest23; 93 | } 94 | else 95 | { 96 | p_MQclosest = p_MQclosest31; 97 | } 98 | } 99 | 100 | return p_MQclosest; 101 | } 102 | } // namespace 103 | 104 | void RasterizeTriangle( 105 | const std::vector& vertices, 106 | const std::vector& triangles, 107 | const size_t triangle_index, 108 | voxelized_geometry_tools::CollisionMap& collision_map, 109 | const bool enforce_collision_map_contains_triangle) 110 | { 111 | if (!collision_map.IsInitialized()) 112 | { 113 | throw std::invalid_argument("collision_map must be initialized"); 114 | } 115 | 116 | const double min_check_radius = collision_map.GetResolution() * 0.5; 117 | const double max_check_radius = min_check_radius * std::sqrt(3.0); 118 | const double max_check_radius_squared = std::pow(max_check_radius, 2.0); 119 | 120 | const Eigen::Vector3i& triangle = triangles.at(triangle_index); 121 | const Eigen::Vector3d& p_Mv1 = vertices.at(static_cast(triangle(0))); 122 | const Eigen::Vector3d& p_Mv2 = vertices.at(static_cast(triangle(1))); 123 | const Eigen::Vector3d& p_Mv3 = vertices.at(static_cast(triangle(2))); 124 | 125 | const Eigen::Vector3d v1v2 = p_Mv2 - p_Mv1; 126 | const Eigen::Vector3d v1v3 = p_Mv3 - p_Mv1; 127 | 128 | // This assumes that the triangle is well-conditioned. Long and skinny 129 | // triangles may lead to problems. 130 | const Eigen::Vector3d normal = v1v2.cross(v1v3); 131 | 132 | const double x_min = std::min({p_Mv1.x(), p_Mv2.x(), p_Mv3.x()}); 133 | const double y_min = std::min({p_Mv1.y(), p_Mv2.y(), p_Mv3.y()}); 134 | const double z_min = std::min({p_Mv1.z(), p_Mv2.z(), p_Mv3.z()}); 135 | 136 | const double x_max = std::max({p_Mv1.x(), p_Mv2.x(), p_Mv3.x()}); 137 | const double y_max = std::max({p_Mv1.y(), p_Mv2.y(), p_Mv3.y()}); 138 | const double z_max = std::max({p_Mv1.z(), p_Mv2.z(), p_Mv3.z()}); 139 | 140 | const auto min_index = 141 | collision_map.LocationToGridIndex(x_min, y_min, z_min); 142 | const auto max_index = 143 | collision_map.LocationToGridIndex(x_max, y_max, z_max); 144 | 145 | for (int64_t x_index = min_index.X(); x_index <= max_index.X(); 146 | x_index++) 147 | { 148 | for (int64_t y_index = min_index.Y(); y_index <= max_index.Y(); 149 | y_index++) 150 | { 151 | for (int64_t z_index = min_index.Z(); z_index <= max_index.Z(); 152 | z_index++) 153 | { 154 | const common_robotics_utilities::voxel_grid::GridIndex 155 | current_index(x_index, y_index, z_index); 156 | const Eigen::Vector3d p_MQ = 157 | collision_map.GridIndexToLocation(current_index).head<3>(); 158 | 159 | const Eigen::Vector3d p_MQclosest = 160 | CalcClosestPointOnTriangle(p_Mv1, p_Mv2, p_Mv3, normal, p_MQ); 161 | 162 | const double distance_squared = (p_MQclosest - p_MQ).squaredNorm(); 163 | 164 | /// TODO(calderpg) Improve this with more a accurate triangle-box check. 165 | /// This is a coarse approximation of true triangle-box collision. 166 | /// The right solution is something along the lines of: 167 | /// 168 | /// if distance_squared > max_check_radius_squared: 169 | /// triangle_intersects = false 170 | /// else if distance_squared < min_check_radius_squared: 171 | /// triangle_intersects = true 172 | /// else: 173 | /// check_index = collision_map.LocationToGridIndex3d(p_MQclosest) 174 | /// if check_index == current_index: 175 | /// triangle_intersects = true; 176 | /// else: 177 | /// Fall back to a better triangle-box check 178 | /// 179 | /// For now the following approximation is enough. 180 | const bool triangle_intersects = 181 | (distance_squared <= max_check_radius_squared); 182 | 183 | if (triangle_intersects) 184 | { 185 | auto query = collision_map.GetIndexMutable(current_index); 186 | if (query) 187 | { 188 | query.Value().SetOccupancy(1.0f); 189 | } 190 | else if (enforce_collision_map_contains_triangle) 191 | { 192 | throw std::runtime_error( 193 | "Triangle is not contained by collision map"); 194 | } 195 | } 196 | } 197 | } 198 | } 199 | } 200 | 201 | void RasterizeMesh( 202 | const std::vector& vertices, 203 | const std::vector& triangles, 204 | voxelized_geometry_tools::CollisionMap& collision_map, 205 | const bool enforce_collision_map_contains_mesh, 206 | const DegreeOfParallelism& parallelism) 207 | { 208 | if (!collision_map.IsInitialized()) 209 | { 210 | throw std::invalid_argument("collision_map must be initialized"); 211 | } 212 | 213 | 214 | // Helper lambda for each item's work 215 | const auto per_item_work = [&](const int32_t, const int64_t triangle_index) 216 | { 217 | RasterizeTriangle( 218 | vertices, triangles, static_cast(triangle_index), collision_map, 219 | enforce_collision_map_contains_mesh); 220 | }; 221 | 222 | // Raycast all points in the pointcloud. Use OpenMP if available, if not fall 223 | // back to manual dispatch via std::async. 224 | StaticParallelForIndexLoop( 225 | parallelism, 0, static_cast(triangles.size()), per_item_work, 226 | ParallelForBackend::BEST_AVAILABLE); 227 | } 228 | 229 | voxelized_geometry_tools::CollisionMap RasterizeMeshIntoCollisionMap( 230 | const std::vector& vertices, 231 | const std::vector& triangles, 232 | const double resolution, 233 | const DegreeOfParallelism& parallelism) 234 | { 235 | if (resolution <= 0.0) 236 | { 237 | throw std::invalid_argument("resolution must be greater than zero"); 238 | } 239 | 240 | Eigen::Vector3d lower_corner = 241 | Eigen::Vector3d::Constant(std::numeric_limits::infinity()); 242 | Eigen::Vector3d upper_corner = 243 | Eigen::Vector3d::Constant(-std::numeric_limits::infinity()); 244 | 245 | for (size_t idx = 0; idx < vertices.size(); idx++) 246 | { 247 | const Eigen::Vector3d& vertex = vertices.at(idx); 248 | lower_corner = lower_corner.cwiseMin(vertex); 249 | upper_corner = upper_corner.cwiseMax(vertex); 250 | } 251 | 252 | const Eigen::Vector3d object_size = upper_corner - lower_corner; 253 | 254 | const double buffer_size = resolution * 2.0; 255 | const common_robotics_utilities::voxel_grid::GridSizes filter_grid_sizes( 256 | resolution, object_size.x() + buffer_size, object_size.y() + buffer_size, 257 | object_size.z() + buffer_size); 258 | 259 | const Eigen::Isometry3d X_OG(Eigen::Translation3d( 260 | lower_corner.x() - resolution, 261 | lower_corner.y() - resolution, 262 | lower_corner.z() - resolution)); 263 | 264 | const voxelized_geometry_tools::CollisionCell empty_cell(0.0f); 265 | voxelized_geometry_tools::CollisionMap collision_map( 266 | X_OG, "mesh", filter_grid_sizes, empty_cell); 267 | 268 | RasterizeMesh(vertices, triangles, collision_map, true, parallelism); 269 | 270 | return collision_map; 271 | } 272 | } // namespace mesh_rasterizer 273 | VGT_NAMESPACE_END 274 | } // namespace voxelized_geometry_tools 275 | 276 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/pointcloud_voxelization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace voxelized_geometry_tools 14 | { 15 | VGT_NAMESPACE_BEGIN 16 | namespace pointcloud_voxelization 17 | { 18 | std::vector GetAvailableBackends() 19 | { 20 | std::vector available_backends; 21 | 22 | const auto cuda_devices = cuda_helpers::GetAvailableDevices(); 23 | for (const auto& cuda_device : cuda_devices) 24 | { 25 | available_backends.push_back(AvailableBackend( 26 | cuda_device, BackendOptions::CUDA)); 27 | } 28 | 29 | const auto opencl_devices = opencl_helpers::GetAvailableDevices(); 30 | for (const auto& opencl_device : opencl_devices) 31 | { 32 | available_backends.push_back(AvailableBackend( 33 | opencl_device, BackendOptions::OPENCL)); 34 | } 35 | 36 | if (common_robotics_utilities::openmp_helpers::IsOmpEnabledInBuild()) 37 | { 38 | available_backends.push_back(AvailableBackend( 39 | "CPU/OpenMP (parallel)", {{"CPU_PARALLELIZE", 1}}, 40 | BackendOptions::CPU)); 41 | } 42 | else 43 | { 44 | available_backends.push_back(AvailableBackend( 45 | "CPU/async (parallel)", {{"CPU_PARALLELIZE", 1}}, 46 | BackendOptions::CPU)); 47 | } 48 | 49 | available_backends.push_back(AvailableBackend( 50 | "CPU (serial)", {{"CPU_PARALLELIZE", 0}}, BackendOptions::CPU)); 51 | 52 | return available_backends; 53 | } 54 | 55 | std::unique_ptr MakePointCloudVoxelizer( 56 | const BackendOptions backend_option, 57 | const std::map& device_options, 58 | const LoggingFunction& logging_fn) 59 | { 60 | if (backend_option == BackendOptions::BEST_AVAILABLE) 61 | { 62 | return MakeBestAvailablePointCloudVoxelizer(device_options, logging_fn); 63 | } 64 | else if (backend_option == BackendOptions::CPU) 65 | { 66 | return std::unique_ptr( 67 | new CpuPointCloudVoxelizer(device_options, logging_fn)); 68 | } 69 | else if (backend_option == BackendOptions::OPENCL) 70 | { 71 | return std::unique_ptr( 72 | new OpenCLPointCloudVoxelizer(device_options, logging_fn)); 73 | } 74 | else if (backend_option == BackendOptions::CUDA) 75 | { 76 | return std::unique_ptr( 77 | new CudaPointCloudVoxelizer(device_options, logging_fn)); 78 | } 79 | else 80 | { 81 | throw std::invalid_argument("Invalid BackendOptions"); 82 | } 83 | } 84 | 85 | std::unique_ptr MakePointCloudVoxelizer( 86 | const AvailableBackend& backend, const LoggingFunction& logging_fn) 87 | { 88 | return MakePointCloudVoxelizer( 89 | backend.BackendOption(), backend.DeviceOptions(), logging_fn); 90 | } 91 | 92 | std::unique_ptr 93 | MakeBestAvailablePointCloudVoxelizer( 94 | const std::map& device_options, 95 | const LoggingFunction& logging_fn) 96 | { 97 | // Since not all voxelizers will be available on all platforms, we try them 98 | // in order of preference. If available (i.e. on NVIDIA platforms), CUDA is 99 | // always preferable to OpenCL, and likewise OpenCL is always preferable to 100 | // CPU-based (OpenMP). If you want a specific implementation, you should 101 | // directly construct the desired voxelizer. 102 | try 103 | { 104 | if (logging_fn) 105 | { 106 | logging_fn("Trying to construct CUDA PointCloud Voxelizer..."); 107 | } 108 | return std::unique_ptr( 109 | new CudaPointCloudVoxelizer(device_options, logging_fn)); 110 | } 111 | catch (const std::runtime_error&) 112 | { 113 | if (logging_fn) 114 | { 115 | logging_fn("CUDA PointCloud Voxelizer is not available"); 116 | } 117 | } 118 | try 119 | { 120 | if (logging_fn) 121 | { 122 | logging_fn("Trying to construct OpenCL PointCloud Voxelizer..."); 123 | } 124 | return std::unique_ptr( 125 | new OpenCLPointCloudVoxelizer(device_options, logging_fn)); 126 | } 127 | catch (const std::runtime_error&) 128 | { 129 | if (logging_fn) 130 | { 131 | logging_fn("OpenCL PointCloud Voxelizer is not available"); 132 | } 133 | } 134 | try 135 | { 136 | if (logging_fn) 137 | { 138 | logging_fn("Trying to construct CPU PointCloud Voxelizer..."); 139 | } 140 | return std::unique_ptr( 141 | new CpuPointCloudVoxelizer(device_options, logging_fn)); 142 | } 143 | catch (const std::runtime_error&) 144 | { 145 | throw std::runtime_error("No PointCloud Voxelizers available"); 146 | } 147 | } 148 | } // namespace pointcloud_voxelization 149 | VGT_NAMESPACE_END 150 | } // namespace voxelized_geometry_tools 151 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/pointcloud_voxelization_ros_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 11 | #include 12 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 13 | #include 14 | #endif 15 | 16 | #include 17 | 18 | namespace voxelized_geometry_tools 19 | { 20 | VGT_NAMESPACE_BEGIN 21 | namespace pointcloud_voxelization 22 | { 23 | 24 | #if VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 2 25 | using PointField = sensor_msgs::msg::PointField; 26 | #elif VOXELIZED_GEOMETRY_TOOLS__SUPPORTED_ROS_VERSION == 1 27 | using PointField = sensor_msgs::PointField; 28 | #endif 29 | 30 | PointCloud2Wrapper::PointCloud2Wrapper( 31 | const PointCloud2* const cloud_ptr, 32 | const Eigen::Isometry3d& origin_transform, const double max_range) 33 | : cloud_ptr_(cloud_ptr), origin_transform_(origin_transform), 34 | max_range_(max_range) 35 | { 36 | if (cloud_ptr_ == nullptr) 37 | { 38 | throw std::invalid_argument("cloud_ptr_ == nullptr"); 39 | } 40 | if (max_range_ <= 0.0) 41 | { 42 | throw std::runtime_error("max_range_ <= 0.0"); 43 | } 44 | // Figure out what the size and offset for XYZ fields in the pointcloud are. 45 | std::map field_type_map; 46 | std::map field_offset_map; 47 | for (const auto& field : cloud_ptr_->fields) 48 | { 49 | field_type_map[field.name] = field.datatype; 50 | field_offset_map[field.name] = static_cast(field.offset); 51 | } 52 | // Check field types 53 | if (field_type_map.at("x") != PointField::FLOAT32) 54 | { 55 | throw std::invalid_argument("PointCloud x field is not FLOAT32"); 56 | } 57 | if (field_type_map.at("y") != PointField::FLOAT32) 58 | { 59 | throw std::invalid_argument("PointCloud y field is not FLOAT32"); 60 | } 61 | if (field_type_map.at("z") != PointField::FLOAT32) 62 | { 63 | throw std::invalid_argument("PointCloud z field is not FLOAT32"); 64 | } 65 | // Check that x, y, and z are sequential. 66 | const size_t x_offset = field_offset_map.at("x"); 67 | const size_t y_offset = field_offset_map.at("y"); 68 | const size_t z_offset = field_offset_map.at("z"); 69 | if ((z_offset - y_offset) == sizeof(float) && 70 | (y_offset - x_offset) == sizeof(float)) 71 | { 72 | xyz_offset_from_point_start_ = x_offset; 73 | } 74 | else 75 | { 76 | throw std::invalid_argument( 77 | "PointCloud does not have sequential xyz fields"); 78 | } 79 | } 80 | } // namespace pointcloud_voxelization 81 | VGT_NAMESPACE_END 82 | } // namespace voxelized_geometry_tools 83 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/signed_distance_field.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | template class voxelized_geometry_tools::SignedDistanceField; 4 | template class voxelized_geometry_tools::SignedDistanceField; 5 | -------------------------------------------------------------------------------- /src/voxelized_geometry_tools/signed_distance_field_generation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 17 | using common_robotics_utilities::parallelism::ParallelForBackend; 18 | using common_robotics_utilities::parallelism::StaticParallelForRangeLoop; 19 | using common_robotics_utilities::parallelism::ThreadWorkRange; 20 | 21 | using Eigen::VectorXd; 22 | using Eigen::MatrixXd; 23 | using VectorXi64 = Eigen::Matrix; 24 | using MatrixXi64 = Eigen::Matrix; 25 | 26 | namespace voxelized_geometry_tools 27 | { 28 | VGT_NAMESPACE_BEGIN 29 | namespace signed_distance_field_generation 30 | { 31 | namespace internal 32 | { 33 | namespace 34 | { 35 | // Helpers for Euclidean Distance Transform implementation below 36 | 37 | class XIndexer 38 | { 39 | public: 40 | XIndexer(const int64_t y_index, const int64_t z_index) 41 | : y_index_(y_index), z_index_(z_index) {} 42 | 43 | GridIndex operator()(const int64_t x_index) const 44 | { 45 | return GridIndex(x_index, y_index_, z_index_); 46 | } 47 | 48 | private: 49 | const int64_t y_index_; 50 | const int64_t z_index_; 51 | }; 52 | 53 | class YIndexer 54 | { 55 | public: 56 | YIndexer(const int64_t x_index, const int64_t z_index) 57 | : x_index_(x_index), z_index_(z_index) {} 58 | 59 | GridIndex operator()(const int64_t y_index) const 60 | { 61 | return GridIndex(x_index_, y_index, z_index_); 62 | } 63 | 64 | private: 65 | const int64_t x_index_; 66 | const int64_t z_index_; 67 | }; 68 | 69 | class ZIndexer 70 | { 71 | public: 72 | ZIndexer(const int64_t x_index, const int64_t y_index) 73 | : x_index_(x_index), y_index_(y_index) {} 74 | 75 | GridIndex operator()(const int64_t z_index) const 76 | { 77 | return GridIndex(x_index_, y_index_, z_index); 78 | } 79 | 80 | private: 81 | const int64_t x_index_; 82 | const int64_t y_index_; 83 | }; 84 | 85 | template 86 | void ComputeOneDimensionDistanceTransformInPlaceBruteForce( 87 | const int64_t num_elements, const Indexer& indexer, 88 | Eigen::Ref scratch_d, EDTDistanceField& distance_field) 89 | { 90 | const auto f = [&](const int64_t element) 91 | { 92 | return distance_field.GetIndexImmutable(indexer(element)).Value(); 93 | }; 94 | 95 | const auto set_f = [&](const int64_t element, const double value) 96 | { 97 | return distance_field.SetIndex(indexer(element), value); 98 | }; 99 | 100 | const auto square = 101 | [](const int64_t value) { return static_cast(value * value); }; 102 | 103 | // Reset elements in scratch space 104 | scratch_d.setConstant(std::numeric_limits::infinity()); 105 | 106 | for (int64_t q = 0; q < num_elements; q++) 107 | { 108 | for (int64_t other = 0; other < num_elements; other++) 109 | { 110 | const double dist = square(q - other) + f(other); 111 | if (dist < scratch_d(q)) 112 | { 113 | scratch_d(q) = dist; 114 | } 115 | } 116 | } 117 | 118 | for (int64_t q = 0; q < num_elements; q++) 119 | { 120 | set_f(q, scratch_d(q)); 121 | } 122 | } 123 | 124 | template 125 | void ComputeOneDimensionDistanceTransformInPlaceLinear( 126 | const int64_t num_elements, const Indexer& indexer, 127 | Eigen::Ref scratch_z, Eigen::Ref scratch_v, 128 | Eigen::Ref scratch_d, EDTDistanceField& distance_field) 129 | { 130 | const auto f = [&](const int64_t element) 131 | { 132 | return distance_field.GetIndexImmutable(indexer(element)).Value(); 133 | }; 134 | 135 | const auto set_f = [&](const int64_t element, const double value) 136 | { 137 | return distance_field.SetIndex(indexer(element), value); 138 | }; 139 | 140 | const auto square = 141 | [](const int64_t value) { return static_cast(value * value); }; 142 | 143 | // Reset elements in scratch space 144 | scratch_z.setZero(); 145 | scratch_v.setZero(); 146 | scratch_d.setZero(); 147 | 148 | // Initialize 149 | scratch_z(0) = -std::numeric_limits::infinity(); 150 | scratch_z(1) = std::numeric_limits::infinity(); 151 | 152 | // Helper to compute subtraction without producing NaN values. 153 | const auto sub = [](const double a, const double b) 154 | { 155 | constexpr double infinity = std::numeric_limits::infinity(); 156 | if (a == infinity && b == infinity) 157 | { 158 | return 0.0; 159 | } 160 | else if (a == infinity) 161 | { 162 | return infinity; 163 | } 164 | else if (b == infinity) 165 | { 166 | return -infinity; 167 | } 168 | else 169 | { 170 | return a - b; 171 | } 172 | }; 173 | 174 | // Helper for computing intermediate value s 175 | const auto compute_s = [&](const int64_t q, const int64_t k) 176 | { 177 | const int64_t v_k = scratch_v(k); 178 | const double fq = f(q); 179 | const double fv_k = f(v_k); 180 | const double top = sub((fq + square(q)), (fv_k + square(v_k))); 181 | const double bottom = static_cast((2 * q) - (2 * v_k)); 182 | const double s = top / bottom; 183 | return s; 184 | }; 185 | 186 | // Phase 1 187 | { 188 | int64_t k = 0; 189 | 190 | for (int64_t q = 1; q < num_elements; q++) 191 | { 192 | double s = compute_s(q, k); 193 | while (k > 0 && s <= scratch_z(k)) 194 | { 195 | k--; 196 | s = compute_s(q, k); 197 | } 198 | 199 | k++; 200 | scratch_v(k) = q; 201 | scratch_z(k) = s; 202 | scratch_z(k + 1) = std::numeric_limits::infinity(); 203 | } 204 | } 205 | 206 | // Phase 2 207 | { 208 | int64_t k = 0; 209 | 210 | for (int q = 0; q < num_elements; q++) 211 | { 212 | while (scratch_z(k + 1) < q) 213 | { 214 | k++; 215 | } 216 | 217 | const int64_t v_k = scratch_v(k); 218 | scratch_d(q) = square(q - v_k) + f(v_k); 219 | } 220 | 221 | for (int q = 0; q < num_elements; q++) 222 | { 223 | set_f(q, scratch_d(q)); 224 | } 225 | } 226 | } 227 | 228 | template 229 | void ComputeOneDimensionDistanceTransformInPlace( 230 | const int64_t num_elements, const Indexer& indexer, 231 | Eigen::Ref scratch_z, Eigen::Ref scratch_v, 232 | Eigen::Ref scratch_d, EDTDistanceField& distance_field) 233 | { 234 | // We expect the brute force O(num_elements^2) strategy to be faster for small 235 | // numbers of elements than the more complex O(num_elements) strategy. 236 | constexpr int64_t kStrategyThreshold = 8; 237 | 238 | if (num_elements > kStrategyThreshold) 239 | { 240 | ComputeOneDimensionDistanceTransformInPlaceLinear( 241 | num_elements, indexer, scratch_z, scratch_v, scratch_d, distance_field); 242 | } 243 | else 244 | { 245 | ComputeOneDimensionDistanceTransformInPlaceBruteForce( 246 | num_elements, indexer, scratch_d, distance_field); 247 | } 248 | } 249 | } // namespace 250 | 251 | // Implementation of the 3-dimensional Euclidean Distance Transform based on a 252 | // sequence of 1-dimensonal distance transforms adapted from 253 | // "Distance Transforms of Sampled Functions",Felzenszwalb & Huttenlocker, 2012. 254 | // Variable names match those used in the paper pseudocode (e.g. f, k, z, v, d), 255 | // with adaptations to allow scratch space reuse between iterations to reduce 256 | // the need for dynamic memory allocations. Parallelism is achieved by 257 | // dispatching multiple 1-dimensional distance transforms simultaneuously. 258 | void ComputeDistanceFieldTransformInPlace( 259 | const DegreeOfParallelism& parallelism, EDTDistanceField& distance_field) 260 | { 261 | const int64_t num_x_cells = distance_field.GetNumXCells(); 262 | const int64_t num_y_cells = distance_field.GetNumYCells(); 263 | const int64_t num_z_cells = distance_field.GetNumZCells(); 264 | 265 | const auto get_indicies_from_iteration = [&]( 266 | const int64_t step, const int64_t iteration) 267 | { 268 | const int64_t first_index = iteration / step; 269 | const int64_t second_index = iteration % step; 270 | return std::make_pair(first_index, second_index); 271 | }; 272 | 273 | const int32_t num_threads = parallelism.GetNumThreads(); 274 | 275 | // Transform along X axis 276 | if (num_x_cells > 1) 277 | { 278 | // Allocate scratch space (don't need to initialize, since transform 279 | // functions reset their scratch space as needed). 280 | 281 | // Note, z requires an additional element. 282 | MatrixXd full_scratch_z(num_x_cells + 1, num_threads); 283 | MatrixXi64 full_scratch_v(num_x_cells, num_threads); 284 | MatrixXd full_scratch_d(num_x_cells, num_threads); 285 | 286 | const auto per_range_work = [&](const ThreadWorkRange& work_range) 287 | { 288 | const int32_t thread_num = work_range.GetThreadNum(); 289 | Eigen::Ref scratch_z = full_scratch_z.col(thread_num); 290 | Eigen::Ref scratch_v = full_scratch_v.col(thread_num); 291 | Eigen::Ref scratch_d = full_scratch_d.col(thread_num); 292 | 293 | for (int64_t iteration = work_range.GetRangeStart(); 294 | iteration < work_range.GetRangeEnd(); 295 | iteration++) 296 | { 297 | const auto indices = 298 | get_indicies_from_iteration(num_z_cells, iteration); 299 | const int64_t y_index = indices.first; 300 | const int64_t z_index = indices.second; 301 | 302 | ComputeOneDimensionDistanceTransformInPlace( 303 | num_x_cells, XIndexer(y_index, z_index), scratch_z, scratch_v, 304 | scratch_d, distance_field); 305 | } 306 | }; 307 | 308 | const int64_t iterations = num_y_cells * num_z_cells; 309 | StaticParallelForRangeLoop( 310 | parallelism, 0, iterations, per_range_work, 311 | ParallelForBackend::BEST_AVAILABLE); 312 | } 313 | 314 | // Transform along Y axis 315 | if (num_y_cells > 1) 316 | { 317 | // Allocate scratch space (don't need to initialize, since transform 318 | // functions reset their scratch space as needed). 319 | 320 | // Note, z requires an additional element. 321 | MatrixXd full_scratch_z(num_y_cells + 1, num_threads); 322 | MatrixXi64 full_scratch_v(num_y_cells, num_threads); 323 | MatrixXd full_scratch_d(num_y_cells, num_threads); 324 | 325 | const auto per_range_work = [&](const ThreadWorkRange& work_range) 326 | { 327 | const int32_t thread_num = work_range.GetThreadNum(); 328 | Eigen::Ref scratch_z = full_scratch_z.col(thread_num); 329 | Eigen::Ref scratch_v = full_scratch_v.col(thread_num); 330 | Eigen::Ref scratch_d = full_scratch_d.col(thread_num); 331 | 332 | for (int64_t iteration = work_range.GetRangeStart(); 333 | iteration < work_range.GetRangeEnd(); 334 | iteration++) 335 | { 336 | const auto indices = 337 | get_indicies_from_iteration(num_z_cells, iteration); 338 | const int64_t x_index = indices.first; 339 | const int64_t z_index = indices.second; 340 | 341 | ComputeOneDimensionDistanceTransformInPlace( 342 | num_y_cells, YIndexer(x_index, z_index), scratch_z, scratch_v, 343 | scratch_d, distance_field); 344 | } 345 | }; 346 | 347 | const int64_t iterations = num_x_cells * num_z_cells; 348 | StaticParallelForRangeLoop( 349 | parallelism, 0, iterations, per_range_work, 350 | ParallelForBackend::BEST_AVAILABLE); 351 | } 352 | 353 | // Transform along Z axis 354 | if (num_z_cells > 1) 355 | { 356 | // Allocate scratch space (don't need to initialize, since transform 357 | // functions reset their scratch space as needed). 358 | 359 | // Note, z requires an additional element. 360 | MatrixXd full_scratch_z(num_z_cells + 1, num_threads); 361 | MatrixXi64 full_scratch_v(num_z_cells, num_threads); 362 | MatrixXd full_scratch_d(num_z_cells, num_threads); 363 | 364 | const auto per_range_work = [&](const ThreadWorkRange& work_range) 365 | { 366 | const int32_t thread_num = work_range.GetThreadNum(); 367 | Eigen::Ref scratch_z = full_scratch_z.col(thread_num); 368 | Eigen::Ref scratch_v = full_scratch_v.col(thread_num); 369 | Eigen::Ref scratch_d = full_scratch_d.col(thread_num); 370 | 371 | for (int64_t iteration = work_range.GetRangeStart(); 372 | iteration < work_range.GetRangeEnd(); 373 | iteration++) 374 | { 375 | const auto indices = 376 | get_indicies_from_iteration(num_y_cells, iteration); 377 | const int64_t x_index = indices.first; 378 | const int64_t y_index = indices.second; 379 | 380 | ComputeOneDimensionDistanceTransformInPlace( 381 | num_z_cells, ZIndexer(x_index, y_index), scratch_z, scratch_v, 382 | scratch_d, distance_field); 383 | } 384 | }; 385 | 386 | const int64_t iterations = num_x_cells * num_y_cells; 387 | StaticParallelForRangeLoop( 388 | parallelism, 0, iterations, per_range_work, 389 | ParallelForBackend::BEST_AVAILABLE); 390 | } 391 | } 392 | } // namespace internal 393 | } // namespace signed_distance_field_generation 394 | VGT_NAMESPACE_END 395 | } // namespace voxelized_geometry_tools 396 | -------------------------------------------------------------------------------- /test/mesh_rasterization_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 9 | 10 | namespace voxelized_geometry_tools 11 | { 12 | namespace 13 | { 14 | class MeshRasterizationTestSuite 15 | : public testing::TestWithParam {}; 16 | 17 | TEST_P(MeshRasterizationTestSuite, Test) 18 | { 19 | const DegreeOfParallelism parallelism = GetParam(); 20 | std::cout << "# of threads = " << parallelism.GetNumThreads() << std::endl; 21 | 22 | const std::vector vertices = { 23 | Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Vector3d(1.0, 0.0, 0.0), 24 | Eigen::Vector3d(0.0, 1.0, 0.0) }; 25 | const std::vector triangles = { Eigen::Vector3i(0, 1, 2) }; 26 | 27 | const double resolution = 0.125; 28 | 29 | const auto collision_map = mesh_rasterizer::RasterizeMeshIntoCollisionMap( 30 | vertices, triangles, resolution, parallelism); 31 | 32 | const auto get_cell_occupancy = 33 | [&] (const int64_t x, const int64_t y, const int64_t z) 34 | { 35 | return collision_map.GetIndexImmutable(x, y, z).Value().Occupancy(); 36 | }; 37 | 38 | // Due to how the triangle discretizes, we expect the lower layer to be empty. 39 | for (int64_t xidx = 0; xidx < collision_map.GetNumXCells(); xidx++) 40 | { 41 | for (int64_t yidx = 0; yidx < collision_map.GetNumYCells(); yidx++) 42 | { 43 | EXPECT_EQ(get_cell_occupancy(xidx, yidx, 0), 0.0f); 44 | } 45 | } 46 | 47 | // Check the upper layer of voxels. 48 | for (int64_t xidx = 0; xidx < collision_map.GetNumXCells(); xidx++) 49 | { 50 | for (int64_t yidx = 0; yidx < collision_map.GetNumYCells(); yidx++) 51 | { 52 | if (xidx == 0 || yidx == 0) 53 | { 54 | EXPECT_EQ(get_cell_occupancy(xidx, yidx, 1), 0.0f); 55 | } 56 | else if (yidx >= (collision_map.GetNumYCells() - xidx)) 57 | { 58 | EXPECT_EQ(get_cell_occupancy(xidx, yidx, 1), 0.0f); 59 | } 60 | else 61 | { 62 | EXPECT_EQ(get_cell_occupancy(xidx, yidx, 1), 1.0f); 63 | } 64 | } 65 | } 66 | } 67 | 68 | INSTANTIATE_TEST_SUITE_P( 69 | SerialMeshRasterizationTest, MeshRasterizationTestSuite, 70 | testing::Values(DegreeOfParallelism::None())); 71 | 72 | // For fallback testing on platforms with no OpenMP support, specify 2 threads. 73 | int32_t GetNumThreads() 74 | { 75 | if (common_robotics_utilities::openmp_helpers::IsOmpEnabledInBuild()) 76 | { 77 | return common_robotics_utilities::openmp_helpers::GetNumOmpThreads(); 78 | } 79 | else 80 | { 81 | return 2; 82 | } 83 | } 84 | 85 | INSTANTIATE_TEST_SUITE_P( 86 | ParallelMeshRasterizationTest, MeshRasterizationTestSuite, 87 | testing::Values(DegreeOfParallelism(GetNumThreads()))); 88 | } // namespace 89 | } // namespace voxelized_geometry_tools 90 | 91 | int main(int argc, char** argv) 92 | { 93 | testing::InitGoogleTest(&argc, argv); 94 | return RUN_ALL_TESTS(); 95 | } 96 | 97 | -------------------------------------------------------------------------------- /test/pointcloud_voxelization_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 17 | 18 | namespace voxelized_geometry_tools 19 | { 20 | namespace 21 | { 22 | using pointcloud_voxelization::AvailableBackend; 23 | using pointcloud_voxelization::PointCloudVoxelizationFilterOptions; 24 | using pointcloud_voxelization::PointCloudVoxelizationInterface; 25 | using pointcloud_voxelization::PointCloudWrapper; 26 | using pointcloud_voxelization::PointCloudWrapperSharedPtr; 27 | 28 | class PointCloudVoxelizationTestSuite 29 | : public testing::TestWithParam {}; 30 | 31 | class VectorVector3dPointCloudWrapper : public PointCloudWrapper 32 | { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | 36 | void PushBack(const Eigen::Vector3d& point) 37 | { 38 | points_.push_back(point); 39 | } 40 | 41 | double MaxRange() const override 42 | { 43 | return std::numeric_limits::infinity(); 44 | } 45 | 46 | int64_t Size() const override { return static_cast(points_.size()); } 47 | 48 | const Eigen::Isometry3d& GetPointCloudOriginTransform() const override 49 | { 50 | return origin_transform_; 51 | } 52 | 53 | void SetPointCloudOriginTransform( 54 | const Eigen::Isometry3d& origin_transform) override 55 | { 56 | origin_transform_ = origin_transform; 57 | } 58 | 59 | private: 60 | void CopyPointLocationIntoDoublePtrImpl( 61 | const int64_t point_index, double* destination) const override 62 | { 63 | const Eigen::Vector3d& point = points_.at(static_cast(point_index)); 64 | std::memcpy(destination, point.data(), sizeof(double) * 3); 65 | } 66 | 67 | void CopyPointLocationIntoFloatPtrImpl( 68 | const int64_t point_index, float* destination) const override 69 | { 70 | const Eigen::Vector3f point = 71 | points_.at(static_cast(point_index)).cast(); 72 | std::memcpy(destination, point.data(), sizeof(float) * 3); 73 | } 74 | 75 | common_robotics_utilities::math::VectorVector3d points_; 76 | Eigen::Isometry3d origin_transform_ = Eigen::Isometry3d::Identity(); 77 | }; 78 | 79 | void check_empty_voxelization(const CollisionMap& occupancy) 80 | { 81 | // Make sure the grid is properly filled 82 | for (int64_t xidx = 0; xidx < occupancy.GetNumXCells(); xidx++) 83 | { 84 | for (int64_t yidx = 0; yidx < occupancy.GetNumYCells(); yidx++) 85 | { 86 | for (int64_t zidx = 0; zidx < occupancy.GetNumZCells(); zidx++) 87 | { 88 | // Check grid querying 89 | const auto occupancy_query = 90 | occupancy.GetIndexImmutable(xidx, yidx, zidx); 91 | // Check grid values 92 | const float cmap_occupancy = occupancy_query.Value().Occupancy(); 93 | // Check the bottom cells 94 | if (zidx == 0) 95 | { 96 | ASSERT_EQ(cmap_occupancy, 1.0f); 97 | } 98 | // All other cells should be marked "unknown" 99 | else 100 | { 101 | ASSERT_EQ(cmap_occupancy, 0.5f); 102 | } 103 | } 104 | } 105 | } 106 | } 107 | 108 | void check_voxelization(const CollisionMap& occupancy) 109 | { 110 | // Make sure the grid is properly filled 111 | for (int64_t xidx = 0; xidx < occupancy.GetNumXCells(); xidx++) 112 | { 113 | for (int64_t yidx = 0; yidx < occupancy.GetNumYCells(); yidx++) 114 | { 115 | for (int64_t zidx = 0; zidx < occupancy.GetNumZCells(); zidx++) 116 | { 117 | // Check grid querying 118 | const auto occupancy_query = 119 | occupancy.GetIndexImmutable(xidx, yidx, zidx); 120 | // Check grid values 121 | const float cmap_occupancy = occupancy_query.Value().Occupancy(); 122 | // Check the bottom cells 123 | if (zidx == 0) 124 | { 125 | ASSERT_EQ(cmap_occupancy, 1.0f); 126 | } 127 | // Check a few "seen empty" cells 128 | if ((xidx == 3) && (yidx >= 3) && (zidx >= 1)) 129 | { 130 | ASSERT_EQ(cmap_occupancy, 0.0f); 131 | } 132 | if ((xidx >= 3) && (yidx == 3) && (zidx >= 1)) 133 | { 134 | ASSERT_EQ(cmap_occupancy, 0.0f); 135 | } 136 | // Check a few "seen filled" cells 137 | if ((xidx == 4) && (yidx >= 4) && (zidx >= 1)) 138 | { 139 | ASSERT_EQ(cmap_occupancy, 1.0f); 140 | } 141 | if ((xidx >= 4) && (yidx == 4) && (zidx >= 1)) 142 | { 143 | ASSERT_EQ(cmap_occupancy, 1.0f); 144 | } 145 | // Check shadowed cells 146 | if ((xidx > 4) && (yidx > 4) && (zidx >= 1)) 147 | { 148 | ASSERT_EQ(cmap_occupancy, 0.5f); 149 | } 150 | } 151 | } 152 | } 153 | } 154 | 155 | TEST_P(PointCloudVoxelizationTestSuite, Test) 156 | { 157 | const DegreeOfParallelism parallelism = GetParam(); 158 | std::cout << "# of threads = " << parallelism.GetNumThreads() << std::endl; 159 | 160 | // Make the static environment 161 | const Eigen::Isometry3d X_WG(Eigen::Translation3d(-1.0, -1.0, -1.0)); 162 | // Grid 2m in each axis 163 | const double x_size = 2.0; 164 | const double y_size = 2.0; 165 | const double z_size = 2.0; 166 | // 1/4 meter resolution, so 8 cells/axis 167 | const double grid_resolution = 0.25; 168 | const double step_size_multiplier = 0.5; 169 | const CollisionCell empty_cell(0.0f); 170 | const CollisionCell filled_cell(1.0f); 171 | 172 | const common_robotics_utilities::voxel_grid::GridSizes grid_size( 173 | grid_resolution, x_size, y_size, z_size); 174 | CollisionMap static_environment(X_WG, "world", grid_size, empty_cell); 175 | 176 | // Set the bottom cells filled 177 | for (int64_t xidx = 0; xidx < static_environment.GetNumXCells(); xidx++) 178 | { 179 | for (int64_t yidx = 0; yidx < static_environment.GetNumYCells(); yidx++) 180 | { 181 | static_environment.SetIndex(xidx, yidx, 0, filled_cell); 182 | } 183 | } 184 | 185 | // Make some test pointclouds 186 | // Make the physical->optical frame transform 187 | const Eigen::Isometry3d X_CO = Eigen::Translation3d(0.0, 0.0, 0.0) * 188 | Eigen::Quaterniond(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitZ()) * 189 | Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX())); 190 | 191 | // Camera 1 pose 192 | const Eigen::Isometry3d X_WC1(Eigen::Translation3d(-2.0, 0.0, 0.0)); 193 | const Eigen::Isometry3d X_WC1O = X_WC1 * X_CO; 194 | PointCloudWrapperSharedPtr cam1_cloud(new VectorVector3dPointCloudWrapper()); 195 | static_cast(cam1_cloud.get()) 196 | ->SetPointCloudOriginTransform(X_WC1O); 197 | for (double x = -2.0; x <= 2.0; x += 0.03125) 198 | { 199 | for (double y = -2.0; y <= 2.0; y += 0.03125) 200 | { 201 | const double z = (x <= 0.0) ? 2.125 : 4.0; 202 | static_cast(cam1_cloud.get())->PushBack( 203 | Eigen::Vector3d(x, y, z)); 204 | } 205 | } 206 | 207 | // Camera 2 pose 208 | const Eigen::Isometry3d X_WC2 = Eigen::Translation3d(0.0, -2.0, 0.0) * 209 | Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); 210 | const Eigen::Isometry3d X_WC2O = X_WC2 * X_CO; 211 | PointCloudWrapperSharedPtr cam2_cloud(new VectorVector3dPointCloudWrapper()); 212 | static_cast(cam2_cloud.get()) 213 | ->SetPointCloudOriginTransform(X_WC2O); 214 | for (double x = -2.0; x <= 2.0; x += 0.03125) 215 | { 216 | for (double y = -2.0; y <= 2.0; y += 0.03125) 217 | { 218 | const double z = (x >= 0.0) ? 2.125 : 4.0; 219 | static_cast(cam2_cloud.get())->PushBack( 220 | Eigen::Vector3d(x, y, z)); 221 | } 222 | } 223 | 224 | // Camera 3 pose 225 | const Eigen::Isometry3d X_WC3 = Eigen::Isometry3d::Identity(); 226 | const Eigen::Isometry3d X_WC3O = X_WC3 * X_CO; 227 | // Cloud 3 is empty 228 | PointCloudWrapperSharedPtr cam3_cloud(new VectorVector3dPointCloudWrapper()); 229 | static_cast(cam3_cloud.get()) 230 | ->SetPointCloudOriginTransform(X_WC3O); 231 | 232 | // Make control parameters 233 | // We require that 100% of points from the camera see through to see a voxel 234 | // as free. 235 | const double percent_seen_free = 1.0; 236 | // We don't worry about outliers. 237 | const int32_t outlier_points_threshold = 1; 238 | // We only need one camera to see a voxel as free. 239 | const int32_t num_cameras_seen_free = 1; 240 | const PointCloudVoxelizationFilterOptions filter_options( 241 | percent_seen_free, outlier_points_threshold, num_cameras_seen_free); 242 | 243 | // Set dispatch parallelism options 244 | std::map parallelism_options; 245 | parallelism_options["DISPATCH_PARALLELIZE"] = 246 | (parallelism.IsParallel()) ? 1 : 0; 247 | parallelism_options["DISPATCH_NUM_THREADS"] = parallelism.GetNumThreads(); 248 | parallelism_options["CPU_NUM_THREADS"] = parallelism.GetNumThreads(); 249 | 250 | const auto add_options_to_backend = [] ( 251 | const AvailableBackend& backend, 252 | const std::map& options) 253 | { 254 | std::map merged_options = backend.DeviceOptions(); 255 | for (auto itr = options.begin(); itr != options.end(); ++itr) 256 | { 257 | merged_options[itr->first] = itr->second; 258 | } 259 | 260 | return AvailableBackend( 261 | backend.DeviceName(), merged_options, backend.BackendOption()); 262 | }; 263 | 264 | // Run all available voxelizers 265 | const auto available_backends = 266 | pointcloud_voxelization::GetAvailableBackends(); 267 | const pointcloud_voxelization::LoggingFunction logging_fn = 268 | [] (const std::string& msg) { std::cout << msg << std::endl; }; 269 | 270 | for (size_t idx = 0; idx < available_backends.size(); idx++) 271 | { 272 | const auto available_backend = 273 | add_options_to_backend(available_backends.at(idx), parallelism_options); 274 | std::cout << "Trying voxelizer [" << idx << "] " 275 | << available_backend.DeviceName() << std::endl; 276 | 277 | auto voxelizer = pointcloud_voxelization::MakePointCloudVoxelizer( 278 | available_backend, logging_fn); 279 | 280 | const auto empty_voxelized = voxelizer->VoxelizePointClouds( 281 | static_environment, step_size_multiplier, filter_options, {}); 282 | check_empty_voxelization(empty_voxelized); 283 | 284 | const auto voxelized = voxelizer->VoxelizePointClouds( 285 | static_environment, step_size_multiplier, filter_options, 286 | {cam1_cloud, cam2_cloud, cam3_cloud}); 287 | check_voxelization(voxelized); 288 | } 289 | 290 | // Ensure all voxelizers support null logging functions 291 | for (size_t idx = 0; idx < available_backends.size(); idx++) 292 | { 293 | const auto& available_backend = available_backends.at(idx); 294 | std::cout << "Trying voxelizer [" << idx << "] " 295 | << available_backend.DeviceName() << std::endl; 296 | 297 | ASSERT_NO_THROW(pointcloud_voxelization::MakePointCloudVoxelizer( 298 | available_backend, {})); 299 | } 300 | 301 | // Ensure that MakeBestAvailablePointCloudVoxelizer works, with empty device 302 | // options and null logging function. 303 | ASSERT_NO_THROW( 304 | pointcloud_voxelization::MakeBestAvailablePointCloudVoxelizer({}, {})); 305 | } 306 | 307 | INSTANTIATE_TEST_SUITE_P( 308 | SerialDispatchPointCloudVoxelizationTest, PointCloudVoxelizationTestSuite, 309 | testing::Values(DegreeOfParallelism::None())); 310 | 311 | // For fallback testing on platforms with no OpenMP support, specify 2 threads. 312 | int32_t GetNumThreads() 313 | { 314 | if (common_robotics_utilities::openmp_helpers::IsOmpEnabledInBuild()) 315 | { 316 | return common_robotics_utilities::openmp_helpers::GetNumOmpThreads(); 317 | } 318 | else 319 | { 320 | return 2; 321 | } 322 | } 323 | 324 | INSTANTIATE_TEST_SUITE_P( 325 | ParallelDispatchPointCloudVoxelizationTest, PointCloudVoxelizationTestSuite, 326 | testing::Values(DegreeOfParallelism(GetNumThreads()))); 327 | } // namespace 328 | } // namespace voxelized_geometry_tools 329 | 330 | int main(int argc, char** argv) 331 | { 332 | testing::InitGoogleTest(&argc, argv); 333 | return RUN_ALL_TESTS(); 334 | } 335 | 336 | --------------------------------------------------------------------------------