mirrors/unknownfreeoccupied--ufomap
1
0
Fork 0
Code Issues Pull Requests Actions Deployments Models Agent Notes Packages Projects Releases Wiki Activity

chore: import upstream snapshot with attribution

Browse Source
This commit is contained in:
wehub-resource-sync
2026-07-13 12:36:50 +08:00
commit e316dfb128
99 changed files with 16722 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+32
View File
@@ -0,0 +1,32 @@
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
LICENSE
+25
View File
@@ -0,0 +1,25 @@
BSD 2-Clause License
Copyright (c) 2020, UFO
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
README.md
Executable
+44
View File
@@ -0,0 +1,44 @@
# UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
UFOMap is an efficient probabilistic 3D mapping framework with an explicit representation of unknown space.
![UFOMap visualization](https://user-images.githubusercontent.com/6604166/111369561-7bcabd00-8697-11eb-8145-1409ef9709a0.png)
UFOMap with occupied space as colored voxels and unknown space as white transparent voxels. The free space is not visualized here.
Using UFOMap you will be able to create 3D volumetric maps, containing unknown/free/occupied space, similar to the one below in real-time. The UFOMap maps you create can be used for efficient path/trajectory planning, obstacle avoidance, reconstruction, and more.
![Real-time Volumetric Mapping](https://user-images.githubusercontent.com/6604166/111788476-6c7a8800-88c0-11eb-9592-86609b9eb57c.png)
Colored UFOMap constructed in real-time (2 Hz) at 2 mm voxel size.
## Table of Contents
Please see the [Wiki](https://github.com/UnknownFreeOccupied/ufomap/wiki) for how to install and use UFOMap.
1. [Setup](https://github.com/UnknownFreeOccupied/ufomap/wiki/Setup)
2. [Tutorials](https://github.com/UnknownFreeOccupied/ufomap/wiki/Tutorials)
3. [ROS Tutorials](https://github.com/UnknownFreeOccupied/ufomap/wiki/ROS-Tutorials)
4. [Advanced ROS Tutorials](https://github.com/UnknownFreeOccupied/ufomap/wiki/Advanced-ROS-Tutorials)
5. [Performance](https://github.com/UnknownFreeOccupied/ufomap/wiki/Performance)
6. [Example Outputs](https://github.com/UnknownFreeOccupied/ufomap/wiki/Example-Outputs)
7. [Data Repository](https://github.com/UnknownFreeOccupied/ufomap/wiki/Data-Repository)
8. [API](https://github.com/UnknownFreeOccupied/ufomap/wiki/API)
## Credits
### Paper
* [IEEE](https://ieeexplore.ieee.org/abstract/document/9158399)
* [ArXiv](https://arxiv.org/abs/2003.04749)
### Cite
If you use UFOMap in a scientific publication, please cite the following paper:
* Daniel Duberg and Patric Jensfelt, "UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown," in IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 6411-6418, Oct. 2020, doi: 10.1109/LRA.2020.3013861.
```latex
@article{duberg2020ufomap,
author={Daniel Duberg and Patric Jensfelt},
journal={IEEE Robotics and Automation Letters},
title={{UFOMap}: An Efficient Probabilistic {3D} Mapping Framework That Embraces the Unknown},
year={2020},
volume={5},
number={4},
pages={6411-6418},
doi={10.1109/LRA.2020.3013861}
}
```
### Videos
* [YouTube Playlist](https://youtube.com/playlist?list=PLoZnKRp2UVom4bv2fUVXgI5VCbuTrfrU3)
README.wehub.md
+7
View File
@@ -0,0 +1,7 @@
# WeHub 来源说明
- 原始项目：`UnknownFreeOccupied/ufomap`
- 原始仓库：https://github.com/UnknownFreeOccupied/ufomap
- 导入方式：上游默认分支的最新快照
- 原作者、版权和许可证信息以原始仓库及本仓库 LICENSE 为准
- 本文件仅用于记录来源，不代表 WeHub 是原项目作者
ufomap/.clang-format
Executable
+167
View File
@@ -0,0 +1,167 @@
---
Language: Cpp
BasedOnStyle: Google
AccessModifierOffset: -1
AlignAfterOpenBracket: Align
AlignConsecutiveMacros: false
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllArgumentsOnNextLine: true
AllowAllConstructorInitializersOnNextLine: true
AllowAllParametersOfDeclarationOnNextLine: true
# AllowShortBlocksOnASingleLine: Never
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortLambdasOnASingleLine: All
AllowShortIfStatementsOnASingleLine: WithoutElse
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterCaseLabel: true
AfterClass: true
AfterControlStatement: true
AfterEnum: true
AfterFunction: true
AfterNamespace: true
AfterObjCDeclaration: true
AfterStruct: true
AfterUnion: true
AfterExternBlock: true
BeforeCatch: true
BeforeElse: true
IndentBraces: true
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Linux # Set to Custom to use BraceWrapping
BreakBeforeInheritanceComma: false
BreakInheritanceList: BeforeColon
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakConstructorInitializers: BeforeColon
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 90
CommentPragmas: "^ IWYU pragma:"
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
# DeriveLineEnding: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
FixNamespaceComments: true
ForEachMacros:
- foreach
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Regroup
IncludeCategories:
- Regex: '^<ext/.*\.h>'
Priority: 2
# SortPriority: 0
- Regex: '^<.*\.h>'
Priority: 1
# SortPriority: 0
- Regex: "^<.*"
Priority: 2
# SortPriority: 0
- Regex: ".*"
Priority: 3
# SortPriority: 0
IncludeIsMainRegex: "([-_](test|unittest))?$"
# IncludeIsMainSourceRegex: ""
IndentCaseLabels: true
# IndentGotoLabels: true
IndentPPDirectives: None
IndentWidth: 2
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ""
MacroBlockEnd: ""
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBinPackProtocolList: Never
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: true
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyBreakTemplateDeclaration: 10
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
RawStringFormats:
- Language: Cpp
Delimiters:
- cc
- CC
- cpp
- Cpp
- CPP
- "c++"
- "C++"
CanonicalDelimiter: ""
BasedOnStyle: google
- Language: TextProto
Delimiters:
- pb
- PB
- proto
- PROTO
EnclosingFunctions:
- EqualsProto
- EquivToProto
- PARSE_PARTIAL_TEXT_PROTO
- PARSE_TEST_PROTO
- PARSE_TEXT_PROTO
- ParseTextOrDie
- ParseTextProtoOrDie
CanonicalDelimiter: ""
BasedOnStyle: google
ReflowComments: true
SortIncludes: true
SortUsingDeclarations: true
SpaceAfterCStyleCast: false
SpaceAfterLogicalNot: false
SpaceAfterTemplateKeyword: true
SpaceBeforeAssignmentOperators: true
SpaceBeforeCpp11BracedList: false
SpaceBeforeCtorInitializerColon: true
SpaceBeforeInheritanceColon: true
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: true
# SpaceInEmptyBlock: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
# SpacesInConditionalStatement: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
# SpaceBeforeSquareBrackets: false
Standard: Auto
StatementMacros:
- Q_UNUSED
- QT_REQUIRE_VERSION
TabWidth: 2
# UseCRLF: false
UseTab: ForIndentation
---
ufomap/CMakeLists.txt
Executable
+185
View File
@@ -0,0 +1,185 @@
cmake_minimum_required(VERSION 3.9)
project(ufomap
VERSION 1.0.0
DESCRIPTION "UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown"
LANGUAGES CXX
)
set(default_build_type "Release")
if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
message(STATUS "Setting build type to '${default_build_type}' as none was specified.")
set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE
STRING "Choose the type of build." FORCE)
# Set the possible values of build type for cmake-gui
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS
"Debug" "Release" "MinSizeRel" "RelWithDebInfo")
endif()
# Maybe remove this under
# IF (CMAKE_COMPILER_IS_GNUCC)
# SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-error")
# SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-error -Wpedantic -march=native")
# SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG")
# SET (CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
# # Shared object compilation under 64bit (vtable)
# ADD_DEFINITIONS(-fPIC)
# ENDIF()
# Only do these if this is the main project, and not if it is included through add_subdirectory
if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME)
# Support folders in IDE's
set_property(GLOBAL PROPERTY USE_FOLDERS ON)
# Should this be here?
# set(MODERN_CMAKE_BUILD_TESTING ON)
include(CTest)
find_package(Doxygen)
if(Doxygen_FOUND)
add_subdirectory(docs)
else()
message(STATUS "Doxygen not found, not building docs")
endif()
endif()
# To make headers show up in IDEs
set(HEADER_LIST
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/aabb.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/bounding_volume.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/capsule.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/collision_checks.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/cone.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/cylinder.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/ellipsoid.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/frustum.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/line_segment.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/obb.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/plane.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/point.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/ray.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/sphere.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/triangle.h"
"${PROJECT_SOURCE_DIR}/include/ufo/geometry/types.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/iterator/occupancy_map_nearest.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/iterator/occupancy_map.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/iterator/octree_nearest.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/iterator/octree.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/code.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/color.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/key.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/occupancy_map_base.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/occupancy_map_color.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/occupancy_map_node.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/occupancy_map.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/octree_node.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/octree.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/point_cloud.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/types.h"
"${PROJECT_SOURCE_DIR}/include/ufo/map/ufomap.h"
"${PROJECT_SOURCE_DIR}/include/ufo/math/pose6.h"
"${PROJECT_SOURCE_DIR}/include/ufo/math/quaternion.h"
"${PROJECT_SOURCE_DIR}/include/ufo/math/vector3.h"
)
set(SRC_LIST
"${PROJECT_SOURCE_DIR}/src/geometry/bounding_volume.cpp"
"${PROJECT_SOURCE_DIR}/src/geometry/collision_checks.cpp"
"${PROJECT_SOURCE_DIR}/src/map/occupancy_map_color.cpp"
"${PROJECT_SOURCE_DIR}/src/map/occupancy_map.cpp"
)
find_package(PkgConfig REQUIRED)
pkg_check_modules(LZ4 REQUIRED liblz4)
add_library(Map SHARED ${SRC_LIST} ${HEADER_LIST})
add_library(UFO::Map ALIAS Map)
set_target_properties(Map
PROPERTIES
VERSION ${PROJECT_VERSION}
SOVERSION ${PROJECT_VERSION_MAJOR}
CXX_STANDARD 17
CXX_STANDARD_REQUIRED YES
CXX_EXTENSIONS NO
)
target_include_directories(Map
PUBLIC
$<INSTALL_INTERFACE:include>
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
)
target_link_libraries(Map
PUBLIC
${LZ4_LIBRARIES}
tbb
)
target_compile_features(Map
PUBLIC
cxx_std_17
)
target_compile_options(Map
PUBLIC
-fPIC
)
set(UFOMAP_BMI2 FALSE CACHE BOOL "Enable/disable BMI2 instructions")
if(DEFINED ENV{UFOMAP_BMI2})
set(UFOMAP_BMI2 $ENV{UFOMAP_BMI2})
endif(DEFINED ENV{UFOMAP_BMI2})
if(UFOMAP_BMI2)
message(STATUS "UFOMAP BMI2 instructions enabled")
target_compile_options(Map
PUBLIC
-mbmi2
)
else()
message(STATUS "UFOMAP BMI2 instructions disabled")
endif(UFOMAP_BMI2)
# IDEs should put the headers in a nice place
source_group(TREE "${PROJECT_SOURCE_DIR}/include" PREFIX "Header Files" FILES ${HEADER_LIST})
install(TARGETS Map
EXPORT ufomapTargets
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
)
install(EXPORT ufomapTargets
FILE "ufomapTargets.cmake"
NAMESPACE UFO::
DESTINATION lib/cmake/${PROJECT_NAME}
)
include(CMakePackageConfigHelpers)
configure_package_config_file(
"${PROJECT_SOURCE_DIR}/cmake/ufomapConfig.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/ufomapConfig.cmake"
INSTALL_DESTINATION lib/cmake/${PROJECT_NAME}
)
write_basic_package_version_file(
"${CMAKE_CURRENT_BINARY_DIR}/ufomapConfigVersion.cmake"
VERSION ${PROJECT_VERSION}
COMPATIBILITY SameMajorVersion
)
INSTALL(
FILES
"${CMAKE_CURRENT_BINARY_DIR}/ufomapConfig.cmake"
"${CMAKE_CURRENT_BINARY_DIR}/ufomapConfigVersion.cmake"
DESTINATION lib/cmake/${PROJECT_NAME}
)
install(
DIRECTORY ${PROJECT_SOURCE_DIR}/include/
DESTINATION include
)
ufomap/Doxyfile
Executable
+2494
View File
File diff suppressed because it is too large Load Diff
ufomap/README.md
Executable
+36
View File
@@ -0,0 +1,36 @@
# UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
UFOMap is an effienct probabilistic 3D mapping framework with an explicit representation of unknown space.
## Table of Contents
Please see the [Wiki](https://github.com/UnknownFreeOccupied/ufomap/wiki) for how to install and use UFOMap.
1. [Setup](https://github.com/UnknownFreeOccupied/ufomap/wiki/Tutorials)
2. [Tutorials](https://github.com/UnknownFreeOccupied/ufomap/wiki/Setup)
3. [ROS Tutorials](https://github.com/UnknownFreeOccupied/ufomap/wiki/ROS-Tutorials)
4. [Advanced ROS Tutorials](https://github.com/UnknownFreeOccupied/ufomap/wiki/Advanced-ROS-Tutorials)
5. [Performance](https://github.com/UnknownFreeOccupied/ufomap/wiki/Performance)
6. [Example Outputs](https://github.com/UnknownFreeOccupied/ufomap/wiki/Example-Outputs)
7. [Data Repository](https://github.com/UnknownFreeOccupied/ufomap/wiki/Data-Repository)
8. [API](https://github.com/UnknownFreeOccupied/ufomap/wiki/API)
## Credits
### Paper
* [IEEE](https://ieeexplore.ieee.org/abstract/document/9158399)
* [ArXiv](https://arxiv.org/abs/2003.04749)
### Cite
If you use UFOMap in a scientific publication, please cite the following paper:
* Daniel Duberg and Patric Jensfelt, "UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown," in IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 6411-6418, Oct. 2020, doi: 10.1109/LRA.2020.3013861.
```latex
@article{duberg2020ufomap,
author={Daniel Duberg and Patric Jensfelt},
journal={IEEE Robotics and Automation Letters},
title={{UFOMap}: An Efficient Probabilistic {3D} Mapping Framework That Embraces the Unknown},
year={2020},
volume={5},
number={4},
pages={6411-6418},
doi={10.1109/LRA.2020.3013861}
}
```
### Videos
* [YouTube Playlist](https://youtube.com/playlist?list=PLoZnKRp2UVom4bv2fUVXgI5VCbuTrfrU3)
ufomap/cmake/ufomapConfig.cmake.in
Executable
+4
View File
@@ -0,0 +1,4 @@
@PACKAGE_INIT@
include("${CMAKE_CURRENT_LIST_DIR}/ufomapTargets.cmake")
check_required_components("ufomap")
ufomap/docs/CMakeLists.txt
Executable
View File
ufomap/include/ufo/geometry/README.md
+47
View File
@@ -0,0 +1,47 @@
Supports intersection test between
| | AABB | Frustum | Line Segment | OBB | Plane | Point | Ray | Sphere |
| ---------------- |:----:|:-------:|:------------:|:---:|:-----:|:-----:|:---:|:------:|
| **AABB** | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ |
| **Frustum** | ✔ | ✖ | ✖ | ✔ | ✖ | ✔ | ✖ | ✔ |
| **Line Segment** | ✔ | ✖ | ✖ | ✔ | ✔ | ✔ | ✖ | ✔ |
| **OBB** | ✔ | ✔ | ✔ | ✔ | ✔ | ✖* | ✔ | ✔ |
| **Plane** | ✔ | ✖ | ✔ | ✖ | ✔ | ✔ | ✔ | ✔ |
| **Point** | ✔ | ✔ | ✔ | ✖* | ✔ | ✔ | ✔ | ✔ |
| **Ray** | ✔ | ✖ | ✖ | ✔ | ✔ | ✔ | ✖ | ✔ |
| **Sphere** | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ |
✔: means implemented<br>
✖: not implemented<br>
✖*: implemented but wrong
In progress:
| | AABB | Capsule | Cone | Cylinder | Ellipsoid | Frustum | Line Segment | OBB | Plane | Point | Ray | Sphere | Triangle |
| ---------------- |:----:|:-------:|:----:|:--------:|:---------:|:-------:|:------------:|:---:|:-----:|:-----:|:---:|:------:|:--------:|
| **AABB** | ✔ | ? | ? | ? | ? | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ? |
| **Capsule** | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? |
| **Cone** | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? |
| **Cylinder** | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? |
| **Ellipsoid** | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? |
| **Frustum** | ✔ | ? | ? | ? | ? | ✖ | ✖ | ✔ | ✖ | ✔ | ✖ | ✔ | ? |
| **Line Segment** | ✔ | ? | ? | ? | ? | ✖ | ✖ | ✔ | ✔ | ✔ | ✖ | ✔ | ? |
| **OBB** | ✔ | ? | ? | ? | ? | ✔ | ✔ | ✔ | ✔ | ✖* | ✔ | ✔ | ? |
| **Plane** | ✔ | ? | ? | ? | ? | ✖ | ✔ | ✖ | ✔ | ✔ | ✔ | ✔ | ? |
| **Point** | ✔ | ? | ? | ? | ? | ✔ | ✔ | ✖* | ✔ | ✔ | ✔ | ✔ | ? |
| **Ray** | ✔ | ? | ? | ? | ? | ✖ | ✖ | ✔ | ✔ | ✔ | ✖ | ✔ | ? |
| **Sphere** | ✔ | ? | ? | ? | ? | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ✔ | ? |
| **Triangle** | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? | ? |
✔: means implemented<br>
✖: not implemented<br>
✖*: implemented but wrong<br>
?: type does not even exist
ufomap/include/ufo/geometry/aabb.h
Executable
+73
View File
@@ -0,0 +1,73 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_AXIS_ALIGNED_BOUNDING_BOX_H
#define UFO_GEOMETRY_AXIS_ALIGNED_BOUNDING_BOX_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct AABB {
Point center;
Point half_size;
AABB() {}
AABB(AABB const& aabb) : center(aabb.center), half_size(aabb.half_size) {}
AABB(Point const& center, double half_size)
: center(center), half_size(half_size, half_size, half_size)
{
}
AABB(Point const& min, Point const& max) : half_size((max - min) / 2.0)
{
center = min + half_size;
}
Point getMin() const { return center - half_size; }
Point getMax() const { return center + half_size; }
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_AXIS_ALIGNED_BOUNDING_BOX_H
ufomap/include/ufo/geometry/bounding_volume.h
Executable
+116
View File
@@ -0,0 +1,116 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_BOUDING_VOLUME_H
#define UFO_GEOMETRY_BOUDING_VOLUME_H
#include <ufo/geometry/collision_checks.h>
#include <ufo/geometry/types.h>
namespace ufo::geometry
{
class BoundingVolume
{
public:
void add(BoundingVar const& bv) { bounding_volume_.push_back(bv); }
size_t size() const { return bounding_volume_.size(); }
bool empty() const { return bounding_volume_.empty(); }
bool intersects(BoundingVar const& bv) const;
bool intersects(BoundingVolume const& other) const;
std::vector<BoundingVar>::iterator begin() { return bounding_volume_.begin(); }
std::vector<BoundingVar>::const_iterator begin() const
{
return bounding_volume_.begin();
}
std::vector<BoundingVar>::const_iterator cbegin() const
{
return bounding_volume_.cbegin();
}
std::vector<BoundingVar>::iterator end() { return bounding_volume_.end(); }
std::vector<BoundingVar>::const_iterator end() const { return bounding_volume_.end(); }
std::vector<BoundingVar>::const_iterator cend() const
{
return bounding_volume_.cend();
}
std::vector<BoundingVar>::reverse_iterator rbegin()
{
return bounding_volume_.rbegin();
}
std::vector<BoundingVar>::const_reverse_iterator rbegin() const
{
return bounding_volume_.rbegin();
}
std::vector<BoundingVar>::const_reverse_iterator crbegin() const
{
return bounding_volume_.crbegin();
}
std::vector<BoundingVar>::reverse_iterator rend() { return bounding_volume_.rend(); }
std::vector<BoundingVar>::const_reverse_iterator rend() const
{
return bounding_volume_.rend();
}
std::vector<BoundingVar>::const_reverse_iterator crend() const
{
return bounding_volume_.crend();
}
private:
std::vector<BoundingVar> bounding_volume_;
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_BOUDING_VOLUME_H
ufomap/include/ufo/geometry/capsule.h
Executable
+67
View File
@@ -0,0 +1,67 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_CAPSULE_H
#define UFO_GEOMETRY_CAPSULE_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Capsule {
Point start;
Point end;
double radius;
Capsule() : start(0.0, 0.0, 0.0), end(0.0, 0.0, 0.0), radius(0.0) {}
Capsule(Point const& start, Point const& end, double radius)
: start(start), end(end), radius(radius)
{
}
Capsule(Capsule const& other) : start(other.start), end(other.end), radius(other.radius)
{
}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_CAPSULE_H
ufomap/include/ufo/geometry/collision_checks.h
Executable
+267
View File
@@ -0,0 +1,267 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_COLLISION_CHECKS_H
#define UFO_GEOMETRY_COLLISION_CHECKS_H
#include <ufo/geometry/aabb.h>
#include <ufo/geometry/bounding_volume.h>
// #include <ufo/geometry/capsule.h>
#include <ufo/geometry/frustum.h>
#include <ufo/geometry/line_segment.h>
#include <ufo/geometry/obb.h>
#include <ufo/geometry/plane.h>
#include <ufo/geometry/ray.h>
#include <ufo/geometry/sphere.h>
namespace ufo::geometry {
/////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// Intersection tests
//////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
// AABB
bool intersects(const AABB& aabb_1, const AABB& aabb_2);
// bool intersects(const AABB& aabb, const Capsule& capsule);
bool intersects(const AABB& aabb, const Frustum& frustum);
bool intersects(const AABB& aabb, const LineSegment& line_segment);
bool intersects(const AABB& aabb, const OBB& obb);
bool intersects(const AABB& aabb, const Plane& plane);
bool intersects(const AABB& aabb, const Point& point);
bool intersects(const AABB& aabb, const Ray& ray);
bool intersects(const AABB& aabb, const Sphere& sphere);
// TODO: Capsule
// bool intersects(const Capsule& capsule, const AABB& aabb);
// bool intersects(const Capsule& capsule_1, const Capsule& capsule_2);
// bool intersects(const Capsule& capsule, const Frustum& frustum);
// bool intersects(const Capsule& capsule, const LineSegment& line_segment);
// bool intersects(const Capsule& capsule, const OBB& obb);
// bool intersects(const Capsule& capsule, const Plane& plane);
// bool intersects(const Capsule& capsule, const Point& point);
// bool intersects(const Capsule& capsule, const Ray& ray);
// bool intersects(const Capsule& capsule, const Sphere& sphere);
// TODO: Cone
// TODO: Cylinder
// TODO: Ellipsoid
// Frustum
bool intersects(const Frustum& frustum, const AABB& aabb);
// bool intersects(const Frustum& frustum, const Capsule& capsule);
bool intersects(const Frustum& frustum_1, const Frustum& frustum_2);
bool intersects(const Frustum& frustum, const LineSegment& line_segment);
bool intersects(const Frustum& frustum, const OBB& obb);
bool intersects(const Frustum& frustum, const Plane& plane);
bool intersects(const Frustum& frustum, const Point& point);
bool intersects(const Frustum& frustum, const Ray& ray);
bool intersects(const Frustum& frustum, const Sphere& sphere);
// Line segment
bool intersects(const LineSegment& line_segment, const AABB& aabb);
// bool intersects(const LineSegment& line_segment, const Capsule& capsule);
bool intersects(const LineSegment& line_segment, const Frustum& frustum);
bool intersects(const LineSegment& line_segment_1,
const LineSegment& line_segment_2);
bool intersects(const LineSegment& line_segment, const OBB& obb);
bool intersects(const LineSegment& line_segment, const Plane& plane);
bool intersects(const LineSegment& line_segment, const Point& point);
bool intersects(const LineSegment& line_segment, const Ray& ray);
bool intersects(const LineSegment& line_segment, const Sphere& sphere);
// OBB
bool intersects(const OBB& obb, const AABB& aabb);
// bool intersects(const OBB& obb, const Capsule& capsule);
bool intersects(const OBB& obb, const Frustum& frustum);
bool intersects(const OBB& obb, const LineSegment& line_segment);
bool intersects(const OBB& obb_1, const OBB& obb_2);
bool intersects(const OBB& obb, const Plane& plane);
bool intersects(const OBB& obb, const Point& point);
bool intersects(const OBB& obb, const Ray& ray);
bool intersects(const OBB& obb, const Sphere& sphere);
// Plane
bool intersects(const Plane& plane, const AABB& aabb);
// bool intersects(const Plane& plane, const Capsule& capsule);
bool intersects(const Plane& plane, const Frustum& frustum);
bool intersects(const Plane& plane, const LineSegment& line_segment);
bool intersects(const Plane& plane, const OBB& obb);
bool intersects(const Plane& plane_1, const Plane& plane_2);
bool intersects(const Plane& plane, const Point& point);
bool intersects(const Plane& plane, const Ray& ray);
bool intersects(const Plane& plane, const Sphere& sphere);
// Point
bool intersects(const Point& point, const AABB& aabb);
// bool intersects(const Point& point, const Capsule& capsule);
bool intersects(const Point& point, const Frustum& frustum);
bool intersects(const Point& point, const LineSegment& line_segment);
bool intersects(const Point& point, const OBB& obb);
bool intersects(const Point& point, const Plane& plane);
bool intersects(const Point& point_1, const Point& point_2);
bool intersects(const Point& point, const Ray& ray);
bool intersects(const Point& point, const Sphere& sphere);
// Ray
bool intersects(const Ray& ray, const AABB& aabb);
// bool intersects(const Ray& ray, const Capsule& capsule);
bool intersects(const Ray& ray, const Frustum& frustum);
bool intersects(const Ray& ray, const LineSegment& line_segment);
bool intersects(const Ray& ray, const OBB& obb);
bool intersects(const Ray& ray, const Plane& plane);
bool intersects(const Ray& ray, const Point& point);
bool intersects(const Ray& ray_1, const Ray& ray_2);
bool intersects(const Ray& ray, const Sphere& sphere);
// Sphere
bool intersects(const Sphere& sphere, const AABB& aabb);
// bool intersects(const Sphere& sphere, const Capsule& capsule);
bool intersects(const Sphere& sphere, const Frustum& frustum);
bool intersects(const Sphere& sphere, const LineSegment& line_segment);
bool intersects(const Sphere& sphere, const OBB& obb);
bool intersects(const Sphere& sphere, const Plane& plane);
bool intersects(const Sphere& sphere, const Point& point);
bool intersects(const Sphere& sphere, const Ray& ray);
bool intersects(const Sphere& sphere_1, const Sphere& sphere_2);
// TODO: Triangle
/////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////// Inside tests
/////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
// AABB
bool inside(const AABB& aabb_1, const AABB& aabb_2);
bool inside(const AABB& aabb, const Frustum& frustum);
bool inside(const AABB& aabb, const LineSegment& line_segment);
bool inside(const AABB& aabb, const OBB& obb);
bool inside(const AABB& aabb, const Plane& plane);
bool inside(const AABB& aabb, const Point& point);
bool inside(const AABB& aabb, const Ray& ray);
bool inside(const AABB& aabb, const Sphere& sphere);
// TODO: Capsule
// TODO: Cone
// TODO: Cylinder
// TODO: Ellipsoid
// Frustum
bool inside(const Frustum& frustum, const AABB& aabb);
bool inside(const Frustum& frustum_1, const Frustum& frustum_2);
bool inside(const Frustum& frustum, const LineSegment& line_segment);
bool inside(const Frustum& frustum, const OBB& obb);
bool inside(const Frustum& frustum, const Plane& plane);
bool inside(const Frustum& frustum, const Point& point);
bool inside(const Frustum& frustum, const Ray& ray);
bool inside(const Frustum& frustum, const Sphere& sphere);
// Line segment
bool inside(const LineSegment& line_segment, const AABB& aabb);
bool inside(const LineSegment& line_segment, const Frustum& frustum);
bool inside(const LineSegment& line_segment_1,
const LineSegment& line_segment_2);
bool inside(const LineSegment& line_segment, const OBB& obb);
bool inside(const LineSegment& line_segment, const Plane& plane);
bool inside(const LineSegment& line_segment, const Point& point);
bool inside(const LineSegment& line_segment, const Ray& ray);
bool inside(const LineSegment& line_segment, const Sphere& sphere);
// OBB
bool inside(const OBB& obb, const AABB& aabb);
bool inside(const OBB& obb, const Frustum& frustum);
bool inside(const OBB& obb, const LineSegment& line_segment);
bool inside(const OBB& obb_1, const OBB& obb_2);
bool inside(const OBB& obb, const Plane& plane);
bool inside(const OBB& obb, const Point& point);
bool inside(const OBB& obb, const Ray& ray);
bool inside(const OBB& obb, const Sphere& sphere);
// Plane
bool inside(const Plane& plane, const AABB& aabb);
bool inside(const Plane& plane, const Frustum& frustum);
bool inside(const Plane& plane, const LineSegment& line_segment);
bool inside(const Plane& plane, const OBB& obb);
bool inside(const Plane& plane_1, const Plane& plane_2);
bool inside(const Plane& plane, const Point& point);
bool inside(const Plane& plane, const Ray& ray);
bool inside(const Plane& plane, const Sphere& sphere);
// Point
bool inside(const Point& point, const AABB& aabb);
bool inside(const Point& point, const Frustum& frustum);
bool inside(const Point& point, const LineSegment& line_segment);
bool inside(const Point& point, const OBB& obb);
bool inside(const Point& point, const Plane& plane);
bool inside(const Point& point_1, const Point& point_2);
bool inside(const Point& point, const Ray& ray);
bool inside(const Point& point, const Sphere& sphere);
// Ray
bool inside(const Ray& ray, const AABB& aabb);
bool inside(const Ray& ray, const Frustum& frustum);
bool inside(const Ray& ray, const LineSegment& line_segment);
bool inside(const Ray& ray, const OBB& obb);
bool inside(const Ray& ray, const Plane& plane);
bool inside(const Ray& ray, const Point& point);
bool inside(const Ray& ray_1, const Ray& ray_2);
bool inside(const Ray& ray, const Sphere& sphere);
// Sphere
bool inside(const Sphere& sphere, const AABB& aabb);
bool inside(const Sphere& sphere, const Frustum& frustum);
bool inside(const Sphere& sphere, const LineSegment& line_segment);
bool inside(const Sphere& sphere, const OBB& obb);
bool inside(const Sphere& sphere, const Plane& plane);
bool inside(const Sphere& sphere, const Point& point);
bool inside(const Sphere& sphere, const Ray& ray);
bool inside(const Sphere& sphere_1, const Sphere& sphere_2);
// TODO: Triangle
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_COLLISION_CHECKS_H
ufomap/include/ufo/geometry/cone.h
Executable
+52
View File
@@ -0,0 +1,52 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_CONE_H
#define UFO_GEOMETRY_CONE_H
#include <ufo/geometry/point.h>
namespace ufo::geometry {
struct Cone {
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_CONE_H
ufomap/include/ufo/geometry/cylinder.h
Executable
+63
View File
@@ -0,0 +1,63 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_CYLINDER_H
#define UFO_GEOMETRY_CYLINDER_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Cylinder {
Point start;
Point end;
double radius;
Cylinder() : start(0.0, 0.0, 0.0), end(0.0, 0.0, 0.0), radius(0.0) {}
Cylinder(Point const& start, Point const& end, double radius)
: start(start), end(end), radius(radius)
{
}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_CYLINDER_H
ufomap/include/ufo/geometry/ellipsoid.h
Executable
+59
View File
@@ -0,0 +1,59 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_ELLIPSOID_H
#define UFO_GEOMETRY_ELLIPSOID_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Ellipsoid {
Point center;
Point radius;
Ellipsoid() : center(0.0, 0.0, 0.0), radius(0.0, 0.0, 0.0) {}
Ellipsoid(Point const& center, Point const& radius) : center(center), radius(radius) {}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_ELLIPSOID_H
ufomap/include/ufo/geometry/frustum.h
Executable
+130
View File
@@ -0,0 +1,130 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_FRUSTUM_H
#define UFO_GEOMETRY_FRUSTUM_H
// UFO
#include <ufo/geometry/plane.h>
#include <ufo/geometry/point.h>
// STD
#include <array>
#include <cmath>
namespace ufo::geometry
{
struct Frustum {
std::array<Plane, 6> planes;
Frustum() {}
Frustum(Frustum const& frustum) : planes(frustum.planes) {}
// TODO: Horizontal or vertical angle?
Frustum(Point const& pos, Point const& target, Point const& up, double vertical_angle,
double horizontal_angle, double near_distance, double far_distance)
{
double ratio = horizontal_angle / vertical_angle;
// TODO: Check if correct
double tang = tan(vertical_angle * 0.5);
double near_height = near_distance * tang;
double near_width = near_height * ratio;
double far_height = far_distance * tang;
double far_width = far_height * ratio;
Point Z = pos - target;
Z.normalize();
Point X = Point::cross(up, Z);
X.normalize();
Point Y = Point::cross(Z, X);
Point nc = pos - Z * near_distance;
Point fc = pos - Z * far_distance;
Point near_top_left = nc + Y * near_height - X * near_width;
Point near_top_right = nc + Y * near_height + X * near_width;
Point near_bottom_left = nc - Y * near_height - X * near_width;
Point near_bottom_right = nc - Y * near_height + X * near_width;
Point far_top_left = fc + Y * far_height - X * far_width;
Point far_top_right = fc + Y * far_height + X * far_width;
Point far_bottom_left = fc - Y * far_height - X * far_width;
Point far_bottom_right = fc - Y * far_height + X * far_width;
top() = Plane(near_top_right, near_top_left, far_top_left);
bottom() = Plane(near_bottom_left, near_bottom_right, far_bottom_right);
left() = Plane(near_top_left, near_bottom_left, far_bottom_left);
right() = Plane(near_bottom_right, near_top_right, far_bottom_right);
near() = Plane(near_top_left, near_top_right, near_bottom_right);
far() = Plane(far_top_right, far_top_left, far_bottom_left);
}
Plane const& top() const { return planes[0]; }
Plane& top() { return planes[0]; }
Plane const& bottom() const { return planes[1]; }
Plane& bottom() { return planes[1]; }
Plane const& left() const { return planes[2]; }
Plane& left() { return planes[2]; }
Plane const& right() const { return planes[3]; }
Plane& right() { return planes[3]; }
Plane const& near() const { return planes[4]; }
Plane& near() { return planes[4]; }
Plane const& far() const { return planes[5]; }
Plane& far() { return planes[5]; }
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_FRUSTUM_H
ufomap/include/ufo/geometry/line_segment.h
Executable
+64
View File
@@ -0,0 +1,64 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_LINE_SEGMENT_H
#define UFO_GEOMETRY_LINE_SEGMENT_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct LineSegment {
Point start;
Point end;
LineSegment() {}
LineSegment(LineSegment const& line_segment)
: start(line_segment.start), end(line_segment.end)
{
}
LineSegment(Point const& start, Point const& end) : start(start), end(end) {}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_LINE_SEGMENT_H
ufomap/include/ufo/geometry/obb.h
Executable
+146
View File
@@ -0,0 +1,146 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_ORIENTED_BOUNDING_BOX_H
#define UFO_GEOMETRY_ORIENTED_BOUNDING_BOX_H
// UFO
#include <ufo/geometry/point.h>
#include <ufo/math/quaternion.h>
// STD
#include <stdexcept>
namespace ufo::geometry
{
struct OBB {
Point center;
Point half_size;
math::Quaternion rotation;
OBB() {}
OBB(OBB const& obb)
: center(obb.center), half_size(obb.half_size), rotation(obb.rotation)
{
}
OBB(Point const& center, Point const& half_size) : center(center), half_size(half_size)
{
}
OBB(Point const& center, Point const& half_size, math::Quaternion const& rotation)
: center(center), half_size(half_size), rotation(rotation)
{
}
OBB(Point const& center, Point const& half_size, Point const& rotation)
: center(center),
half_size(half_size),
rotation(rotation[0], rotation[1], rotation[2])
{
}
Point getMin() const
{
Point rot_half_size = rotation.rotate(half_size);
Point corners[8]{
Point(center.x() - rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() - rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() + rot_half_size.z()),
Point(center.x() - rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() - rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() + rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() + rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() + rot_half_size.z()),
};
Point minimum = corners[0];
for (int i = 1; i < 8; ++i) {
minimum.x() = std::min(minimum.x(), corners[i].x());
minimum.y() = std::min(minimum.y(), corners[i].y());
minimum.z() = std::min(minimum.z(), corners[i].z());
}
return minimum;
}
Point getMax() const
{
Point rot_half_size = rotation.rotate(half_size);
Point corners[8]{
Point(center.x() - rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() - rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() + rot_half_size.z()),
Point(center.x() - rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() - rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() + rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() - rot_half_size.y(),
center.z() + rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() - rot_half_size.z()),
Point(center.x() + rot_half_size.x(), center.y() + rot_half_size.y(),
center.z() + rot_half_size.z()),
};
Point maximum = corners[0];
for (int i = 1; i < 8; ++i) {
maximum.x() = std::max(maximum.x(), corners[i].x());
maximum.y() = std::max(maximum.y(), corners[i].y());
maximum.z() = std::max(maximum.z(), corners[i].z());
}
return maximum;
}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_ORIENTED_BOUNDING_BOX_H
ufomap/include/ufo/geometry/plane.h
Executable
+77
View File
@@ -0,0 +1,77 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_PLANE_H
#define UFO_GEOMETRY_PLANE_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Plane {
Point normal;
double distance;
Plane() : normal(1.0, 0.0, 0.0) {}
Plane(Plane const& plane) : normal(plane.normal), distance(plane.distance) {}
Plane(Point const& normal, double distance) : normal(normal), distance(distance) {}
Plane(Point const& v_1, Point const& v_2, Point const& v_3)
{
Point aux_1 = v_1 - v_2;
Point aux_2 = v_3 - v_2;
normal = aux_2.cross(aux_1);
normal.normalize();
distance = -normal.dot(v_2);
}
Plane& operator=(Plane const& rhs)
{
normal = rhs.normal;
distance = rhs.distance;
return *this;
}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_PLANE_H
ufomap/include/ufo/geometry/point.h
Executable
+51
View File
@@ -0,0 +1,51 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_POINT_H
#define UFO_GEOMETRY_POINT_H
#include <ufo/math/vector3.h>
namespace ufo::geometry {
using Point = math::Vector3;
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_POINT_H
ufomap/include/ufo/geometry/ray.h
Executable
+64
View File
@@ -0,0 +1,64 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_RAY_H
#define UFO_GEOMETRY_RAY_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Ray {
Point origin;
Point direction;
Ray() : direction(0.0, 0.0, 1.0) {}
Ray(Ray const& ray) : origin(ray.origin), direction(ray.direction) {}
Ray(Point const& origin, Point const& direction)
: origin(origin), direction(direction.normalized())
{
}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_RAY_H
ufomap/include/ufo/geometry/sphere.h
Executable
+61
View File
@@ -0,0 +1,61 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_SPHERE_H
#define UFO_GEOMETRY_SPHERE_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Sphere {
Point center;
double radius;
Sphere() : center(0.0, 0.0, 0.0), radius(0.0) {}
Sphere(Sphere const& sphere) : center(sphere.center), radius(sphere.radius) {}
Sphere(Point const& center, double radius) : center(center), radius(radius) {}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_SPHERE_H
ufomap/include/ufo/geometry/triangle.h
Executable
+66
View File
@@ -0,0 +1,66 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_TRIANGLE_H
#define UFO_GEOMETRY_TRIANGLE_H
#include <ufo/geometry/point.h>
namespace ufo::geometry
{
struct Triangle {
Point points[3];
Triangle() {}
Triangle(Point const& point_1, Point const& point_2, Point const& point_3)
: points{point_1, point_2, point_3}
{
}
Triangle(Triangle const& other)
: points{other.points[0], other.points[1], other.points[2]}
{
}
};
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_TRIANGLE_H
ufomap/include/ufo/geometry/types.h
Executable
+61
View File
@@ -0,0 +1,61 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_GEOMETRY_TYPES_H
#define UFO_GEOMETRY_TYPES_H
#include <ufo/geometry/aabb.h>
#include <ufo/geometry/frustum.h>
#include <ufo/geometry/line_segment.h>
#include <ufo/geometry/obb.h>
#include <ufo/geometry/plane.h>
#include <ufo/geometry/point.h>
#include <ufo/geometry/ray.h>
#include <ufo/geometry/sphere.h>
#include <variant>
namespace ufo::geometry {
using BoundingVar =
std::variant<AABB, Frustum, LineSegment, OBB, Plane, Point, Ray, Sphere>;
} // namespace ufo::geometry
#endif // UFO_GEOMETRY_TYPES_H
ufomap/include/ufo/map/code.h
Executable
+788
View File
@@ -0,0 +1,788 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_CODE_H
#define UFO_MAP_CODE_H
// UFO
#include <ufo/map/key.h>
#include <ufo/map/types.h>
// STD
#include <immintrin.h>
#include <algorithm>
#include <execution>
#include <list>
#include <unordered_map>
#include <unordered_set>
#include <vector>
namespace ufo::map
{
/**
* @brief A code is a single value for indexing a specific node in an octree at
* a specific depth
*
* @details Morton codes are used in UFOMap to increase performance when
* accessing the octree
*
*/
class Code
{
public:
Code() : code_(0), depth_(0) {}
Code(CodeType code, DepthType depth = 0) : code_(code), depth_(depth) {}
Code(Key const& key) : code_(toCode(key)), depth_(key.getDepth()) {}
Code(Code const& other) : code_(other.code_), depth_(other.depth_) {}
Code& operator=(Code const& rhs)
{
code_ = rhs.code_;
depth_ = rhs.depth_;
return *this;
}
bool operator==(Code const& rhs) const
{
return code_ == rhs.code_ && depth_ == rhs.depth_;
}
bool operator!=(Code const& rhs) const
{
return code_ != rhs.code_ || depth_ != rhs.depth_;
}
bool operator<(Code const& rhs) const
{
// TODO: Check
return get3Bits(code_) < get3Bits(rhs.code_) &&
get3Bits(code_ >> 1) < get3Bits(rhs.code_ >> 1) &&
get3Bits(code_ >> 2) < get3Bits(rhs.code_ >> 2);
}
bool operator<=(Code const& rhs) const
{
return get3Bits(code_) <= get3Bits(rhs.code_) &&
get3Bits(code_ >> 1) <= get3Bits(rhs.code_ >> 1) &&
get3Bits(code_ >> 2) <= get3Bits(rhs.code_ >> 2);
}
bool operator>(Code const& rhs) const
{
return get3Bits(code_) > get3Bits(rhs.code_) &&
get3Bits(code_ >> 1) > get3Bits(rhs.code_ >> 1) &&
get3Bits(code_ >> 2) > get3Bits(rhs.code_ >> 2);
}
bool operator>=(Code const& rhs) const
{
return get3Bits(code_) >= get3Bits(rhs.code_) &&
get3Bits(code_ >> 1) >= get3Bits(rhs.code_ >> 1) &&
get3Bits(code_ >> 2) >= get3Bits(rhs.code_ >> 2);
}
/**
* @brief Return the code at a specified depth
*
* @param depth The depth of the code
* @return Code The code at the specified depth
*/
Code toDepth(DepthType depth) const
{
CodeType temp = 3 * depth;
return Code((code_ >> temp) << temp, depth);
}
void moveX(int offset)
{
#if defined(__BMI2__) // TODO: Is correct?
KeyType x = static_cast<KeyType>(_pext_u64(code_, 0x9249249249249249)) + offset;
code_ &= 0x6DB6DB6DB6DB6DB6;
code_ |= _pdep_u64(static_cast<CodeType>(x), 0x9249249249249249);
#else
KeyType x = toKey(0) + offset;
code_ &= 0x6DB6DB6DB6DB6DB6;
code_ |= splitBy3(x);
#endif
}
void moveY(int offset)
{
#if defined(__BMI2__) // TODO: Is correct?
KeyType y = static_cast<KeyType>(_pext_u64(code_, 0x2492492492492492)) + offset;
code_ &= 0x5B6DB6DB6DB6DB6D;
code_ |= _pdep_u64(static_cast<CodeType>(y), 0x2492492492492492);
#else
KeyType y = toKey(1) + offset;
code_ &= 0x5B6DB6DB6DB6DB6D;
code_ |= (splitBy3(y) << 1);
#endif
}
void moveZ(int offset)
{
#if defined(__BMI2__) // TODO: Is correct?
KeyType z = static_cast<KeyType>(_pext_u64(code_, 0x4924924924924924)) + offset;
code_ &= 0xB6DB6DB6DB6DB6DB;
code_ |= _pdep_u64(static_cast<CodeType>(z), 0x4924924924924924);
#else
KeyType z = toKey(2) + offset;
code_ &= 0xB6DB6DB6DB6DB6DB;
code_ |= (splitBy3(z) << 2);
#endif
}
/**
* @brief Converts a key to a code
*
* @param key The key to convert
* @return uint64_t The code corresponding to the key
*/
static CodeType toCode(Key const& key)
{
#if defined(__BMI2__) // TODO: Is correct?
return _pdep_u64(static_cast<CodeType>(key[0]), 0x9249249249249249) |
_pdep_u64(static_cast<CodeType>(key[1]), 0x2492492492492492) |
_pdep_u64(static_cast<CodeType>(key[2]), 0x4924924924924924);
#else
return splitBy3(key[0]) | (splitBy3(key[1]) << 1) | (splitBy3(key[2]) << 2);
#endif
}
/**
* @brief Get the key component from a code
*
* @param code The code to generate the key component from
* @param index The index of the key component
* @return KeyType The key component value
*/
static KeyType toKey(Code const& code, std::size_t index)
{
return get3Bits(code.code_ >> index);
}
/**
* @brief Get the key component from this code
*
* @param index The index of the key component
* @return KeyType The key component value
*/
KeyType toKey(std::size_t index) const { return toKey(*this, index); }
/**
* @brief Get the corresponding key to code
*
* @param code The code the corresponding key should be returned
* @return Key The corresponding key to code
*/
static Key toKey(Code const& code)
{
#if defined(__BMI2__) // TODO: Is correct?
return Key(static_cast<KeyType>(_pext_u64(code.code_, 0x9249249249249249)),
static_cast<KeyType>(_pext_u64(code.code_, 0x2492492492492492)),
static_cast<KeyType>(_pext_u64(code.code_, 0x4924924924924924)),
code.getDepth());
#else
return Key(toKey(code, 0), toKey(code, 1), toKey(code, 2), code.getDepth());
#endif
}
/**
* @brief Get the corresponding key to this code
*
* @return Key The corresponding key to this code
*/
Key toKey() const { return toKey(*this); }
/**
* @brief Get the child index at a specific depth for this code
*
* @param depth The depth the child index is requested for
* @return std::size_t The child index at the specified depth
*/
std::size_t getChildIdx(DepthType depth) const
{
return (code_ >> static_cast<CodeType>(3 * depth)) & ((CodeType)0x7);
}
/**
* @brief Get the code of a specific child to this code
*
* @param index The index of the child
* @return Code The child code
*/
Code getChild(std::size_t index) const
{
if (0 == depth_) {
// TODO: Throw error?
return *this;
}
DepthType child_depth = depth_ - 1;
return Code(
code_ + (static_cast<CodeType>(index) << static_cast<CodeType>(3 * child_depth)),
child_depth);
}
/**
* @brief Get the eight child codes that comes from this code
*
* @return std::vector<Code> The eight child codes
*/
std::vector<Code> getChildren() const
{
std::vector<Code> children;
if (0 == depth_) {
return children;
}
DepthType child_depth = depth_ - 1;
CodeType offset = 3 * child_depth;
for (CodeType i = 0; i < 8; ++i) {
children.emplace_back(code_ + (i << offset), child_depth);
}
return children;
}
/**
* @brief Get all children that this code can have from this code's depth to
* depth 0
*
* @return std::vector<Code> Collection of all possible child codes of this
* code
*/
std::vector<Code> getAllChildren() const
{
std::vector<Code> children;
CodeType max = 8 << (3 * depth_);
for (CodeType i = 0; i < max; ++i) {
children.emplace_back(code_ + i, 0);
}
return children;
}
/**
* @brief Get the code
*
* @return CodeType The code
*/
CodeType getCode() const { return code_; }
/**
* @brief Get the depth that this code is specified at
*
* @return DepthType The depth this code is specified at
*/
DepthType getDepth() const { return depth_; }
/**
* @brief
*
*/
struct Hash {
std::size_t operator()(Code const& code) const
{
return static_cast<std::size_t>(code.code_);
}
static size_t hash(Code const& code) { return code.code_; }
static bool equal(Code const& a, Code const& b) { return a == b; }
};
private:
static CodeType splitBy3(KeyType a)
{
#if defined(__BMI2__) // TODO: Is correct?
return _pdep_u64(static_cast<CodeType>(a), 0x9249249249249249);
#else
CodeType code = static_cast<CodeType>(a) & 0x1fffff;
code = (code | code << 32) & 0x1f00000000ffff;
code = (code | code << 16) & 0x1f0000ff0000ff;
code = (code | code << 8) & 0x100f00f00f00f00f;
code = (code | code << 4) & 0x10c30c30c30c30c3;
code = (code | code << 2) & 0x1249249249249249;
return code;
#endif
}
static KeyType get3Bits(CodeType code)
{
#if defined(__BMI2__) // TODO: Is correct?
return static_cast<KeyType>(_pext_u64(code, 0x9249249249249249));
#else
CodeType a = code & 0x1249249249249249;
a = (a ^ (a >> 2)) & 0x10c30c30c30c30c3;
a = (a ^ (a >> 4)) & 0x100f00f00f00f00f;
a = (a ^ (a >> 8)) & 0x1f0000ff0000ff;
a = (a ^ (a >> 16)) & 0x1f00000000ffff;
a = (a ^ a >> 32) & 0x1fffff;
return static_cast<KeyType>(a);
#endif
}
private:
// The Morton code
CodeType code_;
// The depth of the Morton code
DepthType depth_;
};
// using CodeSet = std::unordered_set<Code, Code::Hash>;
// template <typename T>
// using CodeMap = std::unordered_map<Code, T, Code::Hash>;
using CodeRay = std::vector<Code>;
class CodeSet
{
public:
CodeSet(unsigned int power = 18) : power_(power)
{
num_buckets_ = size_t(1) << power_;
data_.resize(num_buckets_);
}
struct CodeSetIterator {
CodeSetIterator(CodeSet const* set = nullptr) : set_(set)
{
if (!set_) {
return;
}
if (set_->data_.empty()) {
set_ = nullptr;
} else {
outer_iter_ = set_->data_.begin();
outer_iter_end_ = set_->data_.end();
while (outer_iter_ != outer_iter_end_ && outer_iter_->empty()) {
++outer_iter_;
}
if (outer_iter_ == outer_iter_end_) {
set_ = nullptr;
} else {
inner_iter_ = outer_iter_->begin();
inner_iter_end_ = outer_iter_->end();
}
}
}
Code const& operator*() const { return *inner_iter_; }
// Postfix increment
CodeSetIterator operator++(int)
{
CodeSetIterator result = *this;
++(*this);
return result;
}
// Prefix increment
CodeSetIterator& operator++()
{
++inner_iter_;
if (inner_iter_ == inner_iter_end_) {
++outer_iter_;
while (outer_iter_ != outer_iter_end_ && outer_iter_->empty()) {
++outer_iter_;
}
if (outer_iter_ == outer_iter_end_) {
set_ = nullptr;
} else {
inner_iter_ = outer_iter_->begin();
inner_iter_end_ = outer_iter_->end();
}
}
return *this;
}
bool operator==(CodeSetIterator const& rhs) const { return (rhs.set_ == set_); }
bool operator!=(CodeSetIterator const& rhs) const { return (rhs.set_ != set_); }
private:
CodeSet const* set_;
std::vector<std::vector<Code>>::const_iterator outer_iter_;
std::vector<std::vector<Code>>::const_iterator outer_iter_end_;
std::vector<Code>::const_iterator inner_iter_;
std::vector<Code>::const_iterator inner_iter_end_;
// typename decltype(CodeSet::data_)::const_iterator outer_iter_;
// typename decltype(CodeSet::data_)::const_iterator outer_iter_end_;
// typename decltype(CodeSet::data_)::value_type::const_iterator inner_iter_;
// typename decltype(CodeSet::data_)::value_type::const_iterator inner_iter_end_;
};
std::pair<int, bool> insert(Code const& value)
{
size_t hash = getBucket(value);
if (std::any_of(std::execution::seq, data_[hash].begin(), data_[hash].end(),
[&value](auto const& elem) { return value == elem; })) {
return std::make_pair(0, false); // TODO: Fix
}
++size_;
if (load_factor() > max_load_factor() && power_ < MAX_POWER) {
rehash(num_buckets_ * 2);
hash = getBucket(value);
}
data_[hash].push_back(value);
return std::make_pair(0, true); // TOOD: Fix
}
void clear()
{
std::for_each(std::execution::seq, data_.begin(), data_.end(),
[](auto& bucket) { bucket.clear(); });
size_ = 0;
}
bool empty() const noexcept { return 0 == size_; }
size_t size() const noexcept { return size_; }
size_t bucket_count() const noexcept { return num_buckets_; }
unsigned int bucket_count_power() const noexcept { return power_; }
float load_factor() const { return size_ / ((float)num_buckets_); }
float max_load_factor() const { return max_load_factor_; }
void max_load_factor(float max_load_factor)
{
max_load_factor_ = max_load_factor;
if (load_factor() > max_load_factor_ && power_ < MAX_POWER) {
rehash(num_buckets_ * 2);
}
}
void rehash(std::size_t count)
{
std::size_t min_count = std::max((float)count, size() / max_load_factor());
unsigned int power =
std::max(power_, std::min((unsigned int)std::log2(min_count) + 1, MAX_POWER));
if (power_ == power) {
return;
}
power_ = power;
num_buckets_ = std::size_t(1) << power_;
decltype(data_) new_data;
new_data.resize(num_buckets_);
for (Code const& value : *this) {
new_data[getBucket(value)].push_back(value);
}
data_.swap(new_data);
}
void reserve(std::size_t count)
{
power_ = std::max(power_, std::min((unsigned int)std::log2(count) + 1, MAX_POWER));
data_.reserve(std::size_t(1) << power_);
}
CodeSetIterator begin() const { return CodeSetIterator(this); }
CodeSetIterator end() const { return CodeSetIterator(); }
void swap(CodeSet& other)
{
data_.swap(other.data_);
std::swap(power_, other.power_);
std::swap(num_buckets_, other.num_buckets_);
std::swap(size_, other.size_);
std::swap(max_load_factor_, other.max_load_factor_);
}
using const_iterator = CodeSetIterator;
private:
std::size_t getBucket(Code const& key) const
{
unsigned int offset = 3 * key.getDepth();
unsigned int modder = (num_buckets_ - 1) << offset;
return (Code::Hash()(key) & modder) >> offset;
}
private:
std::vector<std::vector<Code>> data_;
unsigned int power_;
std::size_t num_buckets_;
std::size_t size_ = 0;
float max_load_factor_ = 1.0;
inline static const unsigned int MAX_POWER = 28;
friend struct CodeSetIterator;
};
template <class T>
class CodeMap
{
public:
CodeMap(unsigned int power = 18) : power_(power)
{
num_buckets_ = std::size_t(1) << power_; // std::pow(2, power_);
data_.resize(num_buckets_);
}
struct CodeMapIterator {
CodeMapIterator(const CodeMap* map = nullptr) : map_(map)
{
if (nullptr == map_) {
return;
}
if (map_->data_.empty()) {
map_ = nullptr;
} else {
outer_iter_ = map_->data_.begin();
outer_iter_end_ = map_->data_.end();
while (outer_iter_ != outer_iter_end_ && outer_iter_->empty()) {
++outer_iter_;
}
if (outer_iter_ == outer_iter_end_) {
map_ = nullptr;
} else {
inner_iter_ = outer_iter_->begin();
inner_iter_end_ = outer_iter_->end();
}
}
}
const std::pair<Code, T>& operator*() const
{
return *inner_iter_; // map_->data_[outer_index_][inner_index_];
}
std::pair<Code, T> operator*()
{
return *inner_iter_; // map_->data_[outer_index_][inner_index_];
}
// Postfix increment
CodeMapIterator operator++(int)
{
CodeMapIterator result = *this;
++(*this);
return result;
}
// Prefix increment
CodeMapIterator& operator++()
{
++inner_iter_;
if (inner_iter_ == inner_iter_end_) {
++outer_iter_;
while (outer_iter_ != outer_iter_end_ && outer_iter_->empty()) {
++outer_iter_;
}
if (outer_iter_ == outer_iter_end_) {
map_ = nullptr;
} else {
inner_iter_ = outer_iter_->begin();
inner_iter_end_ = outer_iter_->end();
}
}
return *this;
}
bool operator==(const CodeMapIterator& rhs) const { return (rhs.map_ == map_); }
bool operator!=(const CodeMapIterator& rhs) const { return (rhs.map_ != map_); }
private:
const CodeMap* map_;
typename std::vector<std::list<std::pair<Code, T>>>::const_iterator outer_iter_;
typename std::vector<std::list<std::pair<Code, T>>>::const_iterator outer_iter_end_;
typename std::list<std::pair<Code, T>>::const_iterator inner_iter_;
typename std::list<std::pair<Code, T>>::const_iterator inner_iter_end_;
// typename decltype(CodeMap<T>::data_)::const_iterator outer_iter_;
// typename decltype(CodeMap<T>::data_)::const_iterator outer_iter_end_;
// typename decltype(CodeMap<T>::data_)::value_type::const_iterator inner_iter_;
// typename decltype(CodeMap<T>::data_)::value_type::const_iterator inner_iter_end_;
};
T& operator[](Code const& key)
{
std::size_t hash = getBucket(key);
auto it = std::find_if(std::execution::seq, data_[hash].begin(), data_[hash].end(),
[&key](const auto& elem) { return key == elem.first; });
if (it != data_[hash].end()) {
return it->second;
}
++size_;
if (load_factor() > max_load_factor() && power_ < MAX_POWER) {
rehash(num_buckets_ * 2);
hash = getBucket(key);
}
return std::get<1>(data_[hash].emplace_front(key, T())); // TODO: How to
// call default?
}
std::pair<int, bool> try_emplace(Code const& key,
const T& value) // TODO: Fix
{
std::size_t hash = getBucket(key);
if (std::any_of(std::execution::seq, data_[hash].begin(), data_[hash].end(),
[&key](const auto& elem) { return key == elem.first; })) {
return std::make_pair(0, false); // TODO: Fix
}
++size_;
if (load_factor() > max_load_factor() && power_ < MAX_POWER) {
rehash(num_buckets_ * 2);
hash = getBucket(key);
}
data_[hash].emplace_front(key, value);
return std::make_pair(0, true); // TODO: Fix
}
void clear()
{
std::for_each(std::execution::seq, data_.begin(), data_.end(),
[](auto& bucket) { bucket.clear(); });
size_ = 0;
}
bool empty() const { return 0 == size_; }
std::size_t size() { return size_; }
std::size_t bucket_count() const { return num_buckets_; }
unsigned int bucket_count_power() const { return power_; }
float load_factor() const { return size_ / ((float)num_buckets_); }
float max_load_factor() const { return max_load_factor_; }
void max_load_factor(float max_load_factor)
{
max_load_factor_ = max_load_factor;
if (load_factor() > max_load_factor_ && power_ < MAX_POWER) {
rehash(num_buckets_ * 2);
}
}
void rehash(std::size_t count)
{
std::size_t min_count = std::max((float)count, size() / max_load_factor());
unsigned int power =
std::max(power_, std::min((unsigned int)std::log2(min_count) + 1, MAX_POWER));
if (power_ == power) {
return;
}
power_ = power;
num_buckets_ = std::size_t(1) << power_;
decltype(data_) new_data;
new_data.resize(num_buckets_);
for (const auto& [key, value] : *this) {
new_data[getBucket(key)].emplace_front(key, value);
}
data_.swap(new_data);
// fprintf(stderr, "\n\nRehash, new power: %u\n\n", power_);
}
void reserve(std::size_t count)
{
power_ = std::max(power_, std::min((unsigned int)std::log2(count) + 1, MAX_POWER));
data_.reserve(std::size_t(1) << power_);
}
CodeMapIterator begin() const { return CodeMapIterator(this); }
CodeMapIterator end() const { return CodeMapIterator(); }
void swap(CodeMap<T>& other)
{
data_.swap(other.data_);
std::swap(power_, other.power_);
std::swap(num_buckets_, other.num_buckets_);
std::swap(size_, other.size_);
std::swap(max_load_factor_, other.max_load_factor_);
}
private:
std::size_t getBucket(Code const& key) const
{
unsigned int offset = 3 * key.getDepth();
unsigned int modder = (num_buckets_ - 1) << offset;
return (Code::Hash()(key) & modder) >> offset;
}
private:
std::vector<std::list<std::pair<Code, T>>> data_;
unsigned int power_;
std::size_t num_buckets_;
std::size_t size_ = 0;
float max_load_factor_ = 1.0;
inline static const unsigned int MAX_POWER = 28;
friend struct CodeMapIterator;
};
} // namespace ufo::map
#endif // UFO_MAP_CODE_H
ufomap/include/ufo/map/color.h
Executable
+89
View File
@@ -0,0 +1,89 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_COLOR_H
#define UFO_MAP_COLOR_H
// STD
#include <cstdint>
namespace ufo::map
{
using ColorType = uint8_t;
/**
* @brief RGB color
*
*/
struct Color {
ColorType r;
ColorType g;
ColorType b;
Color() : r(0), g(0), b(0) {}
Color(ColorType r, ColorType g, ColorType b) : r(r), g(g), b(b) {}
Color(Color const& other) : r(other.r), g(other.g), b(other.b) {}
Color& operator=(Color const& rhs)
{
r = rhs.r;
g = rhs.g;
b = rhs.b;
return *this;
}
bool operator==(Color const& other) const
{
return other.r == r && other.g == g && other.b == b;
}
bool operator!=(Color const& other) const
{
return other.r != r || other.g != g || other.b != b;
}
bool isSet() const { return 0 != r || 0 != g || 0 != b; }
};
} // namespace ufo::map
#endif // UFO_MAP_COLOR_H
ufomap/include/ufo/map/iterator/occupancy_map.h
Executable
+219
View File
@@ -0,0 +1,219 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_ITERATOR_OCCUPANCY_MAP_H
#define UFO_MAP_ITERATOR_OCCUPANCY_MAP_H
#include <ufo/map/iterator/octree.h>
namespace ufo::map
{
template <typename TREE, typename DATA_TYPE, typename INNER_NODE, typename LEAF_NODE,
bool ONLY_LEAF>
class OccupancyMapIterator
: public OctreeIterator<TREE, DATA_TYPE, INNER_NODE, LEAF_NODE, ONLY_LEAF>
{
private:
using Base = OctreeIterator<TREE, DATA_TYPE, INNER_NODE, LEAF_NODE, ONLY_LEAF>;
using IteratorNode = typename Base::IteratorNode;
public:
OccupancyMapIterator() {}
OccupancyMapIterator(TREE const* tree, INNER_NODE const& root,
ufo::geometry::BoundingVolume const& bounding_volume,
bool occupied_space = true, bool free_space = true,
bool unknown_space = false, bool contains = false,
DepthType min_depth = 0)
: Base(tree, bounding_volume, min_depth),
occupied_space_(occupied_space),
free_space_(free_space),
unknown_space_(unknown_space),
contains_(contains)
{
Base::init(root);
}
OccupancyMapIterator(OccupancyMapIterator const& other)
: Base(other),
occupied_space_(other.occupied_space_),
free_space_(other.free_space_),
unknown_space_(other.unknown_space_),
contains_(other.contains_)
{
}
OccupancyMapIterator& operator=(OccupancyMapIterator const& rhs)
{
Base::operator=(rhs);
occupied_space_ = rhs.occupied_space_;
free_space_ = rhs.free_space_;
unknown_space_ = rhs.unknown_space_;
contains_ = rhs.contains_;
return *this;
}
bool operator==(OccupancyMapIterator const& rhs) const { return Base::operator==(rhs); }
bool operator!=(OccupancyMapIterator const& rhs) const { return Base::operator!=(rhs); }
// Postfix increment
OccupancyMapIterator operator++(int)
{
OccupancyMapIterator result = *this;
++(*this);
return result;
}
// Prefix increment
OccupancyMapIterator& operator++()
{
Base::increment();
return *this;
}
public:
bool isOccupied() const { return isOccupied(Base::path_[Base::getDepth()]); }
bool isFree() const { return isFree(Base::path_[Base::getDepth()]); }
bool isUnknown() const { return isUnknown(Base::path_[Base::getDepth()]); }
bool containsOccupied() const
{
return containsOccupied(Base::path_[Base::getDepth()], Base::getDepth());
}
bool containsFree() const
{
return containsFree(Base::path_[Base::getDepth()], Base::getDepth());
}
bool containsUnknown() const
{
return containsUnknown(Base::path_[Base::getDepth()], Base::getDepth());
}
double getOccupancy() const
{
return Base::tree_->getOccupancy(*Base::path_[Base::getDepth()].node);
}
protected:
bool isOccupied(IteratorNode const& node) const
{
return Base::tree_->isOccupied(*node.node);
}
bool isFree(IteratorNode const& node) const { return Base::tree_->isFree(*node.node); }
bool isUnknown(IteratorNode const& node) const
{
return Base::tree_->isUnknown(*node.node);
}
bool containsOccupied(IteratorNode const& node, unsigned int depth) const
{
return Base::tree_->containsOccupied(*node.node, depth);
}
bool containsFree(IteratorNode const& node, unsigned int depth) const
{
return Base::tree_->containsFree(*node.node, depth);
}
bool containsUnknown(IteratorNode const& node, unsigned int depth) const
{
return Base::tree_->containsUnknown(*node.node, depth);
}
virtual bool validNode(IteratorNode& node, unsigned int depth) const override
{
if (!Base::validNode(node, depth)) {
return false;
}
// printf("Depth %u\n", depth);
// printf("Min depth %u\n", Base::min_depth_);
if (contains_ || Base::min_depth_ != depth) {
// printf("Occupied space: %s\n", occupied_space_ ? "True" : "False");
// printf("Unknown space: %s\n", unknown_space_ ? "True" : "False");
// printf("Free space: %s\n", free_space_ ? "True" : "False");
// printf("Contains occupied: %s\n", containsOccupied(node, depth) ? "True" : "False");
// printf("Contains unknown: %s\n", containsUnknown(node, depth) ? "True" : "False");
// printf("Contains Free: %s\n", containsFree(node, depth) ? "True" : "False");
return (occupied_space_ && containsOccupied(node, depth)) ||
(unknown_space_ && containsUnknown(node, depth)) ||
(free_space_ && containsFree(node, depth));
}
return (occupied_space_ && isOccupied(node)) || (unknown_space_ && isUnknown(node)) ||
(free_space_ && isFree(node));
}
virtual bool validReturnNode() const override
{
if (!Base::validReturnNode()) {
return false;
}
if constexpr (!ONLY_LEAF) {
if (contains_) {
return (occupied_space_ && containsOccupied()) ||
(unknown_space_ && containsUnknown()) || (free_space_ && containsFree());
}
}
return (occupied_space_ && isOccupied()) || (unknown_space_ && isUnknown()) ||
(free_space_ && isFree());
}
protected:
bool occupied_space_;
bool free_space_;
bool unknown_space_;
bool contains_;
};
} // namespace ufo::map
#endif // UFO_MAP_ITERATOR_OCCUPANCY_MAP_H
ufomap/include/ufo/map/iterator/occupancy_map_nearest.h
Executable
+217
View File
@@ -0,0 +1,217 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_ITERATOR_NEAREST_OCCUPANCY_MAP_H
#define UFO_MAP_ITERATOR_NEAREST_OCCUPANCY_MAP_H
#include <ufo/map/iterator/octree_nearest.h>
namespace ufo::map
{
template <typename TREE, typename DATA_TYPE, typename INNER_NODE, typename LEAF_NODE,
bool ONLY_LEAF>
class OccupancyMapNearestIterator
: public OctreeNearestIterator<TREE, DATA_TYPE, INNER_NODE, LEAF_NODE, ONLY_LEAF>
{
private:
using Base = OctreeNearestIterator<TREE, DATA_TYPE, INNER_NODE, LEAF_NODE, ONLY_LEAF>;
using IteratorNode = typename Base::IteratorNode;
public:
OccupancyMapNearestIterator() {}
OccupancyMapNearestIterator(TREE const* tree, INNER_NODE const& root,
ufo::geometry::BoundingVolume const& bounding_volume,
Point3 const& coordinate, bool occupied_space = true,
bool free_space = true, bool unknown_space = false,
bool contains = false, DepthType min_depth = 0)
: Base(tree, bounding_volume, coordinate, min_depth),
occupied_space_(occupied_space),
free_space_(free_space),
unknown_space_(unknown_space),
contains_(contains)
{
Base::init(root);
}
OccupancyMapNearestIterator(OccupancyMapNearestIterator const& other)
: Base(other),
occupied_space_(other.occupied_space_),
free_space_(other.free_space_),
unknown_space_(other.unknown_space_),
contains_(other.contains_)
{
}
OccupancyMapNearestIterator& operator=(OccupancyMapNearestIterator const& rhs)
{
Base::operator=(rhs);
occupied_space_ = rhs.occupied_space_;
free_space_ = rhs.free_space_;
unknown_space_ = rhs.unknown_space_;
contains_ = rhs.contains_;
return *this;
}
bool operator==(OccupancyMapNearestIterator const& rhs) const
{
return Base::operator==(rhs);
}
bool operator!=(OccupancyMapNearestIterator const& rhs) const
{
return Base::operator!=(rhs);
}
// Postfix increment
OccupancyMapNearestIterator operator++(int)
{
OccupancyMapNearestIterator result = *this;
++(*this);
return result;
}
// Prefix increment
OccupancyMapNearestIterator& operator++()
{
Base::increment();
return *this;
}
public:
bool isOccupied() const { return isOccupied(Base::container_.top()); }
bool isFree() const { return isFree(Base::container_.top()); }
bool isUnknown() const { return isUnknown(Base::container_.top()); }
bool containsOccupied() const
{
return containsOccupied(Base::container_.top(), Base::getDepth());
}
bool containsFree() const
{
return containsFree(Base::container_.top(), Base::getDepth());
}
bool containsUnknown() const
{
return containsUnknown(Base::container_.top(), Base::getDepth());
}
double getOccupancy() const
{
return Base::tree_->getOccupancy(*Base::container_.top().node);
}
protected:
bool isOccupied(IteratorNode const& node) const
{
return Base::tree_->isOccupied(*node.node);
}
bool isFree(IteratorNode const& node) const { return Base::tree_->isFree(*node.node); }
bool isUnknown(IteratorNode const& node) const
{
return Base::tree_->isUnknown(*node.node);
}
bool containsOccupied(IteratorNode const& node) const
{
return Base::tree_->containsOccupied(*node.node, node.depth);
}
bool containsFree(IteratorNode const& node) const
{
return Base::tree_->containsFree(*node.node, node.depth);
}
bool containsUnknown(IteratorNode const& node) const
{
return Base::tree_->containsUnknown(*node.node, node.depth);
}
virtual bool validNode(IteratorNode const& node,
ufo::geometry::AABB const& node_aabb) const override
{
if (!Base::validNode(node, node_aabb)) {
return false;
}
if (contains_ || Base::min_depth_ != node.depth) {
return (occupied_space_ && containsOccupied(node)) ||
(unknown_space_ && containsUnknown(node)) ||
(free_space_ && containsFree(node));
}
return (occupied_space_ && isOccupied(node)) || (unknown_space_ && isUnknown(node)) ||
(free_space_ && isFree(node));
}
virtual bool validReturnNode() const override
{
if (!Base::validReturnNode()) {
return false;
}
if constexpr (!ONLY_LEAF) {
if (contains_) {
return (occupied_space_ && containsOccupied()) ||
(unknown_space_ && containsUnknown()) || (free_space_ && containsFree());
}
}
return (occupied_space_ && isOccupied()) || (unknown_space_ && isUnknown()) ||
(free_space_ && isFree());
}
protected:
bool occupied_space_;
bool free_space_;
bool unknown_space_;
bool contains_;
};
} // namespace ufo::map
#endif // UFO_MAP_ITERATOR_NEAREST_OCCUPANCY_MAP_H
ufomap/include/ufo/map/iterator/octree.h
Executable
+328
View File
@@ -0,0 +1,328 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_ITERATOR_OCTREE_H
#define UFO_MAP_ITERATOR_OCTREE_H
#include <ufo/geometry/bounding_volume.h>
#include <ufo/geometry/collision_checks.h>
#include <ufo/map/code.h>
#include <ufo/map/types.h>
#include <array>
#include <type_traits>
namespace ufo::map
{
template <typename TREE, typename DATA_TYPE, typename INNER_NODE, typename LEAF_NODE,
bool ONLY_LEAF>
class OctreeIterator
{
protected:
struct IteratorNode {
// Pointer to the actual node
LEAF_NODE const* node;
// The index of the node from its parent
unsigned int index;
// The AABB for the node
ufo::geometry::AABB aabb;
// Indicates if this node is completely inside the bounding volume.
// Meaning its children does not have to do any intersection checks.
bool inside;
};
public:
OctreeIterator() {}
OctreeIterator(TREE const* tree, ufo::geometry::BoundingVolume const& bounding_volume,
DepthType min_depth = 0)
: tree_(tree), bounding_volume_(bounding_volume), min_depth_(min_depth)
{
}
OctreeIterator(TREE const* tree, INNER_NODE const& root,
ufo::geometry::BoundingVolume const& bounding_volume,
unsigned int min_depth = 0)
: OctreeIterator(tree, bounding_volume, min_depth)
{
init(root);
}
OctreeIterator(OctreeIterator const& other)
: tree_(other.tree_),
bounding_volume_(other.bounding_volume_),
path_(other.path_),
current_depth_(other.current_depth_),
min_depth_(other.min_depth_),
max_depth_(other.max_depth_)
{
}
OctreeIterator& operator=(OctreeIterator const& rhs)
{
tree_ = rhs.tree_;
bounding_volume_ = rhs.bounding_volume_;
path_ = rhs.path_;
current_depth_ = rhs.current_depth_;
min_depth_ = rhs.min_depth_;
max_depth_ = rhs.max_depth_;
return *this;
}
bool operator==(OctreeIterator const& rhs) const
{
// TODO: Can be improved
return rhs.tree_ == tree_; // && rhs.path_ == path_;
}
bool operator!=(OctreeIterator const& rhs) const { return !(*this == rhs); }
// Postfix increment
OctreeIterator operator++(int)
{
OctreeIterator result = *this;
++(*this);
return result;
}
// Prefix increment
OctreeIterator& operator++()
{
increment();
return *this;
}
DATA_TYPE const* operator->() const { return &(path_[current_depth_].node->value); }
DATA_TYPE const& operator*() const { return path_[current_depth_].node->value; }
double getSize() const { return tree_->getNodeSize(getDepth()); }
double getHalfSize() const { return tree_->getNodeHalfSize(getDepth()); }
ufo::geometry::AABB getBoundingVolume() const { return path_[current_depth_].aabb; }
// bool completelyInsideBoundingVolume() const { return path_[current_depth_].inside; }
DepthType getDepth() const { return current_depth_; }
Point3 getCenter() const { return path_[current_depth_].aabb.center; }
Code getCode(DepthType depth = 0) const
{
// TODO: Improve
Code code = tree_->getRootCode();
for (int d = code.getDepth() - 1;
static_cast<int>(getDepth()) <= d && static_cast<int>(depth) <= d; --d) {
code = code.getChild(path_[d].index);
}
return code;
}
double getX() const { return path_[current_depth_].aabb.center[0]; }
double getY() const { return path_[current_depth_].aabb.center[1]; }
double getZ() const { return path_[current_depth_].aabb.center[2]; }
bool isPureLeaf() const { return 0 == getDepth(); }
bool isLeaf() const { return tree_->isLeaf(path_[getDepth()].node, getDepth()); }
bool hasChildren() const { return !isLeaf(); }
public:
// iterator traits
using difference_type = std::ptrdiff_t; // What should this be?
using value_type = DATA_TYPE;
using pointer = DATA_TYPE const*;
using reference = DATA_TYPE const&;
using iterator_category = std::forward_iterator_tag;
protected:
void init(INNER_NODE const& root)
{
current_depth_ = tree_->getTreeDepthLevels();
max_depth_ = current_depth_;
IteratorNode node;
node.node = &root;
node.index = 7; // Root has no siblings
node.aabb.center = Point3(0, 0, 0);
double s = tree_->getNodeHalfSize(current_depth_);
node.aabb.half_size = Point3(s, s, s);
node.inside = bounding_volume_.empty();
// Prefill AABBs
for (size_t depth = min_depth_; depth < current_depth_; ++depth) {
s = tree_->getNodeHalfSize(depth);
path_[depth].aabb.half_size = Point3(s, s, s);
}
if (validNode(node, current_depth_) && current_depth_ >= min_depth_) {
path_[current_depth_] = node;
if (!validReturnNode()) {
operator++();
}
} else {
// Same as end iterator
tree_ = nullptr;
}
}
bool hasMore() const { return getDepth() <= max_depth_; }
virtual bool validNode(IteratorNode& node, DepthType) const
{
if (node.inside) {
return true;
}
// Bounding volume can only contain one otherwise we cannot make sure the node is
// completely inside and we therefore only do the simple intersection test instead
// if (0 < depth && 1 == bounding_volume_.size()) {
// // Check if the node is completely inside the bounding volume
// double hs = node.aabb.half_size[0];
// Point3 const& c = node.aabb.center;
// std::array<Point3, 8> corners = {Point3(-hs, -hs, -hs), Point3(-hs, -hs, hs),
// Point3(-hs, hs, -hs), Point3(-hs, hs, hs),
// Point3(hs, -hs, -hs), Point3(hs, -hs, hs),
// Point3(hs, hs, -hs), Point3(hs, hs, hs)};
// bool one_inside = false;
// node.inside = true;
// for (Point3 const& offset : corners) {
// if (!bounding_volume_.intersects(c + offset)) {
// // The node is not completely inside the bounding volume
// node.inside = false;
// break;
// }
// one_inside = true;
// }
// if (one_inside) {
// return true;
// }
// }
return bounding_volume_.intersects(node.aabb);
}
virtual bool validReturnNode() const
{
if constexpr (ONLY_LEAF) {
return min_depth_ == getDepth() || isLeaf();
} else {
return true;
}
}
void increment()
{
if (!hasMore()) {
tree_ = nullptr;
} else {
// Skip forward to next valid return node
do {
singleIncrement();
} while (hasMore() && !validReturnNode());
if (!hasMore()) {
tree_ = nullptr;
}
}
}
virtual void singleIncrement()
{
if (getDepth() <= min_depth_ || isLeaf()) {
// Move to next leaf if possible
++path_[current_depth_].index;
} else {
// Current node has children
--current_depth_;
path_[current_depth_].index = 0;
}
if (!getNextNode()) {
// Move up in the tree until valid node is found
for (++current_depth_; current_depth_ < max_depth_; ++current_depth_) {
++path_[current_depth_].index;
if (getNextNode()) {
return;
}
}
if (current_depth_ == max_depth_) {
++current_depth_;
}
}
}
bool getNextNode()
{
DepthType depth = current_depth_;
DepthType parent_depth = depth + 1;
Point3 const& p_center = path_[parent_depth].aabb.center;
double hs = path_[depth].aabb.half_size[0];
for (; 8 > path_[depth].index; ++path_[depth].index) {
INNER_NODE const& inner_node =
static_cast<INNER_NODE const&>(*path_[parent_depth].node);
path_[depth].node = &tree_->getChild(inner_node, depth, path_[depth].index);
path_[depth].aabb.center = tree_->getChildCenter(p_center, hs, path_[depth].index);
path_[depth].inside = path_[parent_depth].inside;
if (validNode(path_[depth], depth)) {
return true;
}
}
return false;
}
protected:
TREE const* tree_ = nullptr;
ufo::geometry::BoundingVolume bounding_volume_;
std::array<IteratorNode, TREE::getMaxDepthLevels()> path_;
DepthType current_depth_;
DepthType min_depth_;
DepthType max_depth_;
};
} // namespace ufo::map
#endif // UFO_MAP_ITERATOR_OCTREE_H
ufomap/include/ufo/map/iterator/octree_nearest.h
Executable
+278
View File
@@ -0,0 +1,278 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_ITERATOR_NEAREST_OCTREE_H
#define UFO_MAP_ITERATOR_NEAREST_OCTREE_H
#include <ufo/geometry/bounding_volume.h>
#include <ufo/geometry/collision_checks.h>
#include <ufo/map/types.h>
#include <queue>
#include <type_traits>
#include <vector>
namespace ufo::map
{
template <typename TREE, typename DATA_TYPE, typename INNER_NODE, typename LEAF_NODE,
bool ONLY_LEAF>
class OctreeNearestIterator
{
protected:
struct IteratorNode {
// Pointer to the actual node
LEAF_NODE const* node;
// Squared distance to coordinate
double squared_distance = 0;
// The center of the node
Point3 center;
// The depth of the node
DepthType depth;
};
public:
OctreeNearestIterator() {}
OctreeNearestIterator(TREE const* tree,
ufo::geometry::BoundingVolume const& bounding_volume,
Point3 const& coordinate, DepthType min_depth = 0)
: tree_(tree),
bounding_volume_(bounding_volume),
coordinate_(coordinate),
min_depth_(min_depth)
{
}
OctreeNearestIterator(TREE const* tree, INNER_NODE const& root,
ufo::geometry::BoundingVolume const& bounding_volume,
Point3 const& coordinate, DepthType min_depth = 0)
: OctreeNearestIterator(tree, bounding_volume, coordinate, min_depth)
{
init(root);
}
OctreeNearestIterator(OctreeNearestIterator const& other)
: tree_(other.tree_),
bounding_volume_(other.bounding_volume_),
coordinate_(other.coordinate_),
container_(other.container_),
min_depth_(other.min_depth_)
{
}
OctreeNearestIterator& operator=(OctreeNearestIterator const& rhs)
{
tree_ = rhs.tree_;
bounding_volume_ = rhs.bounding_volume_;
coordinate_ = rhs.coordinate_;
container_ = rhs.container_;
min_depth_ = rhs.min_depth_;
return *this;
}
bool operator==(OctreeNearestIterator const& rhs) const
{
return rhs.tree_ == tree_;
// return (rhs.tree_ == tree_ && rhs.container_.size() == container_.size() &&
// (container_.empty() || (rhs.container_.top() == container_.top())));
}
bool operator!=(OctreeNearestIterator const& rhs) const { return !(*this == rhs); }
// Postfix increment
OctreeNearestIterator operator++(int)
{
OctreeNearestIterator result = *this;
++(*this);
return result;
}
// Prefix increment
OctreeNearestIterator& operator++()
{
increment();
return *this;
}
DATA_TYPE const* operator->() const { return &(container_.top().node->value); }
DATA_TYPE const& operator*() const { return container_.top().node->value; }
double getDistance() const { return std::sqrt(getSquaredDistance()); }
double getSquaredDistance() const { return container_.top().squared_distance; }
double getSize() const { return tree_->getNodeSize(getDepth()); }
double getHalfSize() const { return tree_->getNodeHalfSize(getDepth()); }
ufo::geometry::AABB getAABB() const
{
return ufo::geometry::AABB(getCenter(), getHalfSize());
}
DepthType getDepth() const { return container_.top().depth; }
Point3 const& getCenter() const { return container_.top().center; }
double getX() const { return getCenter().x(); }
double getY() const { return getCenter().y(); }
double getZ() const { return getCenter().z(); }
bool isPureLeaf() const { return 0 == getDepth(); }
bool isLeaf() const { return tree_->isLeaf(container_.top().node, getDepth()); }
bool hasChildren() const { return !isLeaf(); }
public:
// iterator traits
using difference_type = std::ptrdiff_t; // What should this be?
using value_type = DATA_TYPE;
using pointer = DATA_TYPE const*;
using reference = DATA_TYPE const&;
using iterator_category = std::forward_iterator_tag;
protected:
void init(INNER_NODE const& root)
{
IteratorNode node;
node.node = &root;
node.depth = tree_->getTreeDepthLevels();
node.center = Point3(0, 0, 0);
ufo::geometry::AABB node_aabb(node.center, tree_->getNodeHalfSize(node.depth));
if (validNode(node, node_aabb) && node.depth >= min_depth_) {
node.squared_distance = squaredDistance(node_aabb);
container_.push(node);
if (!validReturnNode()) {
operator++();
}
} else {
// Same as end iterator
tree_ = nullptr;
}
}
// Nearest
double squaredDistance(ufo::geometry::AABB const& aabb) const
{
Point3 closest_point = Point3::clamp(coordinate_, aabb.getMin(), aabb.getMax());
return (coordinate_ - closest_point).squaredNorm();
}
virtual bool validNode(IteratorNode const&, ufo::geometry::AABB const& node_aabb) const
{
return bounding_volume_.empty() || bounding_volume_.intersects(node_aabb);
}
virtual bool validReturnNode() const
{
if constexpr (ONLY_LEAF) {
return min_depth_ == getDepth() || isLeaf();
} else {
return true;
}
}
void increment()
{
if (container_.empty()) {
tree_ = nullptr;
} else {
// Skip forward to next valid node
do {
singleIncrement();
} while (!container_.empty() && !validReturnNode());
if (container_.empty()) {
tree_ = nullptr;
}
}
}
void singleIncrement()
{
IteratorNode top = container_.top();
container_.pop();
if (getDepth() <= min_depth_ || isLeaf()) {
return;
}
// We know it is a inner node
INNER_NODE const& inner_node = static_cast<INNER_NODE const&>(*top.node);
IteratorNode node;
node.depth = top.depth - 1;
double child_half_size = tree_->getNodeHalfSize(node.depth);
for (size_t idx = 0; 8 > idx; ++idx) {
node.node = &tree_->getChild(inner_node, node.depth, idx);
node.center = tree_->getChildCenter(top.center, child_half_size, idx);
ufo::geometry::AABB node_aabb(node.center, child_half_size);
if (validNode(node, node_aabb)) {
node.squared_distance = squaredDistance(node_aabb);
container_.push(node);
}
}
}
protected:
TREE const* tree_ = nullptr;
ufo::geometry::BoundingVolume bounding_volume_;
Point3 coordinate_;
struct Compare {
bool operator()(IteratorNode const& left, IteratorNode const& right)
{
return left.squared_distance > right.squared_distance;
}
};
std::priority_queue<IteratorNode, std::vector<IteratorNode>, Compare> container_;
DepthType min_depth_;
};
} // namespace ufo::map
#endif // UFO_MAP_ITERATOR_NEAREST_OCTREE_H
ufomap/include/ufo/map/key.h
Executable
+203
View File
@@ -0,0 +1,203 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_KEY_H
#define UFO_MAP_KEY_H
// UFO
#include <ufo/map/types.h>
// STD
#include <immintrin.h>
#include <array>
#include <cstddef>
#include <unordered_map>
#include <unordered_set>
#include <vector>
namespace ufo::map
{
/**
* @brief A key represent an octree index at a specified depth
*
*/
class Key
{
public:
Key() {}
Key(KeyType x, KeyType y, KeyType z, DepthType depth) : key_{x, y, z}, depth_(depth) {}
Key(Key const& other) : key_{other.key_}, depth_(other.depth_) {}
Key& operator=(Key const& rhs)
{
key_ = rhs.key_;
depth_ = rhs.depth_;
return *this;
}
/**
* @brief Get the depth that this key is specified at
*
* @return DepthType The depth this key is specified at
*/
DepthType getDepth() const { return depth_; }
/**
* @brief Returns if this key is equal to another key at a certain depth
*
* @param other The other key to compare to
* @param depth The depth to do the comparison at
* @return true If the keys are equal at the specified depth
* @return false If the keys are not equal at the specified depth
*/
bool equals(Key const& other, DepthType depth = 0) const
{
return (key_[0] >> depth) == (other.key_[0] >> depth) &&
(key_[1] >> depth) == (other.key_[1] >> depth) &&
(key_[2] >> depth) == (other.key_[2] >> depth);
}
bool operator==(Key const& rhs) const
{
return (rhs.depth_ == depth_) && (rhs.key_ == key_);
}
bool operator!=(Key const& rhs) const
{
return (rhs.depth_ != depth_) || (rhs.key_ != key_);
}
KeyType const& operator[](std::size_t index) const { return key_[index]; }
KeyType& operator[](std::size_t index) { return key_[index]; }
/**
* @brief Returns the x component of the key
*
* @return const KeyType& The x component of the key
*/
KeyType const& x() const { return key_[0]; }
/**
* @brief Returns the y component of the key
*
* @return const KeyType& The y component of the key
*/
KeyType const& y() const { return key_[1]; }
/**
* @brief Returns the z component of the key
*
* @return const KeyType& The z component of the key
*/
KeyType const& z() const { return key_[2]; }
/**
* @brief Returns the x component of the key
*
* @return KeyType& The x component of the key
*/
KeyType& x() { return key_[0]; }
/**
* @brief Returns the y component of the key
*
* @return KeyType& The y component of the key
*/
KeyType& y() { return key_[1]; }
/**
* @brief Returns the z component of the key
*
* @return KeyType& The z component of the key
*/
KeyType& z() { return key_[2]; }
/**
* @brief
*
*/
struct Hash {
std::size_t operator()(Key const& key) const
{
#if defined(__BMI2__) // TODO: Is correct?
return _pdep_u64(static_cast<CodeType>(key[0]), 0x9249249249249249) |
_pdep_u64(static_cast<CodeType>(key[1]), 0x2492492492492492) |
_pdep_u64(static_cast<CodeType>(key[2]), 0x4924924924924924);
#else
return splitBy3(key[0]) | (splitBy3(key[1]) << 1) | (splitBy3(key[2]) << 2);
#endif
}
};
private:
static uint64_t splitBy3(KeyType a)
{
#if defined(__BMI2__) // TODO: Is correct?
return _pdep_u64(static_cast<uint64_t>(a), 0x9249249249249249);
#else
uint64_t code = static_cast<uint64_t>(a) & 0x1fffff;
code = (code | code << 32) & 0x1f00000000ffff;
code = (code | code << 16) & 0x1f0000ff0000ff;
code = (code | code << 8) & 0x100f00f00f00f00f;
code = (code | code << 4) & 0x10c30c30c30c30c3;
code = (code | code << 2) & 0x1249249249249249;
return code;
#endif
}
private:
// The key
std::array<KeyType, 3> key_;
// The depth of the key
DepthType depth_;
};
using KeySet = std::unordered_set<Key, Key::Hash>;
template <typename T>
using KeyMap = std::unordered_map<Key, T, Key::Hash>;
using KeyRay = std::vector<Key>;
} // namespace ufo::map
#endif // UFO_MAP_KEY_H
ufomap/include/ufo/map/occupancy_map.h
Executable
+86
View File
@@ -0,0 +1,86 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_OCCUPANCY_MAP_H
#define UFO_MAP_OCCUPANCY_MAP_H
#include <ufo/map/occupancy_map_base.h>
namespace ufo::map
{
class OccupancyMap : public OccupancyMapBase<OccupancyNode<float>>
{
private:
using DATA_TYPE = OccupancyNode<float>;
using Base = OccupancyMapBase<DATA_TYPE>;
public:
//
// Constructors
//
OccupancyMap(double resolution, DepthType depth_levels = 16,
bool automatic_pruning = true, double occupied_thres = 0.5,
double free_thres = 0.5, double prob_hit = 0.7, double prob_miss = 0.4,
double clamping_thres_min = 0.1192, double clamping_thres_max = 0.971);
OccupancyMap(std::string const& filename, bool automatic_pruning = true,
double occupied_thres = 0.5, double free_thres = 0.5,
double prob_hit = 0.7, double prob_miss = 0.4,
double clamping_thres_min = 0.1192, double clamping_thres_max = 0.971);
OccupancyMap(OccupancyMap const& other);
//
// Destructor
//
virtual ~OccupancyMap() {}
//
// Tree Type
//
virtual std::string getTreeType() const noexcept override { return "occupancy_map"; }
};
} // namespace ufo::map
#endif // UFO_MAP_OCCUPANCY_MAP_H
ufomap/include/ufo/map/occupancy_map_base.h
Executable
+1562
View File
File diff suppressed because it is too large Load Diff
ufomap/include/ufo/map/occupancy_map_color.h
Executable
+403
View File
@@ -0,0 +1,403 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_OCCUPANCY_MAP_COLOR_H
#define UFO_MAP_OCCUPANCY_MAP_COLOR_H
#include <ufo/map/occupancy_map_base.h>
namespace ufo::map
{
class OccupancyMapColor : public OccupancyMapBase<ColorOccupancyNode<float>>
{
private:
using DATA_TYPE = ColorOccupancyNode<float>;
using Base = OccupancyMapBase<DATA_TYPE>;
public:
//
// Constructors
//
OccupancyMapColor(double resolution, DepthType depth_levels = 16,
bool automatic_pruning = true, double occupied_thres = 0.5,
double free_thres = 0.5, double prob_hit = 0.7,
double prob_miss = 0.4, double clamping_thres_min = 0.1192,
double clamping_thres_max = 0.971);
OccupancyMapColor(std::string const& filename, bool automatic_pruning = true,
double occupied_thres = 0.5, double free_thres = 0.5,
double prob_hit = 0.7, double prob_miss = 0.4,
double clamping_thres_min = 0.1192,
double clamping_thres_max = 0.971);
OccupancyMapColor(OccupancyMapColor const& other);
//
// Destructor
//
virtual ~OccupancyMapColor() {}
//
// Tree Type
//
virtual std::string getTreeType() const noexcept override
{
return "occupancy_map_color";
}
//
// Integration
//
template <typename T>
void insertPointCloud(Point3 const& sensor_origin, T const& cloud,
double max_range = -1, DepthType depth = 0,
bool simple_ray_casting = false, unsigned int early_stopping = 0,
bool async = false)
{
if constexpr (std::is_same_v<T, PointCloud>) {
Base::insertPointCloud(sensor_origin, cloud, max_range, depth, simple_ray_casting,
early_stopping, async);
if constexpr (std::is_same_v<T, PointCloudColor>) {
integrateColors(sensor_origin, cloud, max_range);
}
} else if constexpr (std::is_same_v<T, PointCloudColor>) {
std::vector<std::tuple<Code, float, Color>> occupied_hits;
occupied_hits.reserve(cloud.size());
PointCloud discretized;
discretized.reserve(cloud.size());
Point3 min_change = Base::getMax();
Point3 max_change = Base::getMin();
for (Point3Color& end_color : cloud) {
Point3 end = end_color;
Point3 origin = sensor_origin;
Point3 direction = (end - origin);
double distance = direction.norm();
// Move origin and end inside BBX
if (!Base::moveLineInside(origin, end)) {
// Line outside of BBX
continue;
}
if (0 > max_range || distance <= max_range) {
// Occupied space
Code end_code = Base::toCode(end);
if (indices_.insert(end_code).second) {
occupied_hits.push_back(
std::make_tuple(end_code, prob_hit_log_, end_color.getColor()));
}
} else {
direction /= distance;
end = origin + (direction * max_range);
}
discretized.push_back(end);
for (int i : {0, 1, 2}) {
min_change[i] = std::min(min_change[i], std::min(end[i], origin[i]));
max_change[i] = std::max(max_change[i], std::max(end[i], origin[i]));
}
}
LogitType prob_miss_log = prob_miss_log_ / double((2.0 * depth) + 1);
indices_.clear();
Base::insertPointCloudWait();
if (async) {
integrate_ =
std::async(std::launch::async, &OccupancyMapColor::insertPointCloudHelper,
this, sensor_origin, std::move(discretized),
std::move(occupied_hits), prob_miss_log, depth, simple_ray_casting,
early_stopping, min_change, max_change);
} else {
insertPointCloudHelper(sensor_origin, std::move(discretized),
std::move(occupied_hits), prob_miss_log, depth,
simple_ray_casting, early_stopping, min_change,
max_change);
}
}
}
template <typename T>
void insertPointCloud(Point3 const& sensor_origin, T cloud,
math::Pose6 const& frame_origin, double max_range = -1,
DepthType depth = 0, bool simple_ray_casting = false,
unsigned int early_stopping = 0, bool async = false)
{
cloud.transform(frame_origin, async);
insertPointCloud(sensor_origin, cloud, max_range, depth, simple_ray_casting,
early_stopping, async);
}
template <typename T>
void insertPointCloudDiscrete(Point3 const& sensor_origin, T const& cloud,
double max_range = -1, DepthType depth = 0,
bool simple_ray_casting = false,
unsigned int early_stopping = 0, bool async = false)
{
if constexpr (std::is_same_v<T, PointCloud>) {
Base::insertPointCloudDiscrete(sensor_origin, cloud, max_range, depth,
simple_ray_casting, early_stopping);
} else if constexpr (std::is_same_v<T, PointCloudColor>) {
double squared_max_range = max_range * max_range;
std::vector<std::tuple<Code, float, Color>> occupied_hits;
occupied_hits.reserve(cloud.size());
PointCloud discretized;
discretized.reserve(cloud.size());
Point3 min_change = Base::getMax();
Point3 max_change = Base::getMin();
for (Point3Color const& end_color : cloud) {
Point3 end = end_color;
double dist_sqrt = (end - sensor_origin).squaredNorm();
if (0 > max_range || dist_sqrt < squared_max_range) {
if (Base::isInside(end)) {
Code end_code = Base::toCode(end);
if (!indices_.insert(end_code).second) {
continue;
}
// double dist = std::sqrt(dist_sqrt);
// testis.push_back(std::make_tuple(end_code, prob_hit_log_ / (dist * dist),
// end_color.getColor()));
occupied_hits.push_back(
std::make_tuple(end_code, prob_hit_log_, end_color.getColor()));
}
} else {
Point3 direction = Base::toCoord(Base::toKey(end, depth)) - sensor_origin;
dist_sqrt = direction.squaredNorm();
if (0 <= max_range && dist_sqrt > squared_max_range) {
direction /= std::sqrt(dist_sqrt);
end = sensor_origin + (direction * max_range);
}
}
Point3 current = sensor_origin;
// Move origin and end inside map
if (!Base::moveLineInside(current, end)) {
// Line outside of map
continue;
}
Key end_key = Base::toKey(end, depth);
if (0 < depth && !indices_.insert(Base::toCode(end_key)).second) {
continue;
}
Point3 end_coord = Base::toCoord(end_key);
discretized.push_back(end_coord);
// Min/max change detection
Point3 current_center = Base::toCoord(Base::toKey(current, depth));
Point3 end_center = end_coord;
double temp = Base::getNodeHalfSize(depth);
for (int i : {0, 1, 2}) {
min_change[i] = std::min(
min_change[i], std::min(end_center[i] - temp, current_center[i] - temp));
max_change[i] = std::max(
max_change[i], std::max(end_center[i] + temp, current_center[i] + temp));
}
}
LogitType prob_miss_log = prob_miss_log_ / double((2.0 * depth) + 1);
indices_.clear();
Base::insertPointCloudWait();
if (async) {
integrate_ =
std::async(std::launch::async, &OccupancyMapColor::insertPointCloudHelper,
this, sensor_origin, std::move(discretized),
std::move(occupied_hits), prob_miss_log, depth, simple_ray_casting,
early_stopping, min_change, max_change);
} else {
insertPointCloudHelper(sensor_origin, std::move(discretized),
std::move(occupied_hits), prob_miss_log, depth,
simple_ray_casting, early_stopping, min_change,
max_change);
}
}
}
void updateValue(Code const& code, LogitType const& update, Color color)
{
auto path = Base::createNode(code);
DepthType depth = code.getDepth();
if (Base::isLeaf(path[depth], depth)) {
updateNodeColor(*path[depth], color, toProb(update));
if (updateOccupancy(path[depth]->value.occupancy, update)) {
if (change_detection_enabled_) {
changes_.insert(code);
}
}
} else {
// TODO: Error
}
Base::updateParents(path, depth);
}
template <typename T>
void InsertPointCloudDiscrete(Point3 const& sensor_origin, PointCloudColor cloud,
math::Pose6 const& frame_origin, double max_range = -1,
DepthType depth = 0, bool simple_ray_casting = false,
unsigned int early_stopping = 0, bool async = false)
{
cloud.transform(frame_origin, async);
insertPointCloudDiscrete(sensor_origin, cloud, max_range, depth, simple_ray_casting,
early_stopping, async);
}
//
// Set color
//
void setColor(Code const& code, Color color);
void setColor(Point3 const& coord, Color color, DepthType depth = 0)
{
setColor(Base::toCode(coord, depth), color);
}
void setColor(double x, double y, double z, Color color, DepthType depth = 0)
{
setColor(Base::toCode(x, y, z, depth), color);
}
//
// Get color
//
Color getColor(Code const& code) const;
Color getColor(Point3 const& coord, DepthType depth = 0) const
{
return getColor(Base::toCode(coord, depth));
}
Color getColor(double x, double y, double z, DepthType depth = 0) const
{
return getColor(Base::toCode(x, y, z, depth));
}
protected:
//
// Integrate colors
//
void integrateColors(Point3 const& sensor_origin, PointCloudColor const& cloud,
double max_range = -1);
//
// Integrator helper
//
void insertPointCloudHelper(Point3 sensor_origin, PointCloud&& discretized,
std::vector<std::tuple<Code, float, Color>>&& occupied_hits,
LogitType prob_miss_log, DepthType depth,
bool simple_ray_casting, unsigned int early_stopping,
Point3 min_change, Point3 max_change)
{
std::future<void> f = std::async(std::launch::async, [this, &occupied_hits]() {
std::for_each(begin(occupied_hits), end(occupied_hits), [this](auto&& hit) {
updateValue(std::get<0>(hit), std::get<1>(hit), std::get<2>(hit));
});
});
CodeMap<LogitType> free_hits;
freeSpace(sensor_origin, discretized, free_hits, prob_miss_log, depth,
simple_ray_casting, early_stopping);
f.wait();
for (auto const& [code, value] : free_hits) {
Base::updateValue(code, value);
}
if (min_max_change_detection_enabled_) {
for (int i : {0, 1, 2}) {
min_change_[i] = std::min(min_change_[i], min_change[i]);
max_change_[i] = std::max(max_change_[i], max_change[i]);
}
}
}
//
// Update node
//
virtual bool updateNode(INNER_NODE& node, DepthType depth) override;
//
// Update node color
//
void updateNodeColor(Code code, Color update);
void updateNodeColor(LEAF_NODE& node, Color update, double prob);
//
// Average child color
//
Color getAverageChildColor(INNER_NODE const& node, DepthType depth) const;
//
// Average color
//
Color getAverageColor(std::vector<Color> const& colors) const;
};
} // namespace ufo::map
#endif // UFO_MAP_OCCUPANCY_MAP_COLOR_H
ufomap/include/ufo/map/occupancy_map_node.h
Executable
+189
View File
@@ -0,0 +1,189 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_OCCUPANCY_MAP_NODE_H
#define UFO_MAP_OCCUPANCY_MAP_NODE_H
// UFO
#include <ufo/map/code.h>
#include <ufo/map/color.h>
#include <ufo/map/octree_node.h>
namespace ufo::map
{
using Intensity = uint8_t;
template <typename T>
struct OccupancyNode {
T occupancy = 0;
OccupancyNode() {}
OccupancyNode(T occupancy) : occupancy(occupancy) {}
bool operator==(OccupancyNode const& rhs) const { return rhs.occupancy == occupancy; }
bool operator!=(OccupancyNode const& rhs) const { return rhs.occupancy != occupancy; }
/**
* @brief Write the data from this node to stream s
*
* @param s The stream to write the data to
* @return std::ostream&
*/
std::ostream& writeData(std::ostream& s) const
{
s.write(reinterpret_cast<char const*>(&occupancy), sizeof(occupancy));
return s;
}
/**
* @brief Read the data for this node from stream s
*
* @param s The stream to read the data from
* @return std::istream&
*/
std::istream& readData(std::istream& s)
{
s.read(reinterpret_cast<char*>(&occupancy), sizeof(occupancy));
return s;
}
};
struct ColorNode {
Color color;
ColorNode() {}
ColorNode(Color const& color) : color(color) {}
bool operator==(ColorNode const& rhs) const { return rhs.color == color; }
bool operator!=(ColorNode const& rhs) const { return rhs.color != color; }
/**
* @brief Write the data from this node to stream s
*
* @param s The stream to write the data to
* @return std::ostream&
*/
std::ostream& writeData(std::ostream& s) const
{
s.write(reinterpret_cast<char const*>(&color), sizeof(color));
return s;
}
/**
* @brief Read the data for this node from stream s
*
* @param s The stream to read the data from
* @return std::istream&
*/
std::istream& readData(std::istream& s)
{
s.read(reinterpret_cast<char*>(&color), sizeof(color));
return s;
}
};
template <typename T>
struct ColorOccupancyNode : OccupancyNode<T>, ColorNode {
ColorOccupancyNode() {}
ColorOccupancyNode(T occupancy, Color const& color = Color())
: OccupancyNode<T>(occupancy), ColorNode(color)
{
}
bool operator==(ColorOccupancyNode const& rhs) const
{
return OccupancyNode<T>::operator==(rhs) && ColorNode::operator==(rhs);
}
bool operator!=(ColorOccupancyNode const& rhs) const
{
return OccupancyNode<T>::operator!=(rhs) || ColorNode::operator!=(rhs);
}
/**
* @brief Write the data from this node to stream s
*
* @param s The stream to write the data to
* @return std::ostream&
*/
std::ostream& writeData(std::ostream& s) const
{
return ColorNode::writeData(OccupancyNode<T>::writeData(s));
}
/**
* @brief Read the data for this node from stream s
*
* @param s The stream to read the data from
* @return std::istream&
*/
std::istream& readData(std::istream& s)
{
return ColorNode::readData(OccupancyNode<T>::readData(s));
}
};
template <typename T>
using OccupancyMapLeafNode = OctreeLeafNode<T>;
template <typename T>
struct OccupancyMapInnerNodeBase : OccupancyMapLeafNode<T> {
// Indicates whether this node or any of its children contains free space
bool contains_free;
// Indicates whether this node or any of its children contains unknown space
bool contains_unknown;
};
template <typename T>
using OccupancyMapInnerNode = OctreeInnerNodeBase<OccupancyMapInnerNodeBase<T>>;
template <typename T>
struct Node {
OccupancyMapLeafNode<T> const* node;
Code code;
};
} // namespace ufo::map
#endif // UFO_MAP_OCCUPANCY_MAP_NODE_H
ufomap/include/ufo/map/octree.h
Executable
+1518
View File
File diff suppressed because it is too large Load Diff
ufomap/include/ufo/map/octree_node.h
Executable
+114
View File
@@ -0,0 +1,114 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_OCTREE_NODE_H
#define UFO_MAP_OCTREE_NODE_H
// STD
#include <iostream>
namespace ufo::map
{
// Have DEPTH in case it is of type OctreeInnerNode
template <typename T>
struct OctreeLeafNode {
T value;
/**
* @brief Write the data from this node to stream s
*
* @param s The stream to write the data to
* @return std::ostream&
*/
std::ostream& writeData(std::ostream& s) const
{
return value.writeData(s);
}
/**
* @brief Read the data for this node from stream s
*
* @param s The stream to read the data from
* @return std::istream&
*/
std::istream& readData(std::istream& s)
{
return value.readData(s);
}
};
template <typename T>
struct OctreeInnerNodeBase : T {
// Note: Important that the bools are before the children because of alignment
// Indicates whether this is a leaf node (has no children) or not. If true then the
// children are not valid and should not be accessed
bool is_leaf = true;
// Pointer to children
void* children = nullptr;
// void setChildExists(std::size_t index, bool value)
// {
// // Source:
// //
// https://stackoverflow.com/questions/47981/how-do-you-set-clear-and-toggle-a-single-bit
// if (value) {
// child_exists |= 1U << index;
// } else {
// child_exists &= ~(1U << index);
// }
// }
// bool childExists(std::size_t index) const
// {
// // Source:
// //
// https://stackoverflow.com/questions/523724/c-c-check-if-one-bit-is-set-in-i-e-int-variable
// return child_exist & (1U << index);
// }
};
template <typename T>
using OctreeInnerNode = OctreeInnerNodeBase<OctreeLeafNode<T>>;
} // namespace ufo::map
#endif // UFO_MAP_OCTREE_NODE_H
ufomap/include/ufo/map/point_cloud.h
Executable
+282
View File
@@ -0,0 +1,282 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_POINT_CLOUD_H
#define UFO_MAP_POINT_CLOUD_H
// UFO
#include <ufo/map/types.h>
#include <ufo/math/pose6.h>
// STD
#include <algorithm>
#include <execution>
#include <type_traits>
#include <vector>
namespace ufo::map
{
/**
* @brief A collection of 3D coordinates of type T
*
* @tparam T The type of points to store in the point cloud. Has to inhert from
* Point3
*/
template <typename T, typename = std::enable_if_t<std::is_base_of_v<Point3, T>>>
class PointCloudT
{
public:
PointCloudT() {}
PointCloudT(const PointCloudT& other)
{
cloud_.reserve(other.size());
push_back(other);
}
~PointCloudT() {}
/**
* @brief Access specified point
*
* @param index Index of the point to return
* @return const T& Reference to the requested point
*/
const T& operator[](size_t index) const { return cloud_[index]; }
/**
* @brief Access specified point
*
* @param index Index of the point to return
* @return const T& Reference to the requested point
*/
T& operator[](size_t index) { return cloud_[index]; }
/**
* @brief Erases all points from the point cloud
*
*/
void clear() { cloud_.clear(); }
/**
* @brief Increase the capacity of the point cloud to a value that is greater
* or equal to new_cap
*
* @param new_cap New capacity of the point cloud
*/
void reserve(size_t new_cap) { cloud_.reserve(new_cap); }
/**
* @brief Resizes the container to contain count elements.
* If the current size is greater than count, the container is reduced to its first
* count elements. If the current size is less than count, 1) additional
* default-inserted elements are appended 2) additional copies of value are appended.
*
* @param count New size of the point cloud
*/
constexpr void resize(size_t count) { cloud_.resize(count); }
/**
* @brief Resizes the container to contain count elements.
* If the current size is greater than count, the container is reduced to its first
* count elements. If the current size is less than count, 1) additional
* default-inserted elements are appended 2) additional copies of value are appended.
*
* @param count New size of the point cloud
* @param value The value to initialize the new elements with
*/
constexpr void resize(size_t count, T const& value) { cloud_.resize(count, value); }
/**
* @brief Return the number of points in the point cloud
*
* @return size_t The number of points in the point cloud
*/
size_t size() const { return cloud_.size(); }
/**
* @brief Adds a point to the point cloud
*
* @param point The point to add to the point cloud
*/
void push_back(const T& point) { cloud_.push_back(point); }
/**
* @brief Adds all points from another point cloud to this point cloud
*
* @param other The other point cloud that points should be added from
*/
void push_back(const PointCloudT& other)
{
cloud_.insert(cloud_.end(), other.begin(), other.end());
}
/**
* @brief Transform each point in the point cloud
*
* @param transform The transformation to be applied to each point
*/
void transform(const ufo::math::Pose6& transform, bool parallel = false)
{
if (parallel) {
std::for_each(std::execution::par, cloud_.begin(), cloud_.end(),
[&transform](auto&& point) { point = transform.transform(point); });
} else {
std::for_each(std::execution::seq, cloud_.begin(), cloud_.end(),
[&transform](auto&& point) { point = transform.transform(point); });
}
}
/**
* @brief Returns an iterator to the first point of the point cloud
*
* @return std::vector<T>::iterator Iterator to the first point
*/
typename std::vector<T>::iterator begin() { return cloud_.begin(); }
/**
* @brief Returns an iterator to the last element following the last point of
* the point cloud
*
* @return std::vector<T>::iterator Iterator to the element following the last
* point
*/
typename std::vector<T>::iterator end() { return cloud_.end(); }
/**
* @brief Returns an iterator to the first point of the point cloud
*
* @return std::vector<T>::const_iterator Iterator to the first point
*/
typename std::vector<T>::const_iterator begin() const { return cloud_.begin(); }
/**
* @brief Returns an iterator to the last element following the last point of
* the point cloud
*
* @return std::vector<T>::const_iterator Iterator to the element following
* the last point
*/
typename std::vector<T>::const_iterator end() const { return cloud_.end(); }
/**
* @brief Returns an iterator to the first point of the point cloud
*
* @return std::vector<T>::const_iterator Iterator to the first point
*/
typename std::vector<T>::const_iterator cbegin() const { return cloud_.cbegin(); }
/**
* @brief Returns an iterator to the last element following the last point of
* the point cloud
*
* @return std::vector<T>::const_iterator Iterator to the element following
* the last point
*/
typename std::vector<T>::const_iterator cend() const { return cloud_.cend(); }
/**
* @brief Returns a reverse iterator to the first point of the point cloud
*
* @return std::vector<T>::reverse_iterator Reverse iterator to the first
* point
*/
typename std::vector<T>::reverse_iterator rbegin() { return cloud_.rbegin(); }
/**
* @brief Returns a reverse iterator to the element following the last point
* of the reversed point cloud
*
* @return std::vector<T>::reverse_iterator Reverse iterator to the element
* following the last point
*/
typename std::vector<T>::reverse_iterator rend() { return cloud_.rend(); }
/**
* @brief Returns a reverse iterator to the first point of the point cloud
*
* @return std::vector<T>::const_reverse_iterator Reverse iterator to the
* first point
*/
typename std::vector<T>::const_reverse_iterator rbegin() const
{
return cloud_.rbegin();
}
/**
* @brief Returns a reverse iterator to the element following the last point
* of the reversed point cloud
*
* @return std::vector<T>::const_reverse_iterator Reverse iterator to the
* element following the last point
*/
typename std::vector<T>::const_reverse_iterator rend() const { return cloud_.rend(); }
/**
* @brief Returns a reverse iterator to the first point of the point cloud
*
* @return std::vector<T>::const_reverse_iterator Reverse iterator to the
* first point
*/
typename std::vector<T>::const_reverse_iterator crbegin() const
{
return cloud_.rbegin();
}
/**
* @brief Returns a reverse iterator to the element following the last point
* of the reversed point cloud
*
* @return std::vector<T>::const_reverse_iterator Reverse iterator to the
* element following the last point
*/
typename std::vector<T>::const_reverse_iterator crend() const { return cloud_.rend(); }
private:
std::vector<T> cloud_; // The point cloud
};
using PointCloud = PointCloudT<Point3>;
using PointCloudColor = PointCloudT<Point3Color>;
} // namespace ufo::map
#endif // UFO_MAP_POINT_CLOUD_H
ufomap/include/ufo/map/types.h
Executable
+105
View File
@@ -0,0 +1,105 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_TYPES_H
#define UFO_MAP_TYPES_H
// UFO
#include <ufo/map/color.h>
#include <ufo/math/vector3.h>
// STD
#include <cstdint>
namespace ufo::map
{
using CodeType = uint64_t;
using KeyType = unsigned int;
using DepthType = unsigned int;
using Point3 = ufo::math::Vector3;
class Point3Color : public Point3
{
public:
Point3Color() {}
Point3Color(Point3 const& point, Color const& color) : Point3(point), color_(color) {}
Point3Color(double x, double y, double z, uint8_t r, uint8_t g, uint8_t b)
: Point3(x, y, z), color_(r, g, b)
{
}
Point3Color(Point3 const& point) : Point3(point) {}
Point3Color(double x, double y, double z) : Point3(x, y, z) {}
Point3Color(uint8_t r, uint8_t g, uint8_t b) : color_(r, g, b) {}
Point3Color(Color const& color) : color_(color) {}
Point3Color& operator=(Point3Color const& rhs)
{
Point3::operator=(rhs);
color_ = rhs.color_;
return *this;
}
Color const& getColor() const { return color_; }
Color& getColor() { return color_; }
void setColor(Color const& new_color) { color_ = new_color; }
void setColor(uint8_t r, uint8_t g, uint8_t b)
{
color_.r = r;
color_.g = g;
color_.b = b;
}
protected:
Color color_;
};
} // namespace ufo::map
#endif // UFO_MAP_TYPES_H
ufomap/include/ufo/map/ufomap.h
Executable
+50
View File
@@ -0,0 +1,50 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_UFO_MAP_H
#define UFO_MAP_UFO_MAP_H
#include <ufo/map/occupancy_map.h>
#include <ufo/map/occupancy_map_color.h>
#include <ufo/map/point_cloud.h>
#include <ufo/map/types.h>
#endif // UFO_MAP_UFO_MAP_H
ufomap/include/ufo/math/pose6.h
Executable
+175
View File
@@ -0,0 +1,175 @@
/**
* UFOMath - the math library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomath
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MATH_POSE6_H
#define UFO_MATH_POSE6_H
// UFO
#include <ufo/math/quaternion.h>
#include <ufo/math/vector3.h>
// STD
#include <cmath>
#include <type_traits>
namespace ufo::math
{
class Pose6
{
public:
Pose6() {}
~Pose6() {}
Pose6(Pose6 const& other) : translation_(other.translation_), rotation_(other.rotation_)
{
}
Pose6(Vector3 const& translation, Quaternion const& rotation)
: translation_(translation), rotation_(rotation)
{
}
Pose6(double x, double y, double z, double roll, double pitch, double yaw)
: translation_(x, y, z), rotation_(roll, pitch, yaw)
{
}
Pose6(double t_x, double t_y, double t_z, double r_w, double r_x, double r_y,
double r_z)
: translation_(t_x, t_y, t_z), rotation_(r_w, r_x, r_y, r_z)
{
}
Pose6& operator=(Pose6 const& rhs)
{
translation_ = rhs.translation_;
rotation_ = rhs.rotation_;
return *this;
}
bool operator==(Pose6 const& rhs)
{
return translation_ == rhs.translation_ && rotation_ == rhs.rotation_;
}
bool operator!=(Pose6 const& rhs)
{
return translation_ != rhs.translation_ || rotation_ != rhs.rotation_;
}
Vector3& translation() { return translation_; }
Vector3 translation() const { return translation_; }
Quaternion& rotation() { return rotation_; }
Quaternion rotation() const { return rotation_; }
double& x() { return translation_[0]; }
double x() const { return translation_[0]; }
double& y() { return translation_[1]; }
double y() const { return translation_[1]; }
double& z() { return translation_[2]; }
double z() const { return translation_[2]; }
double roll() const { return rotation_.toEuler()[0]; }
double pitch() const { return rotation_.toEuler()[1]; }
double yaw() const { return rotation_.toEuler()[2]; }
template <typename T, typename = std::enable_if_t<std::is_base_of_v<Vector3, T>>>
T transform(T const& v) const
{
// TODO: Improve
T new_v = v;
Vector3 result = rotation_.rotate(v);
new_v.x() = result.x();
new_v.y() = result.y();
new_v.z() = result.z();
new_v += translation_;
return new_v;
}
Pose6 inversed() const
{
Pose6 result(*this);
result.rotation_ = result.rotation_.inversed().normalized();
result.translation_ = result.rotation_.rotate(-translation_);
return result;
}
Pose6& inverse()
{
rotation_ = rotation_.inversed().normalized();
translation_ = rotation_.rotate(-translation_);
return *this;
}
Pose6 operator*(Pose6 const& other) const
{
Quaternion rotation_new = rotation_ * other.rotation_;
Vector3 transation_new = rotation_.rotate(other.translation_) + translation_;
return Pose6(transation_new, rotation_new.normalized());
}
Pose6& operator*=(Pose6 const& other)
{
translation_ += rotation_.rotate(other.translation_);
rotation_ = rotation_ * other.rotation_;
return *this;
}
double distance(Pose6 const& other) const
{
double dist_x = x() - other.x();
double dist_y = y() - other.y();
double dist_z = z() - other.z();
return sqrt((dist_x * dist_x) + (dist_y * dist_y) + (dist_z * dist_z));
}
double translationLength() const
{
return sqrt((x() * x()) + (y() * y()) + (z() * z()));
}
private:
Vector3 translation_;
Quaternion rotation_;
};
} // namespace ufo::math
#endif // UFO_MATH_POSE6_H
ufomap/include/ufo/math/quaternion.h
Executable
+302
View File
@@ -0,0 +1,302 @@
/**
* UFOMath - the math library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomath
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MATH_QUATERNION_H
#define UFO_MATH_QUATERNION_H
// UFO
#include <ufo/math/vector3.h>
// STD
#include <array>
#include <cmath>
#include <type_traits>
#include <vector>
namespace ufo::math
{
class Quaternion
{
public:
Quaternion() : data_{1, 0, 0, 0} {}
Quaternion(Quaternion const& other) : data_(other.data_) {}
Quaternion(double w, double x, double y, double z) : data_{w, x, y, z} {}
Quaternion(Vector3 const& other) : Quaternion(other.roll(), other.pitch(), other.yaw())
{
}
Quaternion(double roll, double pitch, double yaw)
{
double sroll = sin(roll);
double spitch = sin(pitch);
double syaw = sin(yaw);
double croll = cos(roll);
double cpitch = cos(pitch);
double cyaw = cos(yaw);
double m[3][3] = {// create rotational Matrix
{cyaw * cpitch, cyaw * spitch * sroll - syaw * croll,
cyaw * spitch * croll + syaw * sroll},
{syaw * cpitch, syaw * spitch * sroll + cyaw * croll,
syaw * spitch * croll - cyaw * sroll},
{-spitch, cpitch * sroll, cpitch * croll}};
double _w = (double)(sqrt(std::max(0.0, 1 + m[0][0] + m[1][1] + m[2][2])) / 2.0);
double _x = (double)(sqrt(std::max(0.0, 1 + m[0][0] - m[1][1] - m[2][2])) / 2.0);
double _y = (double)(sqrt(std::max(0.0, 1 - m[0][0] + m[1][1] - m[2][2])) / 2.0);
double _z = (double)(sqrt(std::max(0.0, 1 - m[0][0] - m[1][1] + m[2][2])) / 2.0);
w() = _w;
x() = (m[2][1] - m[1][2]) >= 0 ? fabs(_x) : -fabs(_x);
y() = (m[0][2] - m[2][0]) >= 0 ? fabs(_y) : -fabs(_y);
z() = (m[1][0] - m[0][1]) >= 0 ? fabs(_z) : -fabs(_z);
}
Quaternion(Vector3 const& axis, double angle)
{
double sa = sin(angle / 2);
double ca = cos(angle / 2);
x() = (double)(axis.x() * sa);
y() = (double)(axis.y() * sa);
z() = (double)(axis.z() * sa);
w() = (double)ca;
}
Vector3 toEuler() const
{ // create rotational matrix
double n = norm();
double s = n > 0 ? 2.0 / (n * n) : 0.0;
double xs = x() * s;
double ys = y() * s;
double zs = z() * s;
double wx = w() * xs;
double wy = w() * ys;
double wz = w() * zs;
double xx = x() * xs;
double xy = x() * ys;
double xz = x() * zs;
double yy = y() * ys;
double yz = y() * zs;
double zz = z() * zs;
double m[3][3];
m[0][0] = 1.0 - (yy + zz);
m[1][1] = 1.0 - (xx + zz);
m[2][2] = 1.0 - (xx + yy);
m[1][0] = xy + wz;
m[0][1] = xy - wz;
m[2][0] = xz - wy;
m[0][2] = xz + wy;
m[2][1] = yz + wx;
m[1][2] = yz - wx;
double roll = (double)atan2(m[2][1], m[2][2]);
double pitch = (double)atan2(-m[2][0], sqrt(m[2][1] * m[2][1] + m[2][2] * m[2][2]));
double yaw = (double)atan2(m[1][0], m[0][0]);
return Vector3(roll, pitch, yaw);
}
void toRotMatrix(std::vector<double>& rot_matrix_3_3) const
{ // create rotational matrix
double n = norm();
double s = n > 0 ? 2.0 / (n * n) : 0.0;
double xs = x() * s;
double ys = y() * s;
double zs = z() * s;
double wx = w() * xs;
double wy = w() * ys;
double wz = w() * zs;
double xx = x() * xs;
double xy = x() * ys;
double xz = x() * zs;
double yy = y() * ys;
double yz = y() * zs;
double zz = z() * zs;
double m[3][3];
m[0][0] = 1.0 - (yy + zz);
m[1][1] = 1.0 - (xx + zz);
m[2][2] = 1.0 - (xx + yy);
m[1][0] = xy + wz;
m[0][1] = xy - wz;
m[2][0] = xz - wy;
m[0][2] = xz + wy;
m[2][1] = yz + wx;
m[1][2] = yz - wx;
rot_matrix_3_3.clear();
rot_matrix_3_3.resize(9, 0.0);
for (unsigned int i = 0; i < 3; i++) {
rot_matrix_3_3[i * 3] = m[i][0];
rot_matrix_3_3[i * 3 + 1] = m[i][1];
rot_matrix_3_3[i * 3 + 2] = m[i][2];
}
}
double const& operator[](size_t index) const { return data_[index]; }
double& operator[](size_t index) { return data_[index]; }
double norm() const
{
double n = 0;
for (unsigned int i = 0; i < 4; i++) {
n += operator[](i) * operator[](i);
}
return (double)sqrt(n);
}
Quaternion normalized() const
{
Quaternion result(*this);
result.normalize();
return result;
}
Quaternion& normalize()
{
double len = norm();
if (len > 0) *this /= (double)len;
return *this;
}
void operator/=(double x)
{
for (unsigned int i = 0; i < 4; ++i) {
operator[](i) /= x;
}
}
Quaternion& operator=(Quaternion const& rhs)
{
w() = rhs.w();
x() = rhs.x();
y() = rhs.y();
z() = rhs.z();
return *this;
}
bool operator==(Quaternion const& rhs) const
{
for (unsigned int i = 0; i < 4; i++) {
if (operator[](i) != rhs[i]) {
return false;
}
}
return true;
}
bool operator!=(Quaternion const& rhs) const
{
for (unsigned int i = 0; i < 4; i++) {
if (operator[](i) != rhs[i]) {
return true;
}
}
return false;
}
Quaternion operator*(Quaternion const& rhs) const
{
return Quaternion(w() * rhs.w() - x() * rhs.x() - y() * rhs.y() - z() * rhs.z(),
y() * rhs.z() - rhs.y() * z() + w() * rhs.x() + rhs.w() * x(),
z() * rhs.x() - rhs.z() * x() + w() * rhs.y() + rhs.w() * y(),
x() * rhs.y() - rhs.x() * y() + w() * rhs.z() + rhs.w() * z());
}
Quaternion operator*(Vector3 const& v) const
{
return *this * Quaternion(0, v(0), v(1), v(2));
}
Quaternion inversed() const { return Quaternion(w(), -x(), -y(), -z()); }
Quaternion& inverse()
{
x() = -x();
y() = -y();
z() = -z();
return *this;
}
template <typename T, typename = std::enable_if_t<std::is_base_of_v<Vector3, T>>>
T rotate(T const& v) const
{
// TODO: Improve
T new_v = v;
Quaternion q = *this * v * this->inversed();
new_v.x() = q.x();
new_v.y() = q.y();
new_v.z() = q.z();
return new_v;
}
double const& w() const { return data_[0]; }
double& w() { return data_[0]; }
double const& x() const { return data_[1]; }
double& x() { return data_[1]; }
double const& y() const { return data_[2]; }
double& y() { return data_[2]; }
double const& z() const { return data_[3]; }
double& z() { return data_[3]; }
private:
std::array<double, 4> data_;
};
} // namespace ufo::math
#endif // UFO_MATH_QUATERNION_H
ufomap/include/ufo/math/vector3.h
Executable
+331
View File
@@ -0,0 +1,331 @@
/**
* UFOMath - the math library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomath
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MATH_VECTOR3_H
#define UFO_MATH_VECTOR3_H
#include <stddef.h>
#include <algorithm>
#include <cmath>
namespace ufo::math
{
class Vector3
{
public:
Vector3() : data_{0.0, 0.0, 0.0} {}
Vector3(double x, double y, double z) : data_{x, y, z} {}
Vector3(Vector3 const& other) : data_{other.data_[0], other.data_[1], other.data_[2]} {}
Vector3& operator=(Vector3 const& other)
{
data_[0] = other.data_[0];
data_[1] = other.data_[1];
data_[2] = other.data_[2];
return *this;
}
Vector3 cross(Vector3 const& other) const { return cross(*this, other); }
static Vector3 cross(Vector3 const& first, Vector3 const& second)
{
return Vector3(
(first.data_[1] * second.data_[2]) - (first.data_[2] * second.data_[1]),
(first.data_[2] * second.data_[0]) - (first.data_[0] * second.data_[2]),
(first.data_[0] * second.data_[1]) - (first.data_[1] * second.data_[0]));
}
double dot(Vector3 const& other) const { return dot(*this, other); }
static double dot(Vector3 const& first, Vector3 const& second)
{
return (first.data_[0] * second.data_[0]) + (first.data_[1] * second.data_[1]) +
(first.data_[2] * second.data_[2]);
}
double& operator()(size_t idx) { return data_[idx]; }
double const& operator()(size_t idx) const { return data_[idx]; }
double& operator[](size_t idx) { return data_[idx]; }
double const& operator[](size_t idx) const { return data_[idx]; }
double& x() { return data_[0]; }
double const& x() const { return data_[0]; }
double& y() { return data_[1]; }
double const& y() const { return data_[1]; }
double& z() { return data_[2]; }
double const& z() const { return data_[2]; }
double& roll() { return data_[0]; }
double const& roll() const { return data_[0]; }
double& pitch() { return data_[1]; }
double const& pitch() const { return data_[1]; }
double& yaw() { return data_[2]; }
double const& yaw() const { return data_[2]; }
Vector3 operator-() const { return Vector3(-data_[0], -data_[1], -data_[2]); }
Vector3 operator-(Vector3 const& other) const
{
return Vector3(data_[0] - other.data_[0], data_[1] - other.data_[1],
data_[2] - other.data_[2]);
}
Vector3 operator-(double value) const
{
return Vector3(data_[0] - value, data_[1] - value, data_[2] - value);
}
Vector3 operator+(Vector3 const& other) const
{
return Vector3(data_[0] + other.data_[0], data_[1] + other.data_[1],
data_[2] + other.data_[2]);
}
Vector3 operator+(double value) const
{
return Vector3(data_[0] + value, data_[1] + value, data_[2] + value);
}
Vector3 operator*(Vector3 const& other) const
{
return Vector3(data_[0] * other.data_[0], data_[1] * other.data_[1],
data_[2] * other.data_[2]);
}
Vector3 operator*(double value) const
{
return Vector3(data_[0] * value, data_[1] * value, data_[2] * value);
}
Vector3 operator/(Vector3 const& other) const
{
return Vector3(data_[0] / other.data_[0], data_[1] / other.data_[1],
data_[2] / other.data_[2]);
}
Vector3 operator/(double value) const
{
return Vector3(data_[0] / value, data_[1] / value, data_[2] / value);
}
void operator-=(Vector3 const& other)
{
data_[0] -= other.data_[0];
data_[1] -= other.data_[1];
data_[2] -= other.data_[2];
}
void operator+=(Vector3 const& other)
{
data_[0] += other.data_[0];
data_[1] += other.data_[1];
data_[2] += other.data_[2];
}
void operator*=(Vector3 const& other)
{
data_[0] *= other.data_[0];
data_[1] *= other.data_[1];
data_[2] *= other.data_[2];
}
void operator/=(Vector3 const& other)
{
data_[0] /= other.data_[0];
data_[1] /= other.data_[1];
data_[2] /= other.data_[2];
}
void operator-=(double value)
{
data_[0] -= value;
data_[1] -= value;
data_[2] -= value;
}
void operator+=(double value)
{
data_[0] += value;
data_[1] += value;
data_[2] += value;
}
void operator*=(double value)
{
data_[0] *= value;
data_[1] *= value;
data_[2] *= value;
}
void operator/=(double value)
{
data_[0] /= value;
data_[1] /= value;
data_[2] /= value;
}
bool operator==(Vector3 const& other) const
{
return data_[0] == other.data_[0] && data_[1] == other.data_[1] &&
data_[2] == other.data_[2];
}
bool operator!=(Vector3 const& other) const
{
return data_[0] != other.data_[0] || data_[1] != other.data_[1] ||
data_[2] != other.data_[2];
}
double norm() const { return std::sqrt(squaredNorm()); }
double squaredNorm() const
{
return (data_[0] * data_[0]) + (data_[1] * data_[1]) + (data_[2] * data_[2]);
}
Vector3& normalize()
{
*this /= norm();
return *this;
}
Vector3 normalized() const
{
Vector3 temp(*this);
return temp.normalize();
}
double angleTo(Vector3 const& other) const
{
return std::acos(dot(other) / (norm() * other.norm()));
}
double distance(Vector3 const& other) const
{
double x = data_[0] - other.data_[0];
double y = data_[1] - other.data_[1];
double z = data_[2] - other.data_[2];
return sqrt((x * x) + (y * y) + (z * z));
}
double distanceXY(Vector3 const& other) const
{
double x = data_[0] - other.data_[0];
double y = data_[1] - other.data_[1];
return sqrt((x * x) + (y * y));
}
size_t size() const { return 3; }
double min() const { return std::min(std::min(data_[0], data_[1]), data_[2]); }
double max() const { return std::max(std::max(data_[0], data_[1]), data_[2]); }
size_t minElementIndex() const
{
if (data_[0] <= data_[1]) {
return data_[0] <= data_[2] ? 0 : 2;
} else {
return data_[1] <= data_[2] ? 1 : 2;
}
}
size_t maxElementIndex() const
{
if (data_[0] >= data_[1]) {
return data_[0] >= data_[2] ? 0 : 2;
} else {
return data_[1] >= data_[2] ? 1 : 2;
}
}
Vector3& ceil()
{
for (int i = 0; i < 3; ++i) {
data_[i] = std::ceil(data_[i]);
}
return *this;
}
Vector3 ceil() const
{
return Vector3(std::ceil(data_[0]), std::ceil(data_[1]), std::ceil(data_[2]));
}
Vector3& floor()
{
for (int i = 0; i < 3; ++i) {
data_[i] = std::floor(data_[i]);
}
return *this;
}
Vector3 floor() const
{
return Vector3(std::floor(data_[0]), std::floor(data_[1]), std::floor(data_[2]));
}
Vector3& trunc()
{
for (int i = 0; i < 3; ++i) {
data_[i] = std::trunc(data_[i]);
}
return *this;
}
Vector3 trunc() const
{
return Vector3(std::trunc(data_[0]), std::trunc(data_[1]), std::trunc(data_[2]));
}
Vector3& round()
{
for (int i = 0; i < 3; ++i) {
data_[i] = std::round(data_[i]);
}
return *this;
}
Vector3 round() const
{
return Vector3(std::round(data_[0]), std::round(data_[1]), std::round(data_[2]));
}
Vector3& clamp(Vector3 const& min, Vector3 const& max)
{
for (int i = 0; i < 3; ++i) {
data_[i] = std::clamp(data_[i], min[i], max[i]);
}
return *this;
}
Vector3 clamp(Vector3 const& min, Vector3 const& max) const
{
return clamp(*this, min, max);
}
static Vector3 clamp(Vector3 const& value, Vector3 const& min, Vector3 const& max)
{
return Vector3(std::clamp(value[0], min[0], max[0]),
std::clamp(value[1], min[1], max[1]),
std::clamp(value[2], min[2], max[2]));
}
protected:
double data_[3];
};
} // namespace ufo::math
#endif // UFO_MATH_VECTOR3_H
ufomap/package.xml
Executable
+23
View File
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="2">
<name>ufomap</name>
<version>1.0.0</version>
<description>The ufomap package</description>
<author email="dduberg@kth.se">Daniel Duberg</author>
<maintainer email="dduberg@kth.se">Daniel Duberg</maintainer>
<license>BSD</license>
<url type="website">https://github.com/danielduberg/ufomap</url>
<buildtool_depend>cmake</buildtool_depend>
<depend>lz4</depend>
<depend>tbb</depend>
<doc_depend>doxygen</doc_depend>
<export>
<build_type>cmake</build_type>
</export>
</package>
ufomap/src/geometry/bounding_volume.cpp
Executable
+67
View File
@@ -0,0 +1,67 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ufo/geometry/bounding_volume.h>
namespace ufo::geometry
{
bool BoundingVolume::intersects(BoundingVar const& bv) const
{
for (BoundingVar const& bv_2 : bounding_volume_) {
if (std::visit([](auto&& arg_1, auto&& arg_2)
-> bool { return geometry::intersects(arg_1, arg_2); },
bv, bv_2)) {
return true;
}
}
return false;
}
bool BoundingVolume::intersects(BoundingVolume const& other) const
{
for (BoundingVar const& other_bv : other.bounding_volume_) {
if (intersects(other_bv)) {
return true;
}
}
return false;
}
} // namespace ufo::geometry
ufomap/src/geometry/collision_checks.cpp
Executable
+825
View File
@@ -0,0 +1,825 @@
/**
* UFOGeometry - the geometry library used in UFO
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufogeometry
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ufo/geometry/collision_checks.h>
#include <exception>
#include <limits>
namespace ufo::geometry
{
/////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////// Help functions
////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
bool intersectsLine(const AABB& aabb, const Ray& ray, double t_near, double t_far)
{
Point min = aabb.getMin();
Point max = aabb.getMax();
for (int i = 0; i < 3; ++i) {
if (0 != ray.direction[i]) {
double reciprocal_direction = 1.0 / ray.direction[i];
double t1 = (min[i] - ray.origin[i]) * reciprocal_direction;
double t2 = (max[i] - ray.origin[i]) * reciprocal_direction;
if (t1 < t2) {
t_near = std::max(t1, t_near);
t_far = std::min(t2, t_far);
} else {
t_near = std::max(t2, t_near);
t_far = std::min(t1, t_far);
}
if (t_near > t_far) {
return false;
}
} else if (min[i] > ray.origin[i] || max[i] < ray.origin[i]) {
return false;
}
}
return true;
}
Point closestPoint(const Sphere& sphere, const Point& point)
{
Point sphere_to_point = point - sphere.center;
sphere_to_point.normalize();
sphere_to_point *= sphere.radius;
return sphere_to_point + sphere.center;
}
Point closestPoint(const AABB& aabb, const Point& point)
{
Point min = aabb.getMin();
Point max = aabb.getMax();
return Point::clamp(point, min, max);
}
Point closestPoint(const OBB& obb, const Point& point)
{
Point result = obb.center;
Point dir = point - obb.center;
std::vector<double> obb_rot_matrix;
obb.rotation.toRotMatrix(obb_rot_matrix);
for (int i = 0; i < 3; ++i) {
Point axis(obb_rot_matrix[i * 3], obb_rot_matrix[(i * 3) + 1],
obb_rot_matrix[(i * 3) + 2]);
double distance = Point::dot(dir, axis);
if (distance > obb.half_size[i]) {
distance = obb.half_size[i];
}
if (distance < -obb.half_size[i]) // TODO: Should this be else if?
{
distance = -obb.half_size[i];
}
result = result + (axis * distance);
}
return result;
}
Point closestPoint(const Plane& plane, const Point& point)
{
double distance = Point::dot(plane.normal, point) - plane.distance;
return point - plane.normal * distance;
}
Point closestPoint(const LineSegment& line_segement, const Point& point)
{
Point direction = line_segement.end - line_segement.start;
double t = Point::dot(point - line_segement.start, direction) /
Point::dot(direction, direction);
t = fmaxf(t, 0.0f);
t = fminf(t, 1.0f);
return line_segement.start + direction * t;
}
Point closestPoint(const Ray& ray, const Point& point)
{
double t = Point::dot(point - ray.origin, ray.direction);
t = fmaxf(t, 0.0f);
return ray.origin + ray.direction * t;
}
// Classify
double classify(const AABB& aabb, const Plane& plane)
{
double r = std::abs(aabb.half_size.x() * plane.normal.x()) +
std::abs(aabb.half_size.y() * plane.normal.y()) +
std::abs(aabb.half_size.z() * plane.normal.z());
double d = Point::dot(plane.normal, aabb.center) + plane.distance;
if (std::abs(d) < r) {
return 0.0f;
} else if (d < 0.0f) {
return d + r;
}
return d - r;
}
double classify(const OBB& obb, const Plane& plane)
{
// TODO: Implement
throw std::logic_error("Function not yet implemented");
// Point normal = plane.normal * obb.rotation;
// double r = std::abs(obb.half_size.x() * normal.x()) +
// std::abs(obb.half_size.y() * normal.y()) +
// std::abs(obb.half_size.z() * normal.z());
// double d = Point::dot(plane.normal, obb.center) + plane.distance;
// if (std::abs(d) < r) {
// return 0.0f;
// } else if (d < 0.0f) {
// return d + r;
// }
// return d - r;
}
std::pair<double, double> getInterval(const AABB& aabb, const Point& axis)
{
Point i = aabb.getMin();
Point a = aabb.getMax();
Point vertex[8] = {Point(i.x(), a.y(), a.z()), Point(i.x(), a.y(), i.z()),
Point(i.x(), i.y(), a.z()), Point(i.x(), i.y(), i.z()),
Point(a.x(), a.y(), a.z()), Point(a.x(), a.y(), i.z()),
Point(a.x(), i.y(), a.z()), Point(a.x(), i.y(), i.z())};
std::pair<double, double> result;
result.first = result.second = Point::dot(axis, vertex[0]);
for (int i = 1; i < 8; ++i) {
double projection = Point::dot(axis, vertex[i]);
result.first = std::min(result.first, projection);
result.second = std::max(result.second, projection);
}
return result;
}
std::pair<double, double> getInterval(const OBB& obb, const Point& axis)
{
Point vertex[8];
Point C = obb.center; // OBB Center
Point E = obb.half_size; // OBB Extents
std::vector<double> obb_rot_matrix;
obb.rotation.toRotMatrix(obb_rot_matrix);
Point A[] = {
// OBB Axis
Point(obb_rot_matrix[0], obb_rot_matrix[1], obb_rot_matrix[2]),
Point(obb_rot_matrix[3], obb_rot_matrix[4], obb_rot_matrix[5]),
Point(obb_rot_matrix[6], obb_rot_matrix[7], obb_rot_matrix[8]),
};
vertex[0] = C + A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
vertex[1] = C - A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
vertex[2] = C + A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
vertex[3] = C + A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
vertex[4] = C - A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
vertex[5] = C + A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
vertex[6] = C - A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
vertex[7] = C - A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
std::pair<double, double> result;
result.first = result.second = Point::dot(axis, vertex[0]);
for (int i = 1; i < 8; ++i) {
double projection = Point::dot(axis, vertex[i]);
result.first = std::min(result.first, projection);
result.second = std::max(result.second, projection);
}
return result;
}
bool overlapOnAxis(const AABB& aabb, const OBB& obb, const Point& axis)
{
auto [a_min, a_max] = getInterval(aabb, axis);
auto [b_min, b_max] = getInterval(obb, axis);
return ((b_min <= a_max) && (a_min <= b_max));
}
bool overlapOnAxis(const OBB& obb_1, const OBB& obb_2, const Point& axis)
{
auto [a_min, a_max] = getInterval(obb_1, axis);
auto [b_min, b_max] = getInterval(obb_2, axis);
return ((b_min <= a_max) && (a_min <= b_max));
}
/////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////// Intersection tests
//////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
// AABB
bool intersects(const AABB& aabb_1, const AABB& aabb_2)
{
Point min_1 = aabb_1.getMin();
Point max_1 = aabb_1.getMax();
Point min_2 = aabb_2.getMin();
Point max_2 = aabb_2.getMax();
return min_1.x() <= max_2.x() && min_1.y() <= max_2.y() && min_1.z() <= max_2.z() &&
min_2.x() <= max_1.x() && min_2.y() <= max_1.y() && min_2.z() <= max_1.z();
}
// bool intersects(const AABB& aabb, const Capsule& capsule)
// {
// }
bool intersects(const AABB& aabb, const Frustum& frustum)
{
// FIXME:
for (int i = 0; i < 6; ++i) {
double side = classify(aabb, frustum.planes[i]);
if (side < 0) {
return false;
}
}
return true;
}
bool intersects(const AABB& aabb, const LineSegment& line_segment)
{
Ray ray;
ray.origin = line_segment.start;
ray.direction = (line_segment.end - line_segment.start);
double length = ray.direction.norm();
ray.direction /= length;
return intersectsLine(aabb, ray, 0.0, length);
}
bool intersects(const AABB& aabb, const OBB& obb)
{
// ufo::geometry::OBB obb_2(aabb.center, aabb.half_size, ufo::math::Point(0, 0,
// 0)); return intersects(obb, obb_2);
std::vector<double> obb_rot_matrix;
obb.rotation.toRotMatrix(obb_rot_matrix);
Point test[15] = {Point(1, 0, 0), // AABB axis 1
Point(0, 1, 0), // AABB axis 2
Point(0, 0, 1), // AABB axis 3
Point(obb_rot_matrix[0], obb_rot_matrix[1], obb_rot_matrix[2]),
Point(obb_rot_matrix[3], obb_rot_matrix[4], obb_rot_matrix[5]),
Point(obb_rot_matrix[6], obb_rot_matrix[7], obb_rot_matrix[8])};
for (int i = 0; i < 3; ++i) { // Fill out rest of axis
test[6 + i * 3 + 0] = Point::cross(test[i], test[3]);
test[6 + i * 3 + 1] = Point::cross(test[i], test[4]);
test[6 + i * 3 + 2] = Point::cross(test[i], test[5]);
}
for (int i = 0; i < 15; ++i) {
if (!overlapOnAxis(aabb, obb, test[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool intersects(const AABB& aabb, const Plane& plane)
{
double p_len = aabb.half_size.x() * std::abs(plane.normal.x()) +
aabb.half_size.y() * std::abs(plane.normal.y()) +
aabb.half_size.z() * std::abs(plane.normal.z());
double distance = Point::dot(plane.normal, aabb.center) - plane.distance;
return std::abs(distance) <= p_len;
}
bool intersects(const AABB& aabb, const Point& point)
{
Point min = aabb.getMin();
Point max = aabb.getMax();
if (point.x() < min.x() || point.y() < min.y() || point.z() < min.z() ||
point.x() > max.x() || point.y() > max.y() || point.z() > max.z()) {
return false;
}
return true;
}
bool intersects(const AABB& aabb, const Ray& ray)
{
return intersectsLine(aabb, ray, 0.0,
std::numeric_limits<double>::infinity()); // TODO: infinity or
// max?
}
bool intersects(const AABB& aabb, const Sphere& sphere)
{
Point closest_point = closestPoint(aabb, sphere.center);
double distance_squared = (sphere.center - closest_point).squaredNorm();
double radius_squared = sphere.radius * sphere.radius;
return distance_squared < radius_squared;
}
// Frustum
bool intersects(const Frustum& frustum, const AABB& aabb)
{
return intersects(aabb, frustum);
}
bool intersects(const Frustum& frustum_1, const Frustum& frustum_2)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const Frustum& frustum, const LineSegment& line_segment)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const Frustum& frustum, const OBB& obb)
{
for (int i = 0; i < 6; ++i) {
double side = classify(obb, frustum.planes[i]);
if (side < 0) {
return false;
}
}
return true;
}
bool intersects(const Frustum& frustum, const Plane& plane)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const Frustum& frustum, const Point& point)
{
// Do similar as for sphere
return intersects(frustum, Sphere(point, 0.0));
}
bool intersects(const Frustum& frustum, const Ray& ray)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const Frustum& frustum, const Sphere& sphere)
{
for (int i = 0; i < 6; ++i) {
const Point& normal = frustum.planes[i].normal;
const double& distance = frustum.planes[i].distance;
const double side = Point::dot(sphere.center, normal) + distance;
if (side < -sphere.radius) {
return false;
}
}
return true;
}
// Line segment
bool intersects(const LineSegment& line_segment, const AABB& aabb)
{
return intersects(aabb, line_segment);
}
bool intersects(const LineSegment& line_segment, const Frustum& frustum)
{
return intersects(frustum, line_segment);
}
bool intersects(const LineSegment& line_segment_1, const LineSegment& line_segment_2)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const LineSegment& line_segment, const OBB& obb)
{
Ray ray;
ray.origin = line_segment.start;
ray.direction = line_segment.end - line_segment.start;
double line_length_squared = ray.direction.squaredNorm();
if (line_length_squared < 0.0000001f) {
return intersects(obb, line_segment.start);
}
ray.direction /= line_length_squared; // Normalize
// Begin ray casting
Point p = obb.center - ray.origin;
Point X(obb.rotation[0], 0, 0);
Point Y(0, obb.rotation[1], 0);
Point Z(0, 0, obb.rotation[2]);
Point f(Point::dot(X, ray.direction), Point::dot(Y, ray.direction),
Point::dot(Z, ray.direction));
Point e(Point::dot(X, p), Point::dot(Y, p), Point::dot(Z, p));
double t[6] = {0, 0, 0, 0, 0, 0};
for (int i = 0; i < 3; ++i) {
if (0.0 == f[i]) // TODO: Should be approximate equal
{
if (-e[i] - obb.half_size[i] > 0 || -e[i] + obb.half_size[i] < 0) {
return false;
}
f[i] = 0.00001f; // Avoid div by 0!
}
t[i * 2 + 0] = (e[i] + obb.half_size[i]) / f[i]; // tmin[x, y, z]
t[i * 2 + 1] = (e[i] - obb.half_size[i]) / f[i]; // tmax[x, y, z]
}
double tmin = std::max(std::max(std::min(t[0], t[1]), std::min(t[2], t[3])),
std::min(t[4], t[5]));
double tmax = std::min(std::min(std::max(t[0], t[1]), std::max(t[2], t[3])),
std::max(t[4], t[5]));
// if tmax < 0, ray is intersecting AABB
// but entire AABB is behing it's origin
if (tmax < 0) {
return false;
}
// if tmin > tmax, ray doesn't intersect AABB
if (tmin > tmax) {
return false;
}
// If tmin is < 0, tmax is closer
double t_result = tmin;
if (tmin < 0.0f) {
t_result = tmax;
}
// End ray casting
return t_result >= 0 && t_result * t_result <= line_length_squared;
}
bool intersects(const LineSegment& line_segment, const Plane& plane)
{
Point ab = line_segment.end - line_segment.start;
double n_A = Point::dot(plane.normal, line_segment.start);
double n_AB = Point::dot(plane.normal, ab);
if (0.0 == n_AB) // TODO: Almost equal?
{
return false;
}
double t = (plane.distance - n_A) / n_AB;
return t >= 0.0 && t <= 1.0;
}
bool intersects(const LineSegment& line_segment, const Point& point)
{
Point closest_point = closestPoint(line_segment, point);
double distance_squared = (closest_point - point).squaredNorm();
return 0.0 == distance_squared; // TODO: Almost equal?
}
bool intersects(const LineSegment& line_segment, const Ray& ray)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const LineSegment& line_segment, const Sphere& sphere)
{
Point closest_point = closestPoint(line_segment, sphere.center);
double distance_squared = (sphere.center - closest_point).squaredNorm();
return distance_squared <= (sphere.radius * sphere.radius);
}
// OBB
bool intersects(const OBB& obb, const AABB& aabb) { return intersects(aabb, obb); }
bool intersects(const OBB& obb, const Frustum& frustum)
{
return intersects(frustum, obb);
}
bool intersects(const OBB& obb, const LineSegment& line_segment)
{
return intersects(line_segment, obb);
}
bool intersects(const OBB& obb_1, const OBB& obb_2)
{
std::vector<double> obb_1_rot_matrix;
obb_1.rotation.toRotMatrix(obb_1_rot_matrix);
std::vector<double> obb_2_rot_matrix;
obb_2.rotation.toRotMatrix(obb_2_rot_matrix);
Point test[15] = {Point(obb_1_rot_matrix[0], obb_1_rot_matrix[1], obb_1_rot_matrix[2]),
Point(obb_1_rot_matrix[3], obb_1_rot_matrix[4], obb_1_rot_matrix[5]),
Point(obb_1_rot_matrix[6], obb_1_rot_matrix[7], obb_1_rot_matrix[8]),
Point(obb_2_rot_matrix[0], obb_2_rot_matrix[1], obb_2_rot_matrix[2]),
Point(obb_2_rot_matrix[3], obb_2_rot_matrix[4], obb_2_rot_matrix[5]),
Point(obb_2_rot_matrix[6], obb_2_rot_matrix[7], obb_2_rot_matrix[8])};
for (int i = 0; i < 3; ++i) { // Fill out rest of axis
test[6 + i * 3 + 0] = Point::cross(test[i], test[0]);
test[6 + i * 3 + 1] = Point::cross(test[i], test[1]);
test[6 + i * 3 + 2] = Point::cross(test[i], test[2]);
}
for (int i = 0; i < 15; ++i) {
if (!overlapOnAxis(obb_1, obb_2, test[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool intersects(const OBB& obb, const Plane& plane)
{
Point rot[] = {
Point(obb.rotation[0], 0, 0),
Point(0, obb.rotation[1], 0),
Point(0, 0, obb.rotation[2]),
};
Point normal = plane.normal;
// Project the half extents of the AABB onto the plane normal
double p_len = obb.half_size.x() * std::fabs(Point::dot(normal, rot[0])) +
obb.half_size.y() * std::fabs(Point::dot(normal, rot[1])) +
obb.half_size.z() * std::fabs(Point::dot(normal, rot[2]));
// Find the distance from the center of the OBB to the plane
double distance = Point::dot(plane.normal, obb.center) - plane.distance;
// Intersection occurs if the distance falls within the projected side
return std::fabs(distance) <= p_len;
}
bool intersects(const OBB& obb, const Point& point)
{
// TODO: Implement look earlier. THIS IS WRONG!
Point dir = point - obb.center;
std::vector<double> obb_rot_matrix;
obb.rotation.toRotMatrix(obb_rot_matrix);
for (int i = 0; i < 3; ++i) {
Point axis(obb_rot_matrix[i * 3], obb_rot_matrix[i * 3 + 1],
obb_rot_matrix[i * 3 + 2]);
double distance = Point::dot(dir, axis);
if (distance > obb.half_size[i]) {
return false;
}
if (distance < -obb.half_size[i]) // TODO: Should this be else if?
{
return false;
}
}
return true;
}
bool intersects(const OBB& obb, const Ray& ray)
{
Point p = obb.center - ray.origin;
Point X(obb.rotation[0], 0, 0);
Point Y(0, obb.rotation[1], 0);
Point Z(0, 0, obb.rotation[2]);
Point f(Point::dot(X, ray.direction), Point::dot(Y, ray.direction),
Point::dot(Z, ray.direction));
Point e(Point::dot(X, p), Point::dot(Y, p), Point::dot(Z, p));
double t[6] = {0, 0, 0, 0, 0, 0};
for (int i = 0; i < 3; ++i) {
if (0.0 == f[i]) // TODO: Should be approximate equal?
{
if (-e[i] - obb.half_size[i] > 0 || -e[i] + obb.half_size[i] < 0) {
return false;
}
f[i] = 0.00001f; // Avoid div by 0!
}
t[i * 2 + 0] = (e[i] + obb.half_size[i]) / f[i]; // tmin[x, y, z]
t[i * 2 + 1] = (e[i] - obb.half_size[i]) / f[i]; // tmax[x, y, z]
}
double tmin = std::max(std::max(std::min(t[0], t[1]), std::min(t[2], t[3])),
std::min(t[4], t[5]));
double tmax = std::min(std::min(std::max(t[0], t[1]), std::max(t[2], t[3])),
std::max(t[4], t[5]));
// if tmax < 0, ray is intersecting AABB
// but entire AABB is behing it's origin
if (tmax < 0) {
return false;
}
// if tmin > tmax, ray doesn't intersect AABB
if (tmin > tmax) {
return false;
}
return true;
}
bool intersects(const OBB& obb, const Sphere& sphere)
{
Point closest_point = closestPoint(obb, sphere.center);
double distance_squared = (sphere.center - closest_point).squaredNorm();
return distance_squared < sphere.radius * sphere.radius;
}
// Plane
bool intersects(const Plane& plane, const AABB& aabb) { return intersects(aabb, plane); }
bool intersects(const Plane& plane, const Frustum& frustum)
{
return intersects(frustum, plane);
}
bool intersects(const Plane& plane, const LineSegment& line_segment)
{
return intersects(line_segment, plane);
}
bool intersects(const Plane& plane, const OBB& obb) { return intersects(obb, plane); }
bool intersects(const Plane& plane_1, const Plane& plane_2)
{
Point d = Point::cross(plane_1.normal, plane_2.normal);
return 0.0 != Point::dot(d, d); // TODO: Almost not equal?
}
bool intersects(const Plane& plane, const Point& point)
{
return Point::dot(point, plane.normal) - plane.distance;
}
bool intersects(const Plane& plane, const Ray& ray)
{
double nd = Point::dot(ray.direction, plane.normal);
double pn = Point::dot(ray.origin, plane.normal);
if (nd >= 0.0f) {
return false;
}
double t = (plane.distance - pn) / nd;
return t >= 0.0;
}
bool intersects(const Plane& plane, const Sphere& sphere)
{
Point closest_point = closestPoint(plane, sphere.center);
double distance_squared = (sphere.center - closest_point).squaredNorm();
return distance_squared < sphere.radius * sphere.radius;
}
// Point
bool intersects(const Point& point, const AABB& aabb) { return intersects(aabb, point); }
bool intersects(const Point& point, const Frustum& frustum)
{
return intersects(frustum, point);
}
bool intersects(const Point& point, const LineSegment& line_segment)
{
return intersects(line_segment, point);
}
bool intersects(const Point& point, const OBB& obb) { return intersects(obb, point); }
bool intersects(const Point& point, const Plane& plane)
{
return intersects(plane, point);
}
bool intersects(const Point& point_1, const Point& point_2)
{
return point_1 == point_2; // TODO: Almost equal?
}
bool intersects(const Point& point, const Ray& ray)
{
if (ray.origin == point) {
return true;
}
Point direction = point - ray.origin;
direction.normalize();
return 1.0 == Point::dot(direction, ray.direction); // TODO: Almost equal?
}
bool intersects(const Point& point, const Sphere& sphere)
{
return (point - sphere.center).squaredNorm() < sphere.radius * sphere.radius;
}
// Ray
bool intersects(const Ray& ray, const AABB& aabb) { return intersects(aabb, ray); }
bool intersects(const Ray& ray, const Frustum& frustum)
{
return intersects(frustum, ray);
}
bool intersects(const Ray& ray, const LineSegment& line_segment)
{
return intersects(line_segment, ray);
}
bool intersects(const Ray& ray, const OBB& obb) { return intersects(obb, ray); }
bool intersects(const Ray& ray, const Plane& plane) { return intersects(plane, ray); }
bool intersects(const Ray& ray, const Point& point) { return intersects(point, ray); }
bool intersects(const Ray& ray_1, const Ray& ray_2)
{
throw std::logic_error("Function not yet implemented");
// TODO: Implement
}
bool intersects(const Ray& ray, const Sphere& sphere)
{
Point e = sphere.center - ray.origin;
double rSq = sphere.radius * sphere.radius;
double eSq = e.squaredNorm();
double a = Point::dot(e, ray.direction);
return rSq - (eSq - a * a) >= 0.0;
}
// Sphere
bool intersects(const Sphere& sphere, const AABB& aabb)
{
return intersects(aabb, sphere);
}
bool intersects(const Sphere& sphere, const Frustum& frustum)
{
return intersects(frustum, sphere);
}
bool intersects(const Sphere& sphere, const LineSegment& line_segment)
{
return intersects(line_segment, sphere);
}
bool intersects(const Sphere& sphere, const OBB& obb) { return intersects(obb, sphere); }
bool intersects(const Sphere& sphere, const Plane& plane)
{
return intersects(plane, sphere);
}
bool intersects(const Sphere& sphere, const Point& point)
{
return intersects(point, sphere);
}
bool intersects(const Sphere& sphere, const Ray& ray) { return intersects(ray, sphere); }
bool intersects(const Sphere& sphere_1, const Sphere& sphere_2)
{
double radius_sum = sphere_1.radius + sphere_2.radius;
double distance_squared = (sphere_1.center - sphere_2.center).squaredNorm();
return distance_squared < radius_sum * radius_sum;
}
/////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////// Inside tests
/////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
} // namespace ufo::geometry
ufomap/src/map/occupancy_map.cpp
Executable
+66
View File
@@ -0,0 +1,66 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ufo/map/occupancy_map.h>
namespace ufo::map
{
OccupancyMap::OccupancyMap(double resolution, DepthType depth_levels,
bool automatic_pruning, double occupied_thres,
double free_thres, double prob_hit, double prob_miss,
double clamping_thres_min, double clamping_thres_max)
: OccupancyMapBase(resolution, depth_levels, automatic_pruning, occupied_thres,
free_thres, prob_hit, prob_miss, clamping_thres_min,
clamping_thres_max)
{
}
OccupancyMap::OccupancyMap(std::string const& filename, bool automatic_pruning,
double occupied_thres, double free_thres, double prob_hit,
double prob_miss, double clamping_thres_min,
double clamping_thres_max)
: OccupancyMapBase(filename, automatic_pruning, occupied_thres, free_thres, prob_hit,
prob_miss, clamping_thres_min, clamping_thres_max)
{
}
OccupancyMap::OccupancyMap(OccupancyMap const& other) : OccupancyMapBase(other) {}
} // namespace ufo::map
ufomap/src/map/occupancy_map_color.cpp
Executable
+223
View File
@@ -0,0 +1,223 @@
/**
* UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ufo/map/occupancy_map_color.h>
namespace ufo::map
{
OccupancyMapColor::OccupancyMapColor(double resolution, DepthType depth_levels,
bool automatic_pruning, double occupied_thres,
double free_thres, double prob_hit, double prob_miss,
double clamping_thres_min, double clamping_thres_max)
: OccupancyMapBase(resolution, depth_levels, automatic_pruning, occupied_thres,
free_thres, prob_hit, prob_miss, clamping_thres_min,
clamping_thres_max)
{
}
OccupancyMapColor::OccupancyMapColor(std::string const& filename, bool automatic_pruning,
double occupied_thres, double free_thres,
double prob_hit, double prob_miss,
double clamping_thres_min, double clamping_thres_max)
: OccupancyMapBase(filename, automatic_pruning, occupied_thres, free_thres, prob_hit,
prob_miss, clamping_thres_min, clamping_thres_max)
{
}
OccupancyMapColor::OccupancyMapColor(OccupancyMapColor const& other)
: OccupancyMapBase(other)
{
}
//
// Set color
//
void OccupancyMapColor::setColor(Code const& code, Color color)
{
auto path = Base::createNode(code);
DepthType depth = code.getDepth();
path[depth]->value.color = color;
Base::updateParents(path, depth);
}
//
// Get color
//
Color OccupancyMapColor::getColor(Code const& code) const
{
return Base::getNode(code).first->value.color;
}
//
// Integrate colors
//
void OccupancyMapColor::integrateColors(Point3 const& sensor_origin,
PointCloudColor const& cloud, double max_range)
{
CodeMap<std::vector<Color>> colors;
for (Point3Color const& point : cloud) {
if (0 > max_range || (point - sensor_origin).norm() < max_range) {
colors[Base::toCode(point)].push_back(point.getColor());
}
}
for (auto const& [code, color] : colors) {
updateNodeColor(code, getAverageColor(color));
}
}
//
// Update node
//
bool OccupancyMapColor::updateNode(INNER_NODE& node, DepthType depth)
{
Color new_color = getAverageChildColor(node, depth);
bool changed = Base::updateNode(node, depth);
changed = changed || (node.value.color != new_color);
node.value.color = new_color;
return changed;
}
//
// Update node color
//
void OccupancyMapColor::updateNodeColor(Code code, Color update)
{
if (!update.isSet()) {
return;
}
auto path = Base::createNode(code);
DepthType depth = code.getDepth();
updateNodeColor(*path[depth], update, 1.0 - Base::getOccupancy(*path[depth]));
Base::updateParents(path, depth);
}
void OccupancyMapColor::updateNodeColor(LEAF_NODE& node, Color update, double prob)
{
Color& current = node.value.color;
if (current == update) {
return;
}
if (!current.isSet()) {
current = update;
} else {
double total_prob = prob + Base::getOccupancy(node);
prob /= total_prob;
// double prob = std::max(0.0, std::min(2.0 * (Base::getOccupancy(node) - 0.5), 0.9));
double prob_inv = 1.0 - prob;
double c_r = static_cast<double>(current.r);
double c_g = static_cast<double>(current.g);
double c_b = static_cast<double>(current.b);
double u_r = static_cast<double>(update.r);
double u_g = static_cast<double>(update.g);
double u_b = static_cast<double>(update.b);
current.r = std::sqrt(((c_r * c_r) * prob_inv) + ((u_r * u_r) * prob));
current.g = std::sqrt(((c_g * c_g) * prob_inv) + ((u_g * u_g) * prob));
current.b = std::sqrt(((c_b * c_b) * prob_inv) + ((u_b * u_b) * prob));
}
}
//
// Average child color
//
Color OccupancyMapColor::getAverageChildColor(INNER_NODE const& node,
DepthType depth) const
{
if (!hasChildren(node)) {
return node.value.color;
}
std::vector<Color> colors;
for (int i = 0; i < 8; ++i) {
LEAF_NODE& child = getChild(node, depth - 1, i);
if (child.value.color.isSet()) {
colors.push_back(child.value.color);
}
}
return getAverageColor(colors);
}
//
// Average color
//
Color OccupancyMapColor::getAverageColor(std::vector<Color> const& colors) const
{
if (colors.empty()) {
return Color();
}
// TODO: Update to LAB space?
double r = 0;
double g = 0;
double b = 0;
for (Color const& color : colors) {
double color_r = static_cast<double>(color.r);
double color_g = static_cast<double>(color.g);
double color_b = static_cast<double>(color.b);
r += (color_r * color_r);
g += (color_g * color_g);
b += (color_b * color_b);
}
double num_colors = static_cast<double>(colors.size());
return Color(std::sqrt(r / num_colors), std::sqrt(g / num_colors),
std::sqrt(b / num_colors));
}
} // namespace ufo::map
ufomap/tests/CMakeLists.txt
Executable
View File
ufomap_ros/.clang-format
Executable
+167
View File
@@ -0,0 +1,167 @@
---
Language: Cpp
BasedOnStyle: Google
AccessModifierOffset: -1
AlignAfterOpenBracket: Align
AlignConsecutiveMacros: false
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllArgumentsOnNextLine: true
AllowAllConstructorInitializersOnNextLine: true
AllowAllParametersOfDeclarationOnNextLine: true
# AllowShortBlocksOnASingleLine: Never
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortLambdasOnASingleLine: All
AllowShortIfStatementsOnASingleLine: WithoutElse
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterCaseLabel: true
AfterClass: true
AfterControlStatement: true
AfterEnum: true
AfterFunction: true
AfterNamespace: true
AfterObjCDeclaration: true
AfterStruct: true
AfterUnion: true
AfterExternBlock: true
BeforeCatch: true
BeforeElse: true
IndentBraces: true
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Linux # Set to Custom to use BraceWrapping
BreakBeforeInheritanceComma: false
BreakInheritanceList: BeforeColon
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakConstructorInitializers: BeforeColon
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 90
CommentPragmas: "^ IWYU pragma:"
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
# DeriveLineEnding: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
FixNamespaceComments: true
ForEachMacros:
- foreach
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Regroup
IncludeCategories:
- Regex: '^<ext/.*\.h>'
Priority: 2
# SortPriority: 0
- Regex: '^<.*\.h>'
Priority: 1
# SortPriority: 0
- Regex: "^<.*"
Priority: 2
# SortPriority: 0
- Regex: ".*"
Priority: 3
# SortPriority: 0
IncludeIsMainRegex: "([-_](test|unittest))?$"
# IncludeIsMainSourceRegex: ""
IndentCaseLabels: true
# IndentGotoLabels: true
IndentPPDirectives: None
IndentWidth: 2
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ""
MacroBlockEnd: ""
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBinPackProtocolList: Never
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: true
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyBreakTemplateDeclaration: 10
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
RawStringFormats:
- Language: Cpp
Delimiters:
- cc
- CC
- cpp
- Cpp
- CPP
- "c++"
- "C++"
CanonicalDelimiter: ""
BasedOnStyle: google
- Language: TextProto
Delimiters:
- pb
- PB
- proto
- PROTO
EnclosingFunctions:
- EqualsProto
- EquivToProto
- PARSE_PARTIAL_TEXT_PROTO
- PARSE_TEST_PROTO
- PARSE_TEXT_PROTO
- ParseTextOrDie
- ParseTextProtoOrDie
CanonicalDelimiter: ""
BasedOnStyle: google
ReflowComments: true
SortIncludes: true
SortUsingDeclarations: true
SpaceAfterCStyleCast: false
SpaceAfterLogicalNot: false
SpaceAfterTemplateKeyword: true
SpaceBeforeAssignmentOperators: true
SpaceBeforeCpp11BracedList: false
SpaceBeforeCtorInitializerColon: true
SpaceBeforeInheritanceColon: true
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: true
# SpaceInEmptyBlock: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
# SpacesInConditionalStatement: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
# SpaceBeforeSquareBrackets: false
Standard: Auto
StatementMacros:
- Q_UNUSED
- QT_REQUIRE_VERSION
TabWidth: 2
# UseCRLF: false
UseTab: ForIndentation
---
ufomap_ros/README.md
Executable
+14
View File
@@ -0,0 +1,14 @@
# ufomap_ros
ufomap_ros is a ROS interface for UFOMap
## Table of Contents
* [ufomap_mapping](https://github.com/UnknownFreeOccupied/ufomap/tree/master/ufomap_ros/ufomap_mapping)
* Default UFOMap mapping server. Use this if you want to quickly get started mapping!
* [ufomap_msgs](https://github.com/UnknownFreeOccupied/ufomap/tree/master/ufomap_ros/ufomap_msgs)
* Contains UFOMap ROS messages and functions to convert between message and UFOMap types.
* [ufomap_ros](https://github.com/UnknownFreeOccupied/ufomap/tree/master/ufomap_ros/ufomap_ros)
* Contains functions to convert between ROS and UFOMap types.
* [ufomap_rviz_plugins](https://github.com/UnknownFreeOccupied/ufomap/tree/master/ufomap_ros/ufomap_rviz_plugins)
* Contains RViz plugins to visualize UFOMap in RViz.
* [ufomap_srvs](https://github.com/UnknownFreeOccupied/ufomap/tree/master/ufomap_ros/ufomap_srvs)
* Contains UFOMap ROS services.
ufomap_ros/ufomap_mapping/.clang-format
Executable
+167
View File
@@ -0,0 +1,167 @@
---
Language: Cpp
BasedOnStyle: Google
AccessModifierOffset: -1
AlignAfterOpenBracket: Align
AlignConsecutiveMacros: false
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllArgumentsOnNextLine: true
AllowAllConstructorInitializersOnNextLine: true
AllowAllParametersOfDeclarationOnNextLine: true
# AllowShortBlocksOnASingleLine: Never
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortLambdasOnASingleLine: All
AllowShortIfStatementsOnASingleLine: WithoutElse
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterCaseLabel: true
AfterClass: true
AfterControlStatement: true
AfterEnum: true
AfterFunction: true
AfterNamespace: true
AfterObjCDeclaration: true
AfterStruct: true
AfterUnion: true
AfterExternBlock: true
BeforeCatch: true
BeforeElse: true
IndentBraces: true
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Linux # Set to Custom to use BraceWrapping
BreakBeforeInheritanceComma: false
BreakInheritanceList: BeforeColon
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakConstructorInitializers: BeforeColon
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 90
CommentPragmas: "^ IWYU pragma:"
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
# DeriveLineEnding: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
FixNamespaceComments: true
ForEachMacros:
- foreach
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Regroup
IncludeCategories:
- Regex: '^<ext/.*\.h>'
Priority: 2
# SortPriority: 0
- Regex: '^<.*\.h>'
Priority: 1
# SortPriority: 0
- Regex: "^<.*"
Priority: 2
# SortPriority: 0
- Regex: ".*"
Priority: 3
# SortPriority: 0
IncludeIsMainRegex: "([-_](test|unittest))?$"
# IncludeIsMainSourceRegex: ""
IndentCaseLabels: true
# IndentGotoLabels: true
IndentPPDirectives: None
IndentWidth: 2
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ""
MacroBlockEnd: ""
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBinPackProtocolList: Never
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: true
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyBreakTemplateDeclaration: 10
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
RawStringFormats:
- Language: Cpp
Delimiters:
- cc
- CC
- cpp
- Cpp
- CPP
- "c++"
- "C++"
CanonicalDelimiter: ""
BasedOnStyle: google
- Language: TextProto
Delimiters:
- pb
- PB
- proto
- PROTO
EnclosingFunctions:
- EqualsProto
- EquivToProto
- PARSE_PARTIAL_TEXT_PROTO
- PARSE_TEST_PROTO
- PARSE_TEXT_PROTO
- ParseTextOrDie
- ParseTextProtoOrDie
CanonicalDelimiter: ""
BasedOnStyle: google
ReflowComments: true
SortIncludes: true
SortUsingDeclarations: true
SpaceAfterCStyleCast: false
SpaceAfterLogicalNot: false
SpaceAfterTemplateKeyword: true
SpaceBeforeAssignmentOperators: true
SpaceBeforeCpp11BracedList: false
SpaceBeforeCtorInitializerColon: true
SpaceBeforeInheritanceColon: true
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: true
# SpaceInEmptyBlock: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
# SpacesInConditionalStatement: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
# SpaceBeforeSquareBrackets: false
Standard: Auto
StatementMacros:
- Q_UNUSED
- QT_REQUIRE_VERSION
TabWidth: 2
# UseCRLF: false
UseTab: ForIndentation
---
ufomap_ros/ufomap_mapping/CMakeLists.txt
Executable
+199
View File
@@ -0,0 +1,199 @@
cmake_minimum_required(VERSION 3.8.2)
project(ufomap_mapping)
set(CMAKE_CXX_STANDARD 17)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
diagnostic_msgs
dynamic_reconfigure
geometry_msgs
roscpp
sensor_msgs
std_msgs
std_srvs
tf2
tf2_ros
tf2_sensor_msgs
ufomap_msgs
ufomap_ros
ufomap_srvs
)
find_package(ufomap REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# ufomap_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/Server.cfg
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES ufomap_mapping_server_nodelet
# CATKIN_DEPENDS roscpp ufomap ufomap_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_server_node
src/server_node.cpp
src/server.cpp
)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_server_node
${catkin_LIBRARIES}
UFO::Map
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
install(TARGETS ${PROJECT_NAME}_server_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ufomap_mapping.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
ufomap_ros/ufomap_mapping/README.md
+98
View File
@@ -0,0 +1,98 @@
## Dynamic Reconfigure API
ufomap_server offer a [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure) inferance to change a number of settings on the fly.
## ufomap_server
ufomap_server builds and distributes volumetric 3D occupancy maps as UFOMap binary stream.
See [launch/server.launch](https://github.com/UnknownFreeOccupied/ufomap/blob/master/ufomap_ros/ufomap_mapping/launch/server.launch) for an example launch file. The rest of the parameters can be changed using dynamic_reconfigure.
### Subscribed Topics
* **cloud_in** ([sensor_msgs/PointCloud2](http://docs.ros.org/en/api/sensor_msgs/html/msg/PointCloud2.html))
Incoming point cloud for integration. You need to remap this topic to your sensor data topic and provide a TF transform between the sensor data and the static map frame.
### Published Topics
* **~map** ([ufomap_msgs/UFOMapStamped](https://github.com/UnknownFreeOccupied/ufomap/blob/master/ufomap_ros/ufomap_msgs/msg/UFOMapStamped.msg))
The complete UFOMap as a binary stream, encoding unknown, free, and occupied space, together with [meta data](https://github.com/UnknownFreeOccupied/ufomap/blob/master/ufomap_ros/ufomap_msgs/msg/UFOMapMetaData.msg).
* **~map_depth_X** (where X is [1, 21], depending on the parameter `publish_depth`) [OPTIONAL] ([ufomap_msgs/UFOMapStamped](https://github.com/UnknownFreeOccupied/ufomap/blob/master/ufomap_ros/ufomap_msgs/msg/UFOMapStamped.msg))
Same as map, except only nodes down to depth X. This substantially reduces the message size and the time it takes to serialize and deserialize the message. Since many nodes does not require a map at finest resolution this is a good way to achieve additional performance.
### Services
* **~get_map** ([ufomap_srvs/GetMap](https://github.com/UnknownFreeOccupied/ufomap_ros/blob/master/ufomap_srvs/srv/GetMap.srv))
The complete UFOMap as a binary stream, encoding unknown, free, and occupied space, together with [meta data](https://github.com/UnknownFreeOccupied/ufomap/blob/master/ufomap_ros/ufomap_msgs/msg/UFOMapMetaData.msg).
* **~clear_volume** ([ufomap_srvs/ClearVolume](https://github.com/UnknownFreeOccupied/ufomap_ros/blob/master/ufomap_srvs/srv/ClearVolume.srv))
Clear a part of the map, specified by a bounding volume. Sets all voxels inside the bounding volume to `~clamping_thres_min`/free space.
* **~reset**
Resets the complete map.
### Parameters
* **~frame_id** (string, default: map)
Frame in which the map will be published. A transform from sensor data to this frame needs to be available when building maps.
* **~resolution** (double, default: 0.05)
Resolution in meter for the map when starting with an empty map; otherwise, the loaded file's resolution is used.
* **~depth_levels** (int, default: 16)
Number of depth levels for the octree when starting with an empty map; otherwise, the loaded file's depth_levels is used.
* **~color_map** (bool, default: false)
If a colored map should be built when starting with an empty map; otherwise, the loaded file's type is used.
* **~max_range** (double, default: -1.0 (unlimited))
Maximum range in meter for inserting point cloud data. Limiting the range to something useful (e.g. 5 m) prevents spurious erroneous points far away from the robot. It also increases the integration speed.
* **~insert_depth** (int, default: 0)
The depth of the octree that free space should be cleared at. 0 means at leaf level (finest resolution), higher value moves higher up to coarser resolutions. This significantly increases the integration speed, making it possible to do mapping at millimeter resolution. It is usually good to keep this at a value such that free space is cleared at around 16 cm. So, if your map has a resolution of 2 cm, you would use a value of 3, since 2^3 is 16. Occupied space is always integrated at the finest resolution.
* **~simple_ray_casting** (bool, default: false)
If the integration of point clouds should use the simple ray casting method. It can be a good idea to enable this if `~insert_depth != 0`.
* **~early_stopping** (int, default: 0 (disabled))
When a ray is cast while being integrated it detects if any other ray has already passed through the current voxel for the same point cloud. If it passes through more than `~early_stopping` voxels other rays have seen in a row, it is taken to be adding no new information and the casting stops. If this is set to 0 it is disabled.
* **~clear_robot** (bool, default: false)
Sets all space at the robots current position to free using the four parameters below. This ensures that the robot never be seen as being inside occupied or unknown space, which can otherwise be a problem for path/trajectory planners.
* **~robot_frame_id** (string, default: base_link)
The frame_id of the robot. Used when clearing space.
* **~robot_height** (double, default: 0.2)
The robot heigh used when clearing space, all space with z [-`~robot_height`, `~robot_height`] relative to `~robot_frame_id` will be cleared.
* **~robot_radius** (double, default: 0.5)
The robot heigh used when clearing space, all space with x/y [-`~robot_radius`, `~robot_radius`] relative to `~robot_frame_id` will be cleared.
* **~clearing_depth** (int, default: 0)
The depth of the octree that space should be cleared around the robot's current pose.
* **~compress** (bool, default: false)
If the published UFOMap ROS messages should be compressed or not. It is a good idea to enable this if transfering the messages between computers.
* **~update_part_of_map** (bool, default: true)
If only the updated part of the map should be published instead of the whole map. It is recommended that this is enabled, as it reduces the amount of data that has to be transfered significantly. It is also a lot faster since only a small amount of data has to be serialized/deserialized.
If a node subscribes to the map in the middle of the mapping process, the whole map is sent only to that node. This is to ensure that every nodes that subscribe to the server has a complete map.
* **~update_rate** (double, default: 0.0 (immediately))
How often the updated part of the map should be published. Setting this to 0 means it will be published as soon as the map has been updated.
* **~publish_depth** (int, default: 4)
What depths should be publish on the `~map_depth_X` topics.
A value of 0 means no `~map_depth_X` will be published.
A value of 2 means, `~map`, `~map_depth_1`, and `~map_depth_2` containing all data, all data down to depth level 1, and all data down to depth level 2, respectively, will be published.
This is useful if you have a system with many nodes that require different map resolutions. By subscribing to the `~map_depth_X` topics, less data has to be transfered.
* **~prob_hit** (double, default: 0.7)
Probabilities for hits in the sensor model when integrating a point cloud.
* **~prob_miss** (double, default: 0.4)
Probabilities for misses in the sensor model when integrating a point cloud.
* **~clamping_thres_min** (double, default: 0.1192)
Minimum probability for clamping when building a map.
* **~clamping_thres_max** (double, default: 0.971)
Maximum probability for clamping when building a map.
* **~transform_timeout** (double, default: 0.1)
Maxmimum time in seconds to wait for the TF transform between the sensor data frame and map frame.
* **~map_queue_size** (int, default: 10)
Queue size for the published maps.
* **~cloud_in_queue_size** (int, default: 10)
Queue size for the subscribed point cloud.
* **~pub_rate** (double, default: 0.0 (never))
How often the whole map should be published. A value of 0.0 means never, then only the updated part of the map will be published (if enabled). This can be good to enable to ensure that every node has a complete map. However, it can take a very long time to publish and process the whole map if using a fine resolution (below 2 cm) and in a big environment.
* **~map_latch** (bool, default: false)
Whether the published topics should be latched or not. For maximum performance, set to false.
* **~verbose** (bool, default: false)
If enable, information, such as statistics, are outputted.
### Required TF Transforms
* **sensor data frame -> map**
Required transform of sensor data into the static map frame. It has to be possible to transform the point cloud coming from `cloud_in` to `frame_id`. You need to supply this transform.
* **robot frame -> map**
Only required if `~clear_robot` is enabled. It has to be possible to transform between the robots frame and the map, in order to get the robot's current pose.
**NOTE: '~' means it is in the ufomap_server's private namespace.**
ufomap_ros/ufomap_mapping/cfg/Server.cfg
Executable
+43
View File
@@ -0,0 +1,43 @@
#!/usr/bin/env python
PACKAGE = "ufomap_mapping"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Level Description Default Min Max
gen.add("frame_id", str_t, 1, "Frame id of the map", "map")
gen.add("max_range", double_t, 2, "Max range (m) when integrating data into map", -1.0, -1.0, 100.0)
gen.add("insert_depth", int_t, 2, "Integration depth", 0, 0, 10)
gen.add("simple_ray_casting", bool_t, 2, "Use simple ray casting", False)
gen.add("early_stopping", int_t, 2, "Early stopping", 0, 0, 10)
gen.add("async", bool_t, 2, "Async integration", True)
gen.add("clear_robot", bool_t, 3, "Clear map at robot position", False)
gen.add("robot_frame_id", str_t, 3, "Frame id of the robot", "base_link")
gen.add("robot_height", double_t, 3, "Robot height (m)", 0.2, 0.0, 100.0)
gen.add("robot_radius", double_t, 3, "Robot radius (m)", 0.5, 0.0, 100.0)
gen.add("clearing_depth", int_t, 3, "Clearing depth", 0, 0, 10)
gen.add("compress", bool_t, 4, "Compress msgs", False)
gen.add("update_part_of_map", bool_t, 4, "Publish updated parts of map", True)
gen.add("update_rate", double_t, 4, "How often map updates should be published (/s) (0 == asap)", 0.0, 0.0, 100.0)
gen.add("publish_depth", int_t, 4, "Depth of published map(s)", 4, 0, 10)
gen.add("prob_hit", double_t, 5, "Probability for hit", 0.7, 0.5, 1.0)
gen.add("prob_miss", double_t, 5, "Probability for miss", 0.4, 0.0, 0.5)
gen.add("clamping_thres_min", double_t, 5, "Minimum clamping threshold", 0.1192, 0.0, 1.0)
gen.add("clamping_thres_max", double_t, 5, "Maximum clamping threshold", 0.971, 0.0, 1.0)
gen.add("pub_rate", double_t, 6, "How often the whole map should be published (/s) (0 == never)", 0.0, 0.0, 100.0)
gen.add("transform_timeout", double_t, 7, "How long to wait for transform (s)", 0.1, 0.0, 100.0)
gen.add("map_queue_size", int_t, 8, "Queue size for map", 10, 0, 10000)
gen.add("cloud_in_queue_size", int_t, 8, "Queue size for cloud_in", 10, 0, 10000)
gen.add("map_latch", bool_t, 8, "Enable latched map topic", False)
gen.add("verbose", bool_t, 9, "Enable verbose output", False)
exit(gen.generate(PACKAGE, "ufomap_mapping", "Server"))
ufomap_ros/ufomap_mapping/include/ufomap_mapping/server.h
Executable
+194
View File
@@ -0,0 +1,194 @@
/**
* UFOMap Mapping
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_mapping
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFO_MAP_MAPPING_SERVER_H
#define UFO_MAP_MAPPING_SERVER_H
// UFO
#include <ufo/map/occupancy_map.h>
#include <ufo/map/occupancy_map_color.h>
#include <ufomap_mapping/ServerConfig.h>
#include <ufomap_srvs/ClearVolume.h>
#include <ufomap_srvs/GetMap.h>
#include <ufomap_srvs/Reset.h>
#include <ufomap_srvs/SaveMap.h>
// ROS
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <dynamic_reconfigure/server.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
// STD
#include <future>
#include <variant>
#include <vector>
namespace ufomap_mapping
{
class Server
{
public:
Server(ros::NodeHandle &nh, ros::NodeHandle &nh_priv);
private:
void cloudCallback(sensor_msgs::PointCloud2::ConstPtr const &msg);
void publishInfo();
void mapConnectCallback(ros::SingleSubscriberPublisher const &pub, int depth);
bool getMapCallback(ufomap_srvs::GetMap::Request &request,
ufomap_srvs::GetMap::Response &response);
bool clearVolumeCallback(ufomap_srvs::ClearVolume::Request &request,
ufomap_srvs::ClearVolume::Response &response);
bool resetCallback(ufomap_srvs::Reset::Request &request,
ufomap_srvs::Reset::Response &response);
bool saveMapCallback(ufomap_srvs::SaveMap::Request &request,
ufomap_srvs::SaveMap::Response &response);
void timerCallback(ros::TimerEvent const &event);
void configCallback(ufomap_mapping::ServerConfig &config, uint32_t level);
private:
//
// ROS parameters
//
// Node handles
ros::NodeHandle &nh_;
ros::NodeHandle &nh_priv_;
// Subscribers
ros::Subscriber cloud_sub_;
unsigned int cloud_in_queue_size_;
// Publishers
std::vector<ros::Publisher> map_pub_;
unsigned int map_queue_size_;
ros::Timer pub_timer_;
double pub_rate_;
ros::Duration update_rate_;
ros::Time last_update_time_;
ros::Publisher info_pub_;
// Services
ros::ServiceServer get_map_server_;
ros::ServiceServer clear_volume_server_;
ros::ServiceServer reset_server_;
ros::ServiceServer save_map_server_;
// TF2
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
ros::Duration transform_timeout_;
// Dynamic reconfigure
dynamic_reconfigure::Server<ufomap_mapping::ServerConfig> cs_;
//
// UFO Parameters
//
// Map
std::variant<std::monostate, ufo::map::OccupancyMap, ufo::map::OccupancyMapColor> map_;
std::string frame_id_;
// Integration
double max_range_;
ufo::map::DepthType insert_depth_;
bool simple_ray_casting_;
unsigned int early_stopping_;
bool async_;
// Clear robot
bool clear_robot_;
std::string robot_frame_id_;
double robot_height_;
double robot_radius_;
int clearing_depth_;
// Publishing
bool compress_;
bool update_part_of_map_;
ufo::map::DepthType publish_depth_;
std::future<void> update_async_handler_;
//
// Information
//
// Integration
double min_integration_time_;
double max_integration_time_ = 0.0;
double accumulated_integration_time_ = 0.0;
int num_integrations_ = 0;
// Clear robot
double min_clear_time_;
double max_clear_time_ = 0.0;
double accumulated_clear_time_ = 0.0;
int num_clears_ = 0;
// Publish update
double min_update_time_;
double max_update_time_ = 0.0;
double accumulated_update_time_ = 0.0;
int num_updates_ = 0;
// Publish whole
double min_whole_time_;
double max_whole_time_ = 0.0;
double accumulated_whole_time_ = 0.0;
int num_wholes_ = 0;
// Verbose
bool verbose_;
};
} // namespace ufomap_mapping
#endif // UFO_MAP_MAPPING_SERVER_H
ufomap_ros/ufomap_mapping/launch/server.launch
Executable
+17
View File
@@ -0,0 +1,17 @@
<?xml version="1.0" ?>
<launch>
<arg name="resolution" default="0.05" />
<arg name="depth_levels" default="16" />
<arg name="num_workers" default="1" />
<arg name="color" default="true" />
<node pkg="ufomap_mapping" type="ufomap_mapping_server_node" name="ufomap_mapping_server_node" output="log" required="true">
<remap from="cloud_in" to="/camera/depth/points" />
<param name="num_workers" value="$(arg num_workers)" />
<param name="resolution" value="$(arg resolution)" />
<param name="depth_levels" value="$(arg depth_levels)" />
<param name="color_map" value="$(arg color)"/>
</node>
</launch>
ufomap_ros/ufomap_mapping/package.xml
Executable
+37
View File
@@ -0,0 +1,37 @@
<?xml version="1.0"?>
<package format="2">
<name>ufomap_mapping</name>
<version>1.0.0</version>
<description>The ufomap_mapping package</description>
<maintainer email="dduberg@kth.se">Daniel Duberg</maintainer>
<license>BSD</license>
<url type="website">https://github.com/danielduberg/UFOMap</url>
<author email="dduberg@kth.se">Daniel Duberg</author>
<buildtool_depend>catkin</buildtool_depend>
<depend>diagnostic_msgs</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_sensor_msgs</depend>
<depend>ufomap_msgs</depend>
<depend>ufomap_ros</depend>
<depend>ufomap_srvs</depend>
<depend>ufomap</depend>
<doc_depend>doxygen</doc_depend>
<export>
</export>
</package>
ufomap_ros/ufomap_mapping/src/server.cpp
Executable
+513
View File
@@ -0,0 +1,513 @@
/**
* UFOMap Mapping
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_mapping
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
// UFO
#include <ufomap_mapping/server.h>
#include <ufomap_msgs/UFOMapStamped.h>
#include <ufomap_msgs/conversions.h>
#include <ufomap_ros/conversions.h>
// STD
#include <chrono>
#include <future>
#include <numeric>
namespace ufomap_mapping
{
Server::Server(ros::NodeHandle &nh, ros::NodeHandle &nh_priv)
: nh_(nh), nh_priv_(nh_priv), tf_listener_(tf_buffer_), cs_(nh_priv)
{
// Set up map
double resolution = nh_priv_.param("resolution", 0.05);
ufo::map::DepthType depth_levels = nh_priv_.param("depth_levels", 16);
// Automatic pruning is disabled so we can work in multiple threads for subscribers,
// services and publishers
if (nh_priv_.param("color_map", false)) {
map_.emplace<ufo::map::OccupancyMapColor>(resolution, depth_levels, false);
} else {
map_.emplace<ufo::map::OccupancyMap>(resolution, depth_levels, false);
}
// Enable min/max change detection
std::visit(
[this](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
map.enableMinMaxChangeDetection(true);
}
},
map_);
// Set up dynamic reconfigure server
cs_.setCallback(boost::bind(&Server::configCallback, this, _1, _2));
// Set up publisher
info_pub_ = nh_priv_.advertise<diagnostic_msgs::DiagnosticStatus>("info", 10, false);
// Enable services
get_map_server_ = nh_priv_.advertiseService("get_map", &Server::getMapCallback, this);
clear_volume_server_ =
nh_priv_.advertiseService("clear_volume", &Server::clearVolumeCallback, this);
reset_server_ = nh_priv_.advertiseService("reset", &Server::resetCallback, this);
save_map_server_ =
nh_priv_.advertiseService("save_map", &Server::saveMapCallback, this);
}
void Server::cloudCallback(sensor_msgs::PointCloud2::ConstPtr const &msg)
{
ufo::math::Pose6 transform;
try {
transform =
ufomap_ros::rosToUfo(tf_buffer_
.lookupTransform(frame_id_, msg->header.frame_id,
msg->header.stamp, transform_timeout_)
.transform);
} catch (tf2::TransformException &ex) {
ROS_WARN_THROTTLE(1, "%s", ex.what());
return;
}
std::visit(
[this, &msg, &transform](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
auto start = std::chrono::steady_clock::now();
// Update map
ufo::map::PointCloudColor cloud;
ufomap_ros::rosToUfo(*msg, cloud);
cloud.transform(transform, true);
map.insertPointCloudDiscrete(transform.translation(), cloud, max_range_,
insert_depth_, simple_ray_casting_,
early_stopping_, async_);
double integration_time =
std::chrono::duration<float, std::chrono::seconds::period>(
std::chrono::steady_clock::now() - start)
.count();
if (0 == num_integrations_ || integration_time < min_integration_time_) {
min_integration_time_ = integration_time;
}
if (integration_time > max_integration_time_) {
max_integration_time_ = integration_time;
}
accumulated_integration_time_ += integration_time;
++num_integrations_;
// Clear robot
if (clear_robot_) {
start = std::chrono::steady_clock::now();
try {
transform = ufomap_ros::rosToUfo(
tf_buffer_
.lookupTransform(frame_id_, robot_frame_id_, msg->header.stamp,
transform_timeout_)
.transform);
} catch (tf2::TransformException &ex) {
ROS_WARN_THROTTLE(1, "%s", ex.what());
return;
}
ufo::map::Point3 r(robot_radius_, robot_radius_, robot_height_ / 2.0);
ufo::geometry::AABB aabb(transform.translation() - r,
transform.translation() + r);
map.setValueVolume(aabb, map.getClampingThresMin(), clearing_depth_);
double clear_time =
std::chrono::duration<float, std::chrono::seconds::period>(
std::chrono::steady_clock::now() - start)
.count();
if (0 == num_clears_ || clear_time < min_clear_time_) {
min_clear_time_ = clear_time;
}
if (clear_time > max_clear_time_) {
max_clear_time_ = clear_time;
}
accumulated_clear_time_ += clear_time;
++num_clears_;
}
// Publish update
if (!map_pub_.empty() && update_part_of_map_ && map.validMinMaxChange() &&
(!last_update_time_.isValid() ||
(msg->header.stamp - last_update_time_) >= update_rate_)) {
bool can_update = true;
if (update_async_handler_.valid()) {
can_update = std::future_status::ready ==
update_async_handler_.wait_for(std::chrono::seconds(0));
}
if (can_update) {
last_update_time_ = msg->header.stamp;
start = std::chrono::steady_clock::now();
ufo::geometry::AABB aabb(map.minChange(), map.maxChange());
// TODO: should this be here?
map.resetMinMaxChangeDetection();
update_async_handler_ = std::async(
std::launch::async, [this, aabb, stamp = msg->header.stamp]() {
std::visit(
[this, &aabb, stamp](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>,
std::monostate>) {
for (int i = 0; i < map_pub_.size(); ++i) {
if (map_pub_[i] && (0 < map_pub_[i].getNumSubscribers() ||
map_pub_[i].isLatched())) {
ufomap_msgs::UFOMapStamped::Ptr msg(
new ufomap_msgs::UFOMapStamped);
if (ufomap_msgs::ufoToMsg(map, msg->map, aabb, compress_,
i)) {
msg->header.stamp = stamp;
msg->header.frame_id = frame_id_;
map_pub_[i].publish(msg);
}
}
}
}
},
map_);
});
double update_time =
std::chrono::duration<float, std::chrono::seconds::period>(
std::chrono::steady_clock::now() - start)
.count();
if (0 == num_updates_ || update_time < min_update_time_) {
min_update_time_ = update_time;
}
if (update_time > max_update_time_) {
max_update_time_ = update_time;
}
accumulated_update_time_ += update_time;
++num_updates_;
}
}
publishInfo();
}
},
map_);
}
void Server::publishInfo()
{
if (verbose_) {
printf("\nTimings:\n");
if (0 != num_integrations_) {
printf("\tIntegration time (s): %5d %09.6f\t(%09.6f +- %09.6f)\n",
num_integrations_, accumulated_integration_time_,
accumulated_integration_time_ / num_integrations_, max_integration_time_);
}
if (0 != num_clears_) {
printf("\tClear time (s): %5d %09.6f\t(%09.6f +- %09.6f)\n", num_clears_,
accumulated_clear_time_, accumulated_clear_time_ / num_clears_,
max_clear_time_);
}
if (0 != num_updates_) {
printf("\tUpdate time (s): %5d %09.6f\t(%09.6f +- %09.6f)\n", num_updates_,
accumulated_update_time_, accumulated_update_time_ / num_updates_,
max_update_time_);
}
if (0 != num_wholes_) {
printf("\tWhole time (s): %5d %09.6f\t(%09.6f +- %09.6f)\n", num_wholes_,
accumulated_whole_time_, accumulated_whole_time_ / num_wholes_,
max_whole_time_);
}
}
if (info_pub_ && 0 < info_pub_.getNumSubscribers()) {
diagnostic_msgs::DiagnosticStatus msg;
msg.level = diagnostic_msgs::DiagnosticStatus::OK;
msg.name = "UFOMap mapping timings";
msg.values.resize(12);
msg.values[0].key = "Min integration time (ms)";
msg.values[0].value = std::to_string(min_integration_time_);
msg.values[1].key = "Max integration time (ms)";
msg.values[1].value = std::to_string(max_integration_time_);
msg.values[2].key = "Average integration time (ms)";
msg.values[2].value =
std::to_string(accumulated_integration_time_ / num_integrations_);
msg.values[3].key = "Min clear time (ms)";
msg.values[3].value = std::to_string(min_clear_time_);
msg.values[4].key = "Max clear time (ms)";
msg.values[4].value = std::to_string(max_clear_time_);
msg.values[5].key = "Average clear time (ms)";
msg.values[5].value = std::to_string(accumulated_clear_time_ / num_clears_);
msg.values[6].key = "Min update time (ms)";
msg.values[6].value = std::to_string(min_update_time_);
msg.values[7].key = "Max update time (ms)";
msg.values[7].value = std::to_string(max_update_time_);
msg.values[8].key = "Average update time (ms)";
msg.values[8].value = std::to_string(accumulated_update_time_ / num_updates_);
msg.values[9].key = "Min whole time (ms)";
msg.values[9].value = std::to_string(min_whole_time_);
msg.values[10].key = "Max whole time (ms)";
msg.values[10].value = std::to_string(max_whole_time_);
msg.values[11].key = "Average whole time (ms)";
msg.values[11].value = std::to_string(accumulated_whole_time_ / num_wholes_);
info_pub_.publish(msg);
}
}
void Server::mapConnectCallback(ros::SingleSubscriberPublisher const &pub, int depth)
{
// When a new node subscribes we will publish the whole map to that node.
// TODO: Make this async
std::visit(
[this, &pub, depth](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
auto start = std::chrono::steady_clock::now();
ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);
if (ufomap_msgs::ufoToMsg(map, msg->map, compress_, depth)) {
msg->header.stamp = ros::Time::now();
msg->header.frame_id = frame_id_;
pub.publish(msg);
}
double whole_time = std::chrono::duration<float, std::chrono::seconds::period>(
std::chrono::steady_clock::now() - start)
.count();
if (0 == num_wholes_ || whole_time < min_whole_time_) {
min_whole_time_ = whole_time;
}
if (whole_time > max_whole_time_) {
max_whole_time_ = whole_time;
}
accumulated_whole_time_ += whole_time;
++num_wholes_;
}
},
map_);
}
bool Server::getMapCallback(ufomap_srvs::GetMap::Request &request,
ufomap_srvs::GetMap::Response &response)
{
std::visit(
[this, &request, &response](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
ufo::geometry::BoundingVolume bv =
ufomap_msgs::msgToUfo(request.bounding_volume);
response.success = ufomap_msgs::ufoToMsg(map, response.map, bv,
request.compress, request.depth);
} else {
response.success = false;
}
},
map_);
return true;
}
bool Server::clearVolumeCallback(ufomap_srvs::ClearVolume::Request &request,
ufomap_srvs::ClearVolume::Response &response)
{
std::visit(
[this, &request, &response](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
ufo::geometry::BoundingVolume bv =
ufomap_msgs::msgToUfo(request.bounding_volume);
for (auto &b : bv) {
map.setValueVolume(b, map.getClampingThresMin(), request.depth);
}
response.success = true;
} else {
response.success = false;
}
},
map_);
return true;
}
bool Server::resetCallback(ufomap_srvs::Reset::Request &request,
ufomap_srvs::Reset::Response &response)
{
std::visit(
[this, &request, &response](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
map.clear(request.new_resolution, request.new_depth_levels);
response.success = true;
} else {
response.success = false;
}
},
map_);
return true;
}
bool Server::saveMapCallback(ufomap_srvs::SaveMap::Request &request,
ufomap_srvs::SaveMap::Response &response)
{
std::visit(
[this, &request, &response](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
ufo::geometry::BoundingVolume bv =
ufomap_msgs::msgToUfo(request.bounding_volume);
response.success = map.write(request.filename, bv, request.compress,
request.depth, 1, request.compression_level);
} else {
response.success = false;
}
},
map_);
return true;
}
void Server::timerCallback(ros::TimerEvent const &event)
{
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id_;
if (!map_pub_.empty()) {
for (int i = 0; i < map_pub_.size(); ++i) {
if (map_pub_[i] &&
(0 < map_pub_[i].getNumSubscribers() || map_pub_[i].isLatched())) {
std::visit(
[this, &header, i](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>,
std::monostate>) {
auto start = std::chrono::steady_clock::now();
ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);
if (ufomap_msgs::ufoToMsg(map, msg->map, compress_, i)) {
msg->header = header;
map_pub_[i].publish(msg);
}
double whole_time =
std::chrono::duration<float, std::chrono::seconds::period>(
std::chrono::steady_clock::now() - start)
.count();
if (0 == num_wholes_ || whole_time < min_whole_time_) {
min_whole_time_ = whole_time;
}
if (whole_time > max_whole_time_) {
max_whole_time_ = whole_time;
}
accumulated_whole_time_ += whole_time;
++num_wholes_;
}
},
map_);
}
}
}
publishInfo();
}
void Server::configCallback(ufomap_mapping::ServerConfig &config, uint32_t level)
{
// Read parameters
frame_id_ = config.frame_id;
verbose_ = config.verbose;
max_range_ = config.max_range;
insert_depth_ = config.insert_depth;
simple_ray_casting_ = config.simple_ray_casting;
early_stopping_ = config.early_stopping;
async_ = config.async;
clear_robot_ = config.clear_robot;
robot_frame_id_ = config.robot_frame_id;
robot_height_ = config.robot_height;
robot_radius_ = config.robot_radius;
clearing_depth_ = config.clearing_depth;
compress_ = config.compress;
update_part_of_map_ = config.update_part_of_map;
publish_depth_ = config.publish_depth;
std::visit(
[this, &config](auto &map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
map.setProbHit(config.prob_hit);
map.setProbMiss(config.prob_miss);
map.setClampingThresMin(config.clamping_thres_min);
map.setClampingThresMax(config.clamping_thres_max);
}
},
map_);
transform_timeout_.fromSec(config.transform_timeout);
// Set up publisher
if (map_pub_.empty() || map_pub_[0].isLatched() != config.map_latch ||
map_queue_size_ != config.map_queue_size) {
map_pub_.resize(publish_depth_ + 1);
for (int i = 0; i < map_pub_.size(); ++i) {
map_queue_size_ = config.map_queue_size;
std::string final_topic = i == 0 ? "map" : "map_depth_" + std::to_string(i);
map_pub_[i] = nh_priv_.advertise<ufomap_msgs::UFOMapStamped>(
final_topic, map_queue_size_,
boost::bind(&Server::mapConnectCallback, this, _1, i),
ros::SubscriberStatusCallback(), ros::VoidConstPtr(), config.map_latch);
}
}
// Set up subscriber
if (!cloud_sub_ || cloud_in_queue_size_ != config.cloud_in_queue_size) {
cloud_in_queue_size_ = config.cloud_in_queue_size;
cloud_sub_ =
nh_.subscribe("cloud_in", cloud_in_queue_size_, &Server::cloudCallback, this);
}
// Set up timer
if (!pub_timer_ || pub_rate_ != config.pub_rate) {
pub_rate_ = config.pub_rate;
if (0 < pub_rate_) {
pub_timer_ =
nh_priv_.createTimer(ros::Rate(pub_rate_), &Server::timerCallback, this);
} else {
pub_timer_.stop();
}
}
update_rate_ = ros::Duration(1.0 / config.update_rate);
}
} // namespace ufomap_mapping
ufomap_ros/ufomap_mapping/src/server_node.cpp
Executable
+66
View File
@@ -0,0 +1,66 @@
/**
* UFOMap Mapping
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_mapping
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
// UFO
#include <ufomap_mapping/server.h>
// ROS
#include <ros/ros.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "ufomap_mapping_server");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
int num_workers = nh_priv.param("num_workers", 1);
ufomap_mapping::Server server(nh, nh_priv);
if (1 < num_workers) {
ros::MultiThreadedSpinner spinner(num_workers);
spinner.spin();
} else {
ros::spin();
}
return 0;
}
ufomap_ros/ufomap_msgs/CMakeLists.txt
Executable
+70
View File
@@ -0,0 +1,70 @@
cmake_minimum_required(VERSION 3.8.2)
project(ufomap_msgs)
set(CMAKE_CXX_STANDARD 17)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
message_generation
std_msgs
)
find_package(ufomap REQUIRED)
add_message_files(
DIRECTORY msg
FILES
AABB.msg
BoundingVolume.msg
# Capsule.msg
# Cone.msg
# Cylinder.msg
# Ellipsoid.msg
Frustum.msg
LineSegment.msg
OBB.msg
Plane.msg
Point.msg
Ray.msg
Sphere.msg
# Triangle.msg
UFOMap.msg
UFOMapMetaData.msg
UFOMapStamped.msg
)
generate_messages(
DEPENDENCIES
geometry_msgs
std_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} ${catkin_LIBRARIES}
CATKIN_DEPENDS geometry_msgs message_runtime std_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME} STATIC src/conversions.cpp)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}
PUBLIC
${catkin_LIBRARIES}
UFO::Map
)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
ufomap_ros/ufomap_msgs/include/ufomap_msgs/conversions.h
Executable
+190
View File
@@ -0,0 +1,190 @@
/**
* UFOMap ROS message conversions
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufo_ros
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFOMAP_ROS_MSGS_CONVERSIONS_H
#define UFOMAP_ROS_MSGS_CONVERSIONS_H
// UFO
#include <ufo/geometry/aabb.h>
#include <ufo/geometry/bounding_volume.h>
#include <ufo/geometry/frustum.h>
#include <ufo/geometry/line_segment.h>
#include <ufo/geometry/obb.h>
#include <ufo/geometry/plane.h>
#include <ufo/geometry/point.h>
#include <ufo/geometry/ray.h>
#include <ufo/geometry/sphere.h>
// UFO msg
#include <ufomap_msgs/AABB.h>
#include <ufomap_msgs/BoundingVolume.h>
#include <ufomap_msgs/Frustum.h>
#include <ufomap_msgs/LineSegment.h>
#include <ufomap_msgs/OBB.h>
#include <ufomap_msgs/Plane.h>
#include <ufomap_msgs/Point.h>
#include <ufomap_msgs/Ray.h>
#include <ufomap_msgs/Sphere.h>
#include <ufomap_msgs/UFOMap.h>
// STD
#include <type_traits>
namespace ufomap_msgs
{
//
// ROS message type to UFO type
//
ufo::geometry::Point msgToUfo(ufomap_msgs::Point const& point);
ufo::geometry::AABB msgToUfo(ufomap_msgs::AABB const& aabb);
ufo::geometry::Plane msgToUfo(ufomap_msgs::Plane const& plane);
ufo::geometry::Frustum msgToUfo(ufomap_msgs::Frustum const& frustum);
ufo::geometry::LineSegment msgToUfo(ufomap_msgs::LineSegment const& line_segment);
ufo::geometry::OBB msgToUfo(ufomap_msgs::OBB const& obb);
ufo::geometry::Ray msgToUfo(ufomap_msgs::Ray const& ray);
ufo::geometry::Sphere msgToUfo(ufomap_msgs::Sphere const& sphere);
ufo::geometry::BoundingVolume msgToUfo(ufomap_msgs::BoundingVolume const& msg);
//
// UFO type to ROS message type
//
ufomap_msgs::Point ufoToMsg(ufo::geometry::Point const& point);
ufomap_msgs::AABB ufoToMsg(ufo::geometry::AABB const& aabb);
ufomap_msgs::Plane ufoToMsg(ufo::geometry::Plane const& plane);
ufomap_msgs::Frustum ufoToMsg(ufo::geometry::Frustum const& frustum);
ufomap_msgs::LineSegment ufoToMsg(ufo::geometry::LineSegment const& line_segment);
ufomap_msgs::OBB ufoToMsg(ufo::geometry::OBB const& obb);
ufomap_msgs::Ray ufoToMsg(ufo::geometry::Ray const& ray);
ufomap_msgs::Sphere ufoToMsg(ufo::geometry::Sphere const& sphere);
ufomap_msgs::BoundingVolume ufoToMsg(
ufo::geometry::BoundingVolume const& bounding_volume);
//
// ROS message type to UFO type
//
template <typename TreeType>
bool msgToUfo(ufomap_msgs::UFOMap const& msg, TreeType& tree)
{
std::stringstream data_stream(std::ios_base::in | std::ios_base::out |
std::ios_base::binary);
if (!msg.data.empty()) {
data_stream.write((char const*)&msg.data[0], msg.data.size());
return tree.readData(data_stream, msgToUfo(msg.info.bounding_volume),
msg.info.resolution, msg.info.depth_levels,
msg.info.uncompressed_data_size, msg.info.compressed);
}
return false;
}
//
// UFO type to ROS message type
//
template <typename TreeType>
bool ufoToMsg(TreeType const& tree, ufomap_msgs::UFOMap& msg, bool compress = false,
unsigned int depth = 0, int compression_acceleration_level = 1,
int compression_level = 0)
{
return ufoToMsg(tree, msg, ufo::geometry::BoundingVolume(), compress, depth,
compression_acceleration_level, compression_level);
}
template <typename TreeType, typename BoundingType>
bool ufoToMsg(TreeType const& tree, ufomap_msgs::UFOMap& msg,
BoundingType const& bounding_volume, bool compress = false,
unsigned int depth = 0, int compression_acceleration_level = 1,
int compression_level = 0)
{
ufo::geometry::BoundingVolume bv;
bv.add(bounding_volume);
return ufoToMsg(tree, msg, bv, compress, depth, compression_acceleration_level,
compression_level);
}
template <typename TreeType>
bool ufoToMsg(TreeType const& tree, ufomap_msgs::UFOMap& msg,
ufo::geometry::BoundingVolume const& bounding_volume, bool compress = false,
unsigned int depth = 0, int compression_acceleration_level = 1,
int compression_level = 0)
{
msg.info.version = tree.getFileVersion();
msg.info.id = tree.getTreeType();
msg.info.resolution = tree.getResolution();
msg.info.depth_levels = tree.getTreeDepthLevels();
msg.info.compressed = compress;
msg.info.bounding_volume = ufoToMsg(bounding_volume);
std::stringstream data_stream(std::ios_base::in | std::ios_base::out |
std::ios_base::binary);
msg.info.uncompressed_data_size =
tree.writeData(data_stream, bounding_volume, compress, depth,
compression_acceleration_level, compression_level);
if (0 > msg.info.uncompressed_data_size) {
return false;
}
std::string const& data_string = data_stream.str();
msg.data = std::vector<int8_t>(data_string.begin(), data_string.end());
return true;
}
} // namespace ufomap_msgs
#endif // UFOMAP_ROS_MSGS_CONVERSIONS_H
ufomap_ros/ufomap_msgs/msg/AABB.msg
Executable
+3
View File
@@ -0,0 +1,3 @@
ufomap_msgs/Point center
ufomap_msgs/Point half_size
ufomap_ros/ufomap_msgs/msg/BoundingVolume.msg
Executable
+25
View File
@@ -0,0 +1,25 @@
ufomap_msgs/AABB[] aabbs
# ufomap_msgs/Capsule[] capsules
# ufomap_msgs/Cone[] cones
# ufomap_msgs/Cylinder[] cylinders
# ufomap_msgs/Ellipsoid[] ellipsoids
ufomap_msgs/Frustum[] frustums
ufomap_msgs/LineSegment[] line_segments
ufomap_msgs/OBB[] obbs
ufomap_msgs/Plane[] planes
ufomap_msgs/Point[] points
ufomap_msgs/Ray[] rays
ufomap_msgs/Sphere[] spheres
# ufomap_msgs/Triangle[] triangles
ufomap_ros/ufomap_msgs/msg/Capsule.msg
Executable
View File
ufomap_ros/ufomap_msgs/msg/Cone.msg
Executable
View File
ufomap_ros/ufomap_msgs/msg/Cylinder.msg
Executable
View File
ufomap_ros/ufomap_msgs/msg/Ellipsoid.msg
Executable
View File
ufomap_ros/ufomap_msgs/msg/Frustum.msg
Executable
+1
View File
@@ -0,0 +1 @@
ufomap_msgs/Plane[6] planes
ufomap_ros/ufomap_msgs/msg/LineSegment.msg
Executable
+3
View File
@@ -0,0 +1,3 @@
ufomap_msgs/Point start
ufomap_msgs/Point end
ufomap_ros/ufomap_msgs/msg/OBB.msg
Executable
+5
View File
@@ -0,0 +1,5 @@
ufomap_msgs/Point center
ufomap_msgs/Point half_size
ufomap_msgs/Point rotation
ufomap_ros/ufomap_msgs/msg/Plane.msg
Executable
+3
View File
@@ -0,0 +1,3 @@
ufomap_msgs/Point normal
float64 distance
ufomap_ros/ufomap_msgs/msg/Point.msg
Executable
+3
View File
@@ -0,0 +1,3 @@
float64 x
float64 y
float64 z
ufomap_ros/ufomap_msgs/msg/Ray.msg
Executable
+3
View File
@@ -0,0 +1,3 @@
ufomap_msgs/Point origin
ufomap_msgs/Point direction
ufomap_ros/ufomap_msgs/msg/Sphere.msg
Executable
+3
View File
@@ -0,0 +1,3 @@
ufomap_msgs/Point center
float64 radius
ufomap_ros/ufomap_msgs/msg/Triangle.msg
Executable
View File
ufomap_ros/ufomap_msgs/msg/UFOMap.msg
Executable
+5
View File
@@ -0,0 +1,5 @@
# A 3D map in binary format, as Octree
ufomap_msgs/UFOMapMetaData info
# Binary serialization of Octree, use conversions.h to read and write octrees
int8[] data
ufomap_ros/ufomap_msgs/msg/UFOMapMetaData.msg
Executable
+20
View File
@@ -0,0 +1,20 @@
# File version used
string version
# Class id of the contained Octree
string id
# Resolution (in m) of the octree leaf nodes
float64 resolution
# Number of depth levels
uint8 depth_levels
# If data is compressed
bool compressed
# Size of data uncompressed
int32 uncompressed_data_size
# Bounding volume
ufomap_msgs/BoundingVolume bounding_volume
ufomap_ros/ufomap_msgs/msg/UFOMapStamped.msg
+3
View File
@@ -0,0 +1,3 @@
Header header
ufomap_msgs/UFOMap map
ufomap_ros/ufomap_msgs/package.xml
Executable
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package format="2">
<name>ufomap_msgs</name>
<version>1.0.0</version>
<description>The ufomap_msgs package provides messages and serializations for the UFOMap library</description>
<author email="dduberg@kth.se">Daniel Duberg</author>
<maintainer email="dduberg@kth.se">Daniel Duberg</maintainer>
<license>BSD</license>
<url type="website">https://github.com/danielduberg/ufo_ros</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<build_depend>ufomap</build_depend>
<doc_depend>doxygen</doc_depend>
</package>
ufomap_ros/ufomap_msgs/src/conversions.cpp
Executable
+226
View File
@@ -0,0 +1,226 @@
/**
* UFOMap ROS message conversions
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufo_ros
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ufomap_msgs/conversions.h>
namespace ufomap_msgs
{
ufo::geometry::Point msgToUfo(ufomap_msgs::Point const& point)
{
return ufo::geometry::Point(point.x, point.y, point.z);
}
ufo::geometry::AABB msgToUfo(ufomap_msgs::AABB const& aabb)
{
ufo::geometry::AABB a;
a.center = msgToUfo(aabb.center);
a.half_size = msgToUfo(aabb.half_size);
return a;
}
ufo::geometry::Plane msgToUfo(ufomap_msgs::Plane const& plane)
{
return ufo::geometry::Plane(msgToUfo(plane.normal), plane.distance);
}
ufo::geometry::Frustum msgToUfo(ufomap_msgs::Frustum const& frustum)
{
ufo::geometry::Frustum f;
for (size_t i = 0; i < frustum.planes.size(); ++i) {
f.planes[i] = msgToUfo(frustum.planes[i]);
}
return f;
}
ufo::geometry::LineSegment msgToUfo(ufomap_msgs::LineSegment const& line_segment)
{
return ufo::geometry::LineSegment(msgToUfo(line_segment.start),
msgToUfo(line_segment.end));
}
ufo::geometry::OBB msgToUfo(ufomap_msgs::OBB const& obb)
{
return ufo::geometry::OBB(msgToUfo(obb.center), msgToUfo(obb.half_size),
msgToUfo(obb.rotation));
}
ufo::geometry::Ray msgToUfo(ufomap_msgs::Ray const& ray)
{
return ufo::geometry::Ray(msgToUfo(ray.origin), msgToUfo(ray.direction));
}
ufo::geometry::Sphere msgToUfo(ufomap_msgs::Sphere const& sphere)
{
return ufo::geometry::Sphere(msgToUfo(sphere.center), sphere.radius);
}
ufo::geometry::BoundingVolume msgToUfo(ufomap_msgs::BoundingVolume const& msg)
{
ufo::geometry::BoundingVolume bv;
for (ufomap_msgs::AABB const& aabb : msg.aabbs) {
bv.add(msgToUfo(aabb));
}
for (ufomap_msgs::Frustum const& frustum : msg.frustums) {
bv.add(msgToUfo(frustum));
}
for (ufomap_msgs::LineSegment const& line_segment : msg.line_segments) {
bv.add(msgToUfo(line_segment));
}
for (ufomap_msgs::OBB const& obb : msg.obbs) {
bv.add(msgToUfo(obb));
}
for (ufomap_msgs::Plane const& plane : msg.planes) {
bv.add(msgToUfo(plane));
}
for (ufomap_msgs::Point const& point : msg.points) {
bv.add(msgToUfo(point));
}
for (ufomap_msgs::Ray const& ray : msg.rays) {
bv.add(msgToUfo(ray));
}
for (ufomap_msgs::Sphere const& sphere : msg.spheres) {
bv.add(msgToUfo(sphere));
}
return bv;
}
//
// UFOMap type to ROS message type
//
ufomap_msgs::Point ufoToMsg(ufo::geometry::Point const& point)
{
ufomap_msgs::Point msg;
msg.x = point.x();
msg.y = point.y();
msg.z = point.z();
return msg;
}
ufomap_msgs::AABB ufoToMsg(ufo::geometry::AABB const& aabb)
{
ufomap_msgs::AABB msg;
msg.center = ufoToMsg(aabb.center);
msg.half_size = ufoToMsg(aabb.half_size);
return msg;
}
ufomap_msgs::Plane ufoToMsg(ufo::geometry::Plane const& plane)
{
ufomap_msgs::Plane msg;
msg.normal = ufoToMsg(plane.normal);
msg.distance = plane.distance;
return msg;
}
ufomap_msgs::Frustum ufoToMsg(ufo::geometry::Frustum const& frustum)
{
ufomap_msgs::Frustum msg;
for (size_t i = 0; i < msg.planes.size(); ++i) {
msg.planes[i] = ufoToMsg(frustum.planes[i]);
}
return msg;
}
ufomap_msgs::LineSegment ufoToMsg(ufo::geometry::LineSegment const& line_segment)
{
ufomap_msgs::LineSegment msg;
msg.start = ufoToMsg(line_segment.start);
msg.end = ufoToMsg(line_segment.end);
return msg;
}
ufomap_msgs::OBB ufoToMsg(ufo::geometry::OBB const& obb)
{
ufomap_msgs::OBB msg;
msg.center = ufoToMsg(obb.center);
msg.half_size = ufoToMsg(obb.half_size);
// TODO: Fix
// msg.rotation = ufoToMsg(obb.rotation);
return msg;
}
ufomap_msgs::Ray ufoToMsg(ufo::geometry::Ray const& ray)
{
ufomap_msgs::Ray msg;
msg.origin = ufoToMsg(ray.origin);
msg.direction = ufoToMsg(ray.direction);
return msg;
}
ufomap_msgs::Sphere ufoToMsg(ufo::geometry::Sphere const& sphere)
{
ufomap_msgs::Sphere msg;
msg.center = ufoToMsg(sphere.center);
msg.radius = sphere.radius;
return msg;
}
ufomap_msgs::BoundingVolume ufoToMsg(ufo::geometry::BoundingVolume const& bounding_volume)
{
ufomap_msgs::BoundingVolume msg;
for (ufo::geometry::BoundingVar const& bv : bounding_volume) {
std::visit(
[&msg](auto&& arg) -> void {
using T = std::decay_t<decltype(arg)>;
if constexpr (std::is_same_v<T, ufo::geometry::AABB>) {
msg.aabbs.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::geometry::Frustum>) {
msg.frustums.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::geometry::LineSegment>) {
msg.line_segments.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::geometry::OBB>) {
msg.obbs.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::geometry::Plane>) {
msg.planes.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::math::Vector3>) {
msg.points.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::geometry::Ray>) {
msg.rays.push_back(ufoToMsg(arg));
} else if constexpr (std::is_same_v<T, ufo::geometry::Sphere>) {
msg.spheres.push_back(ufoToMsg(arg));
}
},
bv);
}
return msg;
}
} // namespace ufomap_msgs
ufomap_ros/ufomap_ros/CMakeLists.txt
Executable
+45
View File
@@ -0,0 +1,45 @@
cmake_minimum_required(VERSION 3.8.2)
project(ufomap_ros)
set(CMAKE_CXX_STANDARD 17)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
sensor_msgs
)
find_package(ufomap REQUIRED)
catkin_package(
INCLUDE_DIRS include ${catkin_INCLUDE_DIRS}
LIBRARIES ${PROJECT_NAME} ${catkin_LIBRARIES}
CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME} STATIC
src/conversions.cpp
)
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}
PUBLIC
${catkin_LIBRARIES}
UFO::Map
)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
)
ufomap_ros/ufomap_ros/include/ufomap_ros/conversions.h
Executable
+107
View File
@@ -0,0 +1,107 @@
/**
* UFO ROS integration
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_ros
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFOMAP_ROS_CONVERSIONS_H
#define UFOMAP_ROS_CONVERSIONS_H
// UFO
#include <ufo/map/point_cloud.h>
#include <ufo/math/pose6.h>
#include <ufo/math/quaternion.h>
#include <ufo/math/vector3.h>
// ROS
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/PointCloud2.h>
namespace ufomap_ros
{
// Point clouds
void rosToUfo(sensor_msgs::PointCloud2 const& cloud_in, ufo::map::PointCloud& cloud_out);
void rosToUfo(sensor_msgs::PointCloud2 const& cloud_in,
ufo::map::PointCloudColor& cloud_out);
void ufoToRos(ufo::map::PointCloud const& cloud_in, sensor_msgs::PointCloud2& cloud_out);
void ufoToRos(ufo::map::PointCloudColor const& cloud_in,
sensor_msgs::PointCloud2& cloud_out);
// Vector3/Point
void rosToUfo(geometry_msgs::Point const& point_in, ufo::math::Vector3& point_out);
void rosToUfo(geometry_msgs::Vector3 const& point_in, ufo::math::Vector3& point_out);
ufo::math::Vector3 rosToUfo(geometry_msgs::Point const& point);
void ufoToRos(ufo::math::Vector3 const& point_in, geometry_msgs::Point& point_out);
void ufoToRos(ufo::math::Vector3 const& point_in, geometry_msgs::Vector3& point_out);
geometry_msgs::Point ufoToRos(ufo::math::Vector3 const& point);
// Quaternion
void rosToUfo(geometry_msgs::Quaternion const& quaternion_in,
ufo::math::Quaternion& quaternion_out);
ufo::math::Quaternion rosToUfo(const geometry_msgs::Quaternion& quaternion);
void ufoToRos(ufo::math::Quaternion const& quaternion_in,
geometry_msgs::Quaternion& quaternion_out);
geometry_msgs::Quaternion ufoToRos(ufo::math::Quaternion const& quaternion);
// Transforms
void rosToUfo(geometry_msgs::Transform const& transform_in,
ufo::math::Pose6& transform_out);
ufo::math::Pose6 rosToUfo(geometry_msgs::Transform const& transform);
void ufoToRos(ufo::math::Pose6 const& transform_in,
geometry_msgs::Transform& transform_out);
geometry_msgs::Transform ufoToRos(ufo::math::Pose6 const& transform);
} // namespace ufomap_ros
#endif // UFOMAP_ROS_CONVERSIONS_H
ufomap_ros/ufomap_ros/package.xml
Executable
+23
View File
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="2">
<name>ufomap_ros</name>
<version>1.0.0</version>
<description>The ufomap_ros package provides conversion functions between UFOMap and ROS native types</description>
<author email="dduberg@kth.se">Daniel Duberg</author>
<maintainer email="dduberg@kth.se">Daniel Duberg</maintainer>
<license>BSD</license>
<url type="website">https://github.com/danielduberg/ufomap_ros</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>geometry_msgs</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<build_depend>ufomap</build_depend>
<doc_depend>doxygen</doc_depend>
</package>
ufomap_ros/ufomap_ros/src/conversions.cpp
Executable
+291
View File
@@ -0,0 +1,291 @@
/**
* UFO ROS integration
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_ros
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
// UFO ROS
#include <ufomap_ros/conversions.h>
// ROS
#include <sensor_msgs/point_cloud2_iterator.h>
namespace ufomap_ros
{
void getFields(sensor_msgs::PointCloud2 const& cloud, bool& has_x, bool& has_y,
bool& has_z, bool& has_rgb)
{
has_x = false;
has_y = false;
has_z = false;
has_rgb = false;
for (auto const& field : cloud.fields) {
if ("x" == field.name) {
has_x = true;
} else if ("y" == field.name) {
has_y = true;
} else if ("z" == field.name) {
has_z = true;
} else if ("rgb" == field.name) {
has_rgb = true;
} else if ("r" == field.name) {
has_rgb = true;
} else if ("g" == field.name) {
has_rgb = true;
} else if ("b" == field.name) {
has_rgb = true;
}
}
}
void rosToUfo(sensor_msgs::PointCloud2 const& cloud_in, ufo::map::PointCloud& cloud_out)
{
cloud_out.reserve(cloud_in.data.size() / cloud_in.point_step);
bool has_x, has_y, has_z, has_rgb;
getFields(cloud_in, has_x, has_y, has_z, has_rgb);
if (!has_x || !has_y || !has_z) {
throw std::runtime_error("cloud_in missing one or more of the xyz fields");
}
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud_in, "z");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
cloud_out.push_back(ufo::map::Point3(*iter_x, *iter_y, *iter_z));
}
}
}
void rosToUfo(sensor_msgs::PointCloud2 const& cloud_in,
ufo::map::PointCloudColor& cloud_out)
{
cloud_out.reserve(cloud_in.data.size() / cloud_in.point_step);
bool has_x, has_y, has_z, has_rgb;
getFields(cloud_in, has_x, has_y, has_z, has_rgb);
if (!has_x || !has_y || !has_z) {
throw std::runtime_error("cloud_in missing one or more of the xyz fields");
}
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud_in, "z");
if (has_rgb) {
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_r(cloud_in, "r");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_g(cloud_in, "g");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_b(cloud_in, "b");
for (; iter_x != iter_x.end();
++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z) &&
!std::isnan(*iter_r) && !std::isnan(*iter_g) && !std::isnan(*iter_b)) {
cloud_out.push_back(
ufo::map::Point3Color(*iter_x, *iter_y, *iter_z, *iter_r, *iter_g, *iter_b));
}
}
} else {
// TODO: Should this throw?
// throw std::runtime_error("cloud_in missing one or more of the rgb fields");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
cloud_out.push_back(ufo::map::Point3Color(*iter_x, *iter_y, *iter_z));
}
}
}
}
void ufoToRos(ufo::map::PointCloud const& cloud_in, sensor_msgs::PointCloud2& cloud_out)
{
bool has_x, has_y, has_z, has_rgb;
getFields(cloud_out, has_x, has_y, has_z, has_rgb);
sensor_msgs::PointCloud2Modifier cloud_out_modifier(cloud_out);
cloud_out_modifier.setPointCloud2FieldsByString(1, "xyz");
cloud_out_modifier.resize(cloud_in.size());
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_out, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_out, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_out, "z");
for (size_t i = 0; i < cloud_in.size(); ++i, ++iter_x, ++iter_y, ++iter_z) {
*iter_x = cloud_in[i][0];
*iter_y = cloud_in[i][1];
*iter_z = cloud_in[i][2];
}
}
void ufoToRos(ufo::map::PointCloudColor const& cloud_in,
sensor_msgs::PointCloud2& cloud_out)
{
bool has_x, has_y, has_z, has_rgb;
getFields(cloud_out, has_x, has_y, has_z, has_rgb);
sensor_msgs::PointCloud2Modifier cloud_out_modifier(cloud_out);
cloud_out_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
cloud_out_modifier.resize(cloud_in.size());
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_out, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_out, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_out, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_out, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_out, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_out, "b");
for (size_t i = 0; i < cloud_in.size();
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) {
*iter_x = cloud_in[i][0];
*iter_y = cloud_in[i][1];
*iter_z = cloud_in[i][2];
*iter_r = cloud_in[i].getColor().r;
*iter_g = cloud_in[i].getColor().g;
*iter_b = cloud_in[i].getColor().b;
}
}
// Vector3/Point
void rosToUfo(geometry_msgs::Point const& point_in, ufo::math::Vector3& point_out)
{
point_out.x() = point_in.x;
point_out.y() = point_in.y;
point_out.z() = point_in.z;
}
void rosToUfo(geometry_msgs::Vector3 const& point_in, ufo::math::Vector3& point_out)
{
point_out.x() = point_in.x;
point_out.y() = point_in.y;
point_out.z() = point_in.z;
}
ufo::math::Vector3 rosToUfo(geometry_msgs::Point const& point)
{
return ufo::math::Vector3(point.x, point.y, point.z);
}
void ufoToRos(ufo::math::Vector3 const& point_in, geometry_msgs::Point& point_out)
{
point_out.x = point_in.x();
point_out.y = point_in.y();
point_out.z = point_in.z();
}
void ufoToRos(ufo::math::Vector3 const& point_in, geometry_msgs::Vector3& point_out)
{
point_out.x = point_in.x();
point_out.y = point_in.y();
point_out.z = point_in.z();
}
geometry_msgs::Point ufoToRos(ufo::math::Vector3 const& point)
{
geometry_msgs::Point point_out;
ufoToRos(point, point_out);
return point_out;
}
// Quaternion
void rosToUfo(geometry_msgs::Quaternion const& quaternion_in,
ufo::math::Quaternion& quaternion_out)
{
quaternion_out.x() = quaternion_in.x;
quaternion_out.y() = quaternion_in.y;
quaternion_out.z() = quaternion_in.z;
quaternion_out.w() = quaternion_in.w;
}
ufo::math::Quaternion rosToUfo(geometry_msgs::Quaternion const& quaternion)
{
return ufo::math::Quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z);
}
void ufoToRos(ufo::math::Quaternion const& quaternion_in,
geometry_msgs::Quaternion& quaternion_out)
{
quaternion_out.x = quaternion_in.x();
quaternion_out.y = quaternion_in.y();
quaternion_out.z = quaternion_in.z();
quaternion_out.w = quaternion_in.w();
}
geometry_msgs::Quaternion ufoToRos(ufo::math::Quaternion const& quaternion)
{
geometry_msgs::Quaternion quaternion_out;
ufoToRos(quaternion, quaternion_out);
return quaternion_out;
}
// Transforms
void rosToUfo(geometry_msgs::Transform const& transform_in,
ufo::math::Pose6& transform_out)
{
rosToUfo(transform_in.translation, transform_out.translation());
rosToUfo(transform_in.rotation, transform_out.rotation());
}
ufo::math::Pose6 rosToUfo(geometry_msgs::Transform const& transform)
{
return ufo::math::Pose6(transform.translation.x, transform.translation.y,
transform.translation.z, transform.rotation.w,
transform.rotation.x, transform.rotation.y,
transform.rotation.z);
}
void ufoToRos(ufo::math::Pose6 const& transform_in,
geometry_msgs::Transform& transform_out)
{
ufoToRos(transform_in.translation(), transform_out.translation);
ufoToRos(transform_in.rotation(), transform_out.rotation);
}
geometry_msgs::Transform ufoToRos(ufo::math::Pose6 const& transform)
{
geometry_msgs::Transform transform_out;
ufoToRos(transform, transform_out);
return transform_out;
}
} // namespace ufomap_ros
ufomap_ros/ufomap_rviz_plugins/CMakeLists.txt
Executable
+89
View File
@@ -0,0 +1,89 @@
cmake_minimum_required(VERSION 3.9)
project(ufomap_rviz_plugins
VERSION 1.0 LANGUAGES CXX
)
set(CMAKE_CXX_STANDARD 17)
find_package(catkin REQUIRED COMPONENTS
roscpp
rviz
ufomap_msgs
ufomap_ros
)
find_package(ufomap REQUIRED)
## This setting causes Qt's "MOC" generation to happen automatically.
set(CMAKE_AUTOMOC ON)
## This plugin includes Qt widgets, so we must include Qt.
## We'll use the version that rviz used so they are compatible.
if(rviz_QT_VERSION VERSION_LESS "5")
# Should be error
message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
## pull in all required include dirs, define QT_LIBRARIES, etc.
include(${QT_USE_FILE})
else()
message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies
set(QT_LIBRARIES Qt5::Widgets)
endif()
## I prefer the Qt signals and slots to avoid defining "emit", "slots",
## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here.
# add_definitions(-DQT_NO_KEYWORDS)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp rviz ufomap_msgs ufomap_ros
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
link_directories(
${catkin_LIBRARY_DIRS}
)
QT5_WRAP_CPP(MOC_FILES
# include/ufomap_rviz_plugins/ufomap_base_display.h
include/ufomap_rviz_plugins/ufomap_display.h
OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_LEXICAL_CAST_INCLUDED
)
add_library(${PROJECT_NAME}
# src/ufomap_base_display.cpp
src/ufomap_display.cpp
${MOC_FILES}
)
target_link_libraries(${PROJECT_NAME}
${QT_LIBRARIES}
${catkin_LIBRARIES}
UFO::Map
)
target_compile_options(${PROJECT_NAME}
PRIVATE
-Wno-register # TODO: Should not be needed if OGRE fixes it
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(FILES plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
ufomap_ros/ufomap_rviz_plugins/include/ufomap_rviz_plugins/ufomap_display.h
Executable
+216
View File
@@ -0,0 +1,216 @@
/**
* UFO RViz integration
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_ros
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UFOMAP_RVIZ_PLUGINS_UFOMAP_DISPLAY_H
#define UFOMAP_RVIZ_PLUGINS_UFOMAP_DISPLAY_H
// UFO
#include <ufo/map/occupancy_map.h>
#include <ufo/map/occupancy_map_color.h>
// UFO ROS
#include <ufomap_msgs/UFOMapStamped.h>
#include <ufomap_msgs/UFOMapMetaData.h>
// ROS
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#include <rviz/display.h>
#include <rviz/ogre_helpers/point_cloud.h>
#endif // Q_MOC_RUN
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreSceneNode.h>
#include <message_filters/subscriber.h>
#include <rviz/frame_manager.h>
#include <rviz/properties/bool_property.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/enum_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/tf_frame_property.h>
#include <rviz/properties/vector_property.h>
#include <rviz/visualization_manager.h>
// STD
#include <memory>
#include <mutex>
#include <unordered_map>
#include <variant>
namespace ufomap_ros::rviz_plugins
{
class UFOMapDisplay : public rviz::Display
{
Q_OBJECT
public:
enum VoxelType {
OCCUPIED,
FREE,
UNKNOWN,
};
enum ColoringMode {
VOXEL_COLOR,
X_AXIS_COLOR,
Y_AXIS_COLOR,
Z_AXIS_COLOR,
PROBABLILTY_COLOR,
FIXED_COLOR,
};
UFOMapDisplay();
virtual ~UFOMapDisplay();
// Overrides from rviz::Display
virtual void onInitialize() override;
virtual void update(float wall_dt, float ros_dt) override;
virtual void reset() override;
protected Q_SLOTS:
void updateQueueSize();
void updateTopic();
void updateDepth();
void updateOccupiedFreeThres();
void updateRenderMode();
void updateRenderStyle();
void updateColorMode();
void updateAlpha();
void updateScale();
void updateBBX();
void updateReset();
protected:
virtual void onEnable() override;
virtual void onDisable() override;
void subscribe();
void unsubscribe();
void mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const& msg);
void addPoint(
QHash<VoxelType, std::vector<std::vector<rviz::PointCloud::Point>>>& points,
QHash<VoxelType, std::vector<std::vector<float>>>& probabilities,
ufo::geometry::AABB const& bbx, VoxelType type, int min_depth,
rviz::PointCloud::Point point, double occupancy, unsigned int depth,
ufo::geometry::AABB const& aabb) const;
void updateInfo(double res, size_t num_leaf_nodes, size_t num_inner_nodes, size_t size);
void colorPoint(rviz::PointCloud::Point& point, ufo::map::Point3 const& min_value,
ufo::map::Point3 max_value, double probability, VoxelType type) const;
void setColor(double value, double min_value, double max_value, double color_factor,
rviz::PointCloud::Point& point) const;
void clear();
bool updateFromTF();
bool createMap(ufomap_msgs::UFOMapMetaData const& type);
bool checkMap(std::string const& type, double resolution,
ufo::map::DepthType depth_levels) const;
virtual void setTopic(QString const& topic, QString const& datatype) override;
std::string getStrVoxelType(VoxelType const& type) const;
protected:
std::variant<std::monostate, ufo::map::OccupancyMap, ufo::map::OccupancyMapColor> map_;
unsigned int num_messages_received_ = 0;
bool should_update_ = false;
std::mutex mutex_;
std::shared_ptr<message_filters::Subscriber<ufomap_msgs::UFOMapStamped>> sub_;
// Plugin properties
rviz::IntProperty* queue_size_property_;
rviz::RosTopicProperty* topic_property_;
QHash<VoxelType, rviz::BoolProperty*> render_type_;
rviz::Property* render_category_property_;
QHash<VoxelType, rviz::EnumProperty*> render_mode_;
QHash<VoxelType, rviz::EnumProperty*> coloring_property_;
QHash<VoxelType, rviz::ColorProperty*> color_property_;
QHash<VoxelType, rviz::FloatProperty*> color_factor_property_;
QHash<VoxelType, rviz::FloatProperty*> alpha_property_;
QHash<VoxelType, rviz::FloatProperty*> scale_property_;
rviz::IntProperty* depth_property_;
rviz::Property* occupancy_thres_category_property_;
rviz::IntProperty* occupied_thres_property_;
rviz::IntProperty* free_thres_property_;
rviz::BoolProperty* use_bbx_property_;
rviz::TfFrameProperty* tf_bbx_property_;
rviz::VectorProperty* min_bbx_property_;
rviz::VectorProperty* max_bbx_property_;
rviz::Property* info_property_;
rviz::StringProperty* resolution_property_;
rviz::StringProperty* num_leaf_nodes_property_;
rviz::StringProperty* num_inner_nodes_property_;
rviz::StringProperty* size_property_;
// Point clouds
QHash<VoxelType, std::vector<rviz::PointCloud>> clouds_;
std_msgs::Header header_;
};
} // namespace ufomap_ros::rviz_plugins
#endif // UFOMAP_RVIZ_PLUGINS_UFOMAP_DISPLAY_H
ufomap_ros/ufomap_rviz_plugins/package.xml
Executable
+43
View File
@@ -0,0 +1,43 @@
<?xml version="1.0"?>
<package format="2">
<name>ufomap_rviz_plugins</name>
<version>1.0.0</version>
<description>The ufomap_rviz_plugins package</description>
<maintainer email="dduberg@kth.se">Daniel Duberg</maintainer>
<license>BSD</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ufo_ros</url> -->
<author email="dduberg@kth.se">Daniel Duberg</author>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>rviz</depend>
<depend>ufomap_msgs</depend>
<depend>ufomap_ros</depend>
<depend>qtbase5-dev</depend>
<depend>libqt5-core</depend>
<depend>libqt5-widgets</depend>
<build_depend>ufomap</build_depend>
<doc_depend>doxygen</doc_depend>
<!-- <build_depend>qtbase5-dev</build_depend>
<build_export_depend>qtbase5-dev</build_export_depend>
<build_export_depend>libqt5-core</build_export_depend>
<exec_depend>libqt5-core</exec_depend>
<build_export_depend>libqt5-widgets</build_export_depend>
<exec_depend>libqt5-widgets</exec_depend> -->
<export>
<rviz plugin="${prefix}/plugin_description.xml"/>
</export>
</package>
ufomap_ros/ufomap_rviz_plugins/plugin_description.xml
Executable
+8
View File
@@ -0,0 +1,8 @@
<library path="lib/libufomap_rviz_plugins">
<class name="UFOMap" type="ufomap_ros::rviz_plugins::UFOMapDisplay" base_class_type="rviz::Display">
<description>
Displays a UFOMap.
</description>
<message_type>ufomap_msgs/UFOMapStamped</message_type>
</class>
</library>
ufomap_ros/ufomap_rviz_plugins/src/ufomap_display.cpp
Executable
+755
View File
@@ -0,0 +1,755 @@
/**
* UFO RViz integration
*
* @author D. Duberg, KTH Royal Institute of Technology, Copyright (c) 2020.
* @see https://github.com/UnknownFreeOccupied/ufomap_ros
* License: BSD 3
*
*/
/*
* BSD 3-Clause License
*
* Copyright (c) 2020, D. Duberg, KTH Royal Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
// UFO ROS
#include <ufomap_msgs/conversions.h>
#include <ufomap_rviz_plugins/ufomap_display.h>
#include <QLocale>
namespace ufomap_ros::rviz_plugins
{
UFOMapDisplay::UFOMapDisplay() : rviz::Display() {}
UFOMapDisplay::~UFOMapDisplay()
{
unsubscribe();
clouds_.clear();
if (scene_node_) {
scene_node_->detachAllObjects();
}
}
void UFOMapDisplay::onInitialize()
{
// *************
// Initialize all the plugin properties for rviz
// *************
topic_property_ = new rviz::RosTopicProperty(
"UFOMap Topic", "",
QString::fromStdString(ros::message_traits::datatype<ufomap_msgs::UFOMapStamped>()),
"ufomap_msgs::UFOMapStamped topic to subscribe to", this, SLOT(updateTopic()));
queue_size_property_ = new rviz::IntProperty(
"Queue size", 10, "Set the size of the incoming message queue", this,
SLOT(updateQueueSize()));
queue_size_property_->setMin(1);
info_property_ = new rviz::Property("Information", QVariant(), "", this);
resolution_property_ = new rviz::StringProperty(
"Resolution", "", "Resolution of the occupancy map", info_property_, nullptr, this);
num_leaf_nodes_property_ =
new rviz::StringProperty("# Leaf Nodes", "", "Number of leaf nodes in the octree",
info_property_, nullptr, this);
num_inner_nodes_property_ =
new rviz::StringProperty("# Inner Nodes", "", "Number of inner nodes in the octree",
info_property_, nullptr, this);
size_property_ = new rviz::StringProperty("Size", "", "Size of the octree",
info_property_, nullptr, this);
render_category_property_ = new rviz::Property("Voxel Rendering", QVariant(), "", this);
for (VoxelType const& type : {OCCUPIED, FREE, UNKNOWN}) {
QString default_coloring;
QColor default_color;
double default_alpha;
switch (type) {
case OCCUPIED:
default_coloring = "Voxel Color"; // FIXME: Should be Z_AXIS if not color UFOMap
default_color = Qt::blue;
default_alpha = 1.0;
break;
case FREE:
default_coloring = "Fixed";
default_color = Qt::green;
default_alpha = 0.03;
break;
case UNKNOWN:
default_coloring = "Fixed";
default_color = Qt::white;
default_alpha = 0.03;
}
QString type_str(getStrVoxelType(type).c_str());
auto it =
render_type_.insert(type, new rviz::BoolProperty(type_str, type == OCCUPIED, "",
render_category_property_,
SLOT(updateRenderMode()), this));
it.value()->setDisableChildrenIfFalse(true);
auto render_mode_it = render_mode_.insert(
type,
new rviz::EnumProperty("Style", "Flat Squares", "Select voxel rendering style",
it.value(), SLOT(updateRenderStyle()), this));
render_mode_it.value()->addOption("Points", rviz::PointCloud::RM_POINTS);
render_mode_it.value()->addOption("Squares", rviz::PointCloud::RM_SQUARES);
render_mode_it.value()->addOption("Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES);
render_mode_it.value()->addOption("Spheres", rviz::PointCloud::RM_SPHERES);
render_mode_it.value()->addOption("Tiles", rviz::PointCloud::RM_TILES);
render_mode_it.value()->addOption("Boxes", rviz::PointCloud::RM_BOXES);
auto coloring_it = coloring_property_.insert(
type,
new rviz::EnumProperty("Coloring", default_coloring, "Select voxel coloring mode",
it.value(), SLOT(updateColorMode()), this));
if (type == OCCUPIED) { // FIXME: Only if color UFOMap
coloring_it.value()->addOption("Voxel Color", OCCUPIED);
}
coloring_it.value()->addOption("X-Axis", X_AXIS_COLOR);
coloring_it.value()->addOption("Y-Axis", Y_AXIS_COLOR);
coloring_it.value()->addOption("Z-Axis", Z_AXIS_COLOR);
coloring_it.value()->addOption("Cell Probability", PROBABLILTY_COLOR);
coloring_it.value()->addOption("Fixed", FIXED_COLOR);
auto color_factor_it = color_factor_property_.insert(
type, new rviz::FloatProperty("Factor", 0.8, "", coloring_it.value(),
SLOT(updateColorMode()), this));
color_factor_it.value()->setMin(0.0);
color_factor_it.value()->setMax(1.0);
if ("Voxel Color" == default_coloring || "Cell Probability" == default_coloring ||
"Fixed" == default_coloring) {
color_factor_it.value()->hide();
}
auto color_it = color_property_.insert(
type, new rviz::ColorProperty("Color", default_color, "", coloring_it.value(),
SLOT(updateColorMode()), this));
if ("Voxel Color" == default_coloring || "X-Axis" == default_coloring ||
"Y-Axis" == default_coloring || "Z-Axis" == default_coloring) {
color_it.value()->hide();
}
auto alpha_it = alpha_property_.insert(
type,
new rviz::FloatProperty("Alpha", default_alpha, "Set voxel transparency alpha",
it.value(), SLOT(updateAlpha()), this));
alpha_it.value()->setMin(0.0);
alpha_it.value()->setMax(1.0);
auto scale_it = scale_property_.insert(
type, new rviz::FloatProperty("Scale", 1.0, "Set the voxel scale", it.value(),
SLOT(updateScale()), this));
}
depth_property_ = new rviz::IntProperty("Min. Depth", 0, "", this, SLOT(updateDepth()));
depth_property_->setMin(0);
depth_property_->setMax(21); // FIXME: Should not be hardcoded
occupancy_thres_category_property_ =
new rviz::Property("Occupancy Thresholds", QVariant(), "", this);
occupied_thres_property_ = new rviz::IntProperty(
"Occupied (%)", 50, "Set occupied threshold", occupancy_thres_category_property_,
SLOT(updateOccupiedFreeThres()), this);
free_thres_property_ = new rviz::IntProperty("Free (%)", 50, "Set free threshold",
occupancy_thres_category_property_,
SLOT(updateOccupiedFreeThres()), this);
occupied_thres_property_->setMin(0);
occupied_thres_property_->setMax(100);
free_thres_property_->setMin(0);
free_thres_property_->setMax(100);
use_bbx_property_ =
new rviz::BoolProperty("Use BBX", false, "", this, SLOT(updateBBX()), this);
use_bbx_property_->setDisableChildrenIfFalse(true);
tf_bbx_property_ = new rviz::TfFrameProperty(
"BBX Frame", rviz::TfFrameProperty::FIXED_FRAME_STRING,
"The frame to use for the BBX", use_bbx_property_, context_->getFrameManager(),
true, SLOT(updateBBX()), this);
min_bbx_property_ = new rviz::VectorProperty(
"Minimum",
Ogre::Vector3(-1000), // FIXME: Should not be hardcoded
"Defines the minimum BBX to display", use_bbx_property_, SLOT(updateBBX()), this);
max_bbx_property_ = new rviz::VectorProperty(
"Maximum",
Ogre::Vector3(1000), // FIXME: Should not be hardcoded
"Defines the maximum BBX to display", use_bbx_property_, SLOT(updateBBX()), this);
std::lock_guard<std::mutex> lock(mutex_);
for (VoxelType const& type : {OCCUPIED, FREE, UNKNOWN}) {
clouds_[type].resize(21); // FIXME: Should not be hardcoded
for (unsigned int d = 0; d < clouds_[type].size(); ++d) {
std::stringstream sname;
sname << getStrVoxelType(type) << " point cloud depth " << d;
clouds_[type][d].setName(sname.str());
clouds_[type][d].setRenderMode(rviz::PointCloud::RM_FLAT_SQUARES);
clouds_[type][d].setCastShadows(false);
scene_node_->attachObject(&clouds_[type][d]);
}
}
}
void UFOMapDisplay::update(float wall_dt, float ros_dt)
{
if (should_update_) {
std::lock_guard<std::mutex> lock(mutex_);
should_update_ = false;
if (!std::holds_alternative<std::monostate>(map_) &&
(render_type_[OCCUPIED]->getBool() || render_type_[FREE]->getBool() ||
render_type_[UNKNOWN]->getBool())) {
std::visit(
[this](auto& map) {
using T = std::decay_t<decltype(map)>;
if constexpr (!std::is_same_v<T, std::monostate>) {
QHash<VoxelType, std::vector<std::vector<rviz::PointCloud::Point>>> points;
QHash<VoxelType, std::vector<std::vector<float>>> probabilities;
for (VoxelType const& type : {OCCUPIED, FREE, UNKNOWN}) {
points[type].resize(clouds_[type].size());
probabilities[type].resize(clouds_[type].size());
}
ufo::map::Point3 min_value = map.getMin();
ufo::map::Point3 max_value = map.getMax();
if (use_bbx_property_->getBool()) {
Ogre::Vector3 position;
Ogre::Quaternion orientation;
context_->getFrameManager()->getTransform(
tf_bbx_property_->getFrameStd(), ros::Time(0), position, orientation);
Ogre::Vector3 min_bbx = min_bbx_property_->getVector() + position;
Ogre::Vector3 max_bbx = max_bbx_property_->getVector() + position;
for (int i = 0; i < 3; ++i) {
min_value[i] = std::max(min_value[i], static_cast<double>(min_bbx[i]));
max_value[i] = std::min(max_value[i], static_cast<double>(max_bbx[i]));
}
}
QHash<VoxelType, ufo::map::Point3> min_coord;
QHash<VoxelType, ufo::map::Point3> max_coord;
ufo::geometry::AABB aabb_bbx(min_value, max_value);
int min_depth = depth_property_->getInt();
for (auto it = map.beginLeaves(aabb_bbx, render_type_[OCCUPIED]->getBool(),
render_type_[FREE]->getBool(),
render_type_[UNKNOWN]->getBool(), false,
min_depth),
end = map.endLeaves();
it != end; ++it) {
VoxelType type;
if (it.isOccupied()) {
type = OCCUPIED;
} else if (it.isFree()) {
type = FREE;
} else {
type = UNKNOWN;
}
ufo::geometry::AABB it_aabb = it.getBoundingVolume();
ufo::map::Point3 it_min = it_aabb.getMin();
ufo::map::Point3 it_max = it_aabb.getMax();
if (min_coord.contains(type)) {
for (int i : {0, 1, 2}) {
min_coord[type][i] = std::min(min_coord[type][i], it_min[i]);
max_coord[type][i] = std::max(max_coord[type][i], it_max[i]);
}
} else {
min_coord[type] = it_min;
max_coord[type] = it_max;
}
rviz::PointCloud::Point point;
if constexpr (std::is_same_v<T, ufo::map::OccupancyMapColor>) {
point.setColor(it->color.r / 255.0, it->color.g / 255.0,
it->color.b / 255.0, it.getOccupancy());
}
addPoint(points, probabilities, aabb_bbx, type, min_depth, point,
it.getOccupancy(), it.getDepth(), it.getBoundingVolume());
}
for (VoxelType const& type : points.keys()) {
if (OCCUPIED != type ||
VOXEL_COLOR != coloring_property_[type]->getOptionInt()) {
for (int i : {0, 1, 2}) {
// Make sure it is not outside BBX
min_coord[type][i] = std::max(min_coord[type][i], min_value[i]);
max_coord[type][i] = std::min(max_coord[type][i], max_value[i]);
}
// Color points
for (size_t i = 0; i < points[type].size(); ++i) {
for (size_t j = 0; j < points[type][i].size(); ++j) {
colorPoint(points[type][i][j], min_coord[type], max_coord[type],
probabilities[type][i][j], type);
}
}
}
}
for (VoxelType const& type : clouds_.keys()) {
float scale = scale_property_[type]->getFloat();
for (size_t i = 0; i < clouds_[type].size(); ++i) {
clouds_[type][i].clear();
clouds_[type][i].setAlpha(alpha_property_[type]->getFloat());
if (i < map.getTreeDepthLevels()) {
float size = scale * map.getNodeSize(i);
clouds_[type][i].setDimensions(size, size, size);
}
if (!points[type][i].empty()) {
clouds_[type][i].addPoints(&points[type][i].front(),
points[type][i].size());
}
}
}
}
},
map_);
} else {
for (VoxelType const& type : clouds_.keys()) {
for (size_t i = 0; i < clouds_[type].size(); ++i) {
clouds_[type][i].clear();
}
}
}
updateRenderStyle(); // TODO: Should not have to call this here
}
updateFromTF();
}
void UFOMapDisplay::reset()
{
clear();
num_messages_received_ = 0;
setStatus(rviz::StatusProperty::Ok, "Messages", QString("0 UFOMap messages received"));
}
void UFOMapDisplay::updateQueueSize() { subscribe(); }
void UFOMapDisplay::updateTopic()
{
unsubscribe();
reset();
subscribe();
context_->queueRender();
}
void UFOMapDisplay::updateDepth() { should_update_ = true; }
void UFOMapDisplay::updateOccupiedFreeThres()
{
std::lock_guard<std::mutex> lock(mutex_);
// Fix floating point accuarcy problems
double occupied_thres = occupied_thres_property_->getInt() / 100.0;
double free_thres = free_thres_property_->getInt() / 100.0;
std::visit(
[this, occupied_thres, free_thres](auto& map) {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
map.setOccupiedFreeThres(occupied_thres, free_thres);
}
},
map_);
should_update_ = true;
}
void UFOMapDisplay::updateRenderMode() { should_update_ = true; }
void UFOMapDisplay::updateRenderStyle()
{
for (VoxelType const& type : render_mode_.keys()) {
for (auto& cloud : clouds_[type]) {
cloud.setRenderMode(
static_cast<rviz::PointCloud::RenderMode>(render_mode_[type]->getOptionInt()));
}
}
should_update_ = true;
}
void UFOMapDisplay::updateColorMode()
{
for (auto const& type : coloring_property_.keys()) {
switch (coloring_property_[type]->getOptionInt()) {
case VOXEL_COLOR:
color_factor_property_[type]->hide();
color_property_[type]->hide();
break;
case PROBABLILTY_COLOR:
case FIXED_COLOR:
color_factor_property_[type]->hide();
color_property_[type]->show();
break;
default:
color_factor_property_[type]->show();
color_property_[type]->hide();
break;
}
}
should_update_ = true;
}
void UFOMapDisplay::updateAlpha() { should_update_ = true; }
void UFOMapDisplay::updateScale() { should_update_ = true; }
void UFOMapDisplay::updateBBX() { should_update_ = true; }
void UFOMapDisplay::onEnable()
{
scene_node_->setVisible(true);
subscribe();
}
void UFOMapDisplay::onDisable()
{
scene_node_->setVisible(false);
unsubscribe();
clear();
}
void UFOMapDisplay::subscribe()
{
if (!isEnabled()) {
return;
}
try {
unsubscribe();
std::string const& topic(topic_property_->getStdString());
if (!topic.empty()) {
sub_.reset(new message_filters::Subscriber<ufomap_msgs::UFOMapStamped>());
sub_->subscribe(threaded_nh_, topic, queue_size_property_->getInt());
sub_->registerCallback(boost::bind(&UFOMapDisplay::mapCallback, this, _1));
}
} catch (ros::Exception& e) {
setStatus(rviz::StatusProperty::Error, "Topic",
(std::string("Error subscribing: ") + e.what()).c_str());
}
}
void UFOMapDisplay::unsubscribe()
{
clear();
try {
sub_.reset();
} catch (ros::Exception& e) {
setStatus(rviz::StatusProperty::Error, "Topic",
(std::string("Error unsubscribing: ") + e.what()).c_str());
}
}
void UFOMapDisplay::mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const& msg)
{
++num_messages_received_;
setStatus(rviz::StatusProperty::Ok, "Messages",
QString::number(num_messages_received_) + " UFOMap messages received");
setStatusStd(rviz::StatusProperty::Ok, "Type", msg->map.info.id.c_str());
header_ = msg->header;
if (!updateFromTF()) {
std::stringstream ss;
ss << "Failed to transform from frame [" << header_.frame_id << "] to frame ["
<< context_->getFrameManager()->getFixedFrame() << "]";
setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
return;
}
std::lock_guard<std::mutex> lock(mutex_);
if (!checkMap(msg->map.info.id, msg->map.info.resolution, msg->map.info.depth_levels)) {
if (!createMap(msg->map.info)) {
setStatusStd(rviz::StatusProperty::Error, "Message",
(std::string("Unknown UFOMap type '") + msg->map.info.id + "'").c_str());
return;
}
}
if (!std::visit(
[this, &msg](auto& map) -> bool {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
if (ufomap_msgs::msgToUfo(msg->map, map)) {
updateInfo(map.getResolution(), map.getNumLeafNodes(),
map.getNumInnerNodes(), map.memoryUsage());
return true;
}
return false;
}
return false;
},
map_)) {
setStatusStd(rviz::StatusProperty::Error, "Message", "Could not create UFOMap");
}
should_update_ = true;
}
void UFOMapDisplay::addPoint(
QHash<VoxelType, std::vector<std::vector<rviz::PointCloud::Point>>>& points,
QHash<VoxelType, std::vector<std::vector<float>>>& probabilities,
ufo::geometry::AABB const& bbx, VoxelType type, int min_depth,
rviz::PointCloud::Point point, double occupancy, unsigned int depth,
ufo::geometry::AABB const& aabb) const
{
// ufo::map::Point3 min = aabb.getMin();
// ufo::map::Point3 max = aabb.getMax();
// bool valid = depth <= min_depth || (ufo::geometry::intersects(bbx, min) &&
// ufo::geometry::intersects(bbx, max));
// if (valid) {
probabilities[type][depth].push_back(occupancy);
point.position.x = aabb.center[0];
point.position.y = aabb.center[1];
point.position.z = aabb.center[2];
points[type][depth].push_back(point);
return;
// }
// // Recurse down
// ufo::geometry::AABB child_aabb;
// child_aabb.half_size = aabb.half_size / 2.0;
// for (size_t i = 0; i < 8; ++i) {
// child_aabb.center[0] +=
// ((i & 1) ? child_aabb.half_size[0] : -child_aabb.half_size[0]);
// child_aabb.center[1] +=
// ((i & 2) ? child_aabb.half_size[1] : -child_aabb.half_size[1]);
// child_aabb.center[2] +=
// ((i & 4) ? child_aabb.half_size[2] : -child_aabb.half_size[2]);
// if (ufo::geometry::intersects(bbx, child_aabb)) {
// addPoint(points, probabilities, bbx, type, min_depth, point, occupancy, depth - 1,
// child_aabb);
// }
// }
}
void UFOMapDisplay::updateInfo(double res, size_t num_leaf_nodes, size_t num_inner_nodes,
size_t size)
{
QString res_str;
if (0.01 > res) {
res_str.setNum(res * 1000.0, 'g', 3);
res_str += " mm";
} else if (1 > res) {
res_str.setNum(res * 100.0, 'g', 3);
res_str += " cm";
} else {
res_str.setNum(res, 'g', 3);
res_str += " m";
}
resolution_property_->setString(res_str);
num_leaf_nodes_property_->setString(QString("%L1").arg(num_leaf_nodes));
num_inner_nodes_property_->setString(QString("%L1").arg(num_inner_nodes));
QLocale locale;
size_property_->setString(locale.formattedDataSize(size));
}
void UFOMapDisplay::colorPoint(rviz::PointCloud::Point& point,
ufo::map::Point3 const& min_value,
ufo::map::Point3 max_value, double probability,
VoxelType type) const
{
switch (coloring_property_[type]->getOptionInt()) {
case X_AXIS_COLOR:
setColor(point.position.x, min_value.x(), max_value.x(),
color_factor_property_[type]->getFloat(), point);
break;
case Y_AXIS_COLOR:
setColor(point.position.y, min_value.y(), max_value.y(),
color_factor_property_[type]->getFloat(), point);
break;
case Z_AXIS_COLOR:
setColor(point.position.z, min_value.z(), max_value.z(),
color_factor_property_[type]->getFloat(), point);
break;
case PROBABLILTY_COLOR: {
QColor color = color_property_[type]->getColor();
point.setColor(probability * (color.red() / 255.0),
probability * (color.green() / 255.0),
probability * (color.blue() / 255.0));
break;
}
case FIXED_COLOR: {
QColor color = color_property_[type]->getColor();
point.setColor(color.red() / 255.0, color.green() / 255.0, color.blue() / 255.0);
break;
}
}
}
void UFOMapDisplay::setColor(double value, double min_value, double max_value,
double color_factor, rviz::PointCloud::Point& point) const
{
int i;
double m, n, f;
double s = 1.0;
double v = 1.0;
double h = (1.0 - std::min(std::max((value - min_value) / (max_value - min_value), 0.0),
1.0)) *
color_factor;
h -= floor(h);
h *= 6;
i = floor(h);
f = h - i;
if (!(i & 1)) f = 1 - f; // if i is even
m = v * (1 - s);
n = v * (1 - s * f);
switch (i) {
case 6:
case 0:
point.setColor(v, n, m);
break;
case 1:
point.setColor(n, v, m);
break;
case 2:
point.setColor(m, v, n);
break;
case 3:
point.setColor(m, n, v);
break;
case 4:
point.setColor(n, m, v);
break;
case 5:
point.setColor(v, m, n);
break;
default:
point.setColor(1, 0.5, 0.5);
break;
}
}
void UFOMapDisplay::clear()
{
std::lock_guard<std::mutex> lock(mutex_);
map_.emplace<std::monostate>();
for (VoxelType const& type : clouds_.keys()) {
for (size_t i = 0; i < clouds_[type].size(); ++i) {
clouds_[type][i].clear();
}
}
// TODO: Implement
}
bool UFOMapDisplay::updateFromTF()
{
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (context_->getFrameManager()->getTransform(header_, position, orientation)) {
scene_node_->setOrientation(orientation);
scene_node_->setPosition(position);
return true;
}
return false;
}
bool UFOMapDisplay::createMap(ufomap_msgs::UFOMapMetaData const& info)
{
// Fix floating point accuarcy problems
double occupied_thres = occupied_thres_property_->getInt() / 100.0;
double free_thres = free_thres_property_->getInt() / 100.0;
// FIXME: Remove hardcoded
if ("occupancy_map" == info.id) {
map_.emplace<ufo::map::OccupancyMap>(info.resolution, info.depth_levels, true,
occupied_thres, free_thres);
return true;
} else if ("occupancy_map_color" == info.id) {
map_.emplace<ufo::map::OccupancyMapColor>(info.resolution, info.depth_levels, true,
occupied_thres, free_thres);
return true;
}
// TODO: Should remove VOXEL_COLOR option if none color UFOMap
return false;
}
bool UFOMapDisplay::checkMap(std::string const& type, double resolution,
ufo::map::DepthType depth_levels) const
{
return std::visit(
[this, &type, resolution, depth_levels](auto& map) -> bool {
if constexpr (!std::is_same_v<std::decay_t<decltype(map)>, std::monostate>) {
return map.getTreeType() == type && map.getResolution() == resolution &&
map.getTreeDepthLevels() == depth_levels;
}
return false;
},
map_);
}
void UFOMapDisplay::setTopic(QString const& topic, QString const& datatype)
{
topic_property_->setString(topic);
}
std::string UFOMapDisplay::getStrVoxelType(VoxelType const& type) const
{
switch (type) {
case OCCUPIED:
return "Occupied";
case FREE:
return "Free";
case UNKNOWN:
return "Unknown";
default:
return "";
}
}
} // namespace ufomap_ros::rviz_plugins
// Tell pluginlib about this class. Every class which should be
// loadable by pluginlib::ClassLoader must have these two lines
// compiled in its .cpp file, outside of any namespace scope.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(ufomap_ros::rviz_plugins::UFOMapDisplay, rviz::Display)
ufomap_ros/ufomap_srvs/CMakeLists.txt
+29
View File
@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.0.2)
project(ufomap_srvs)
find_package(catkin REQUIRED COMPONENTS
message_generation
ufomap_msgs
)
add_service_files(
FILES
ClearVolume.srv
GetMap.srv
Reset.srv
SaveMap.srv
)
generate_messages(
DEPENDENCIES
ufomap_msgs
)
catkin_package(
CATKIN_DEPENDS message_runtime ufomap_msgs
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
ufomap_ros/ufomap_srvs/package.xml
+65
View File
@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>ufomap_srvs</name>
<version>0.0.0</version>
<description>The ufomap_srvs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dduberg@todo.todo">dduberg</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>D</license>
<license>O</license>
<license>O</license>
<license>T</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ufomap_srvs</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<depend>ufomap_msgs</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
ufomap_ros/ufomap_srvs/srv/ClearVolume.srv
+10
View File
@@ -0,0 +1,10 @@
# Clear a part of the map, specified by a bounding volume.
# Sets all voxels inside the bounding volume to ~clamping_thres_min/free space.
# The bounding volume
ufomap_msgs/BoundingVolume bounding_volume
# The depth at which it should be cleared
uint8 depth
---
# If it was successful
bool success
ufomap_ros/ufomap_srvs/srv/GetMap.srv
+13
View File
@@ -0,0 +1,13 @@
# Get the complete map at a specified depth
# The depth of the map to return
uint8 depth
# If the message should be compressed
bool compress
# Bounding volume of which part of the map should be returned
ufomap_msgs/BoundingVolume bounding_volume
---
# If it was successful
bool success
# The map to be returned
ufomap_msgs/UFOMap map
ufomap_ros/ufomap_srvs/srv/Reset.srv
+9
View File
@@ -0,0 +1,9 @@
# Resets the complete map.
# The resolution of the new map
float64 new_resolution
# The number of depth levels in the new map
uint8 new_depth_levels
---
# If it was successful
bool success
ufomap_ros/ufomap_srvs/srv/SaveMap.srv
+15
View File
@@ -0,0 +1,15 @@
# Save the map to file
# Name of the file
string filename
# The depth of the map to save
uint8 depth
# If the file should be compressed
bool compress
# Compression level (higher number equals more compressed)
int32 compression_level
# Bounding volume of which part of the map should be saved
ufomap_msgs/BoundingVolume bounding_volume
---
# If it was successful
bool success