├── .clang-format
├── .clang-tidy
├── .gitignore
├── 3rdparty
└── 3es-core
│ └── 3esservermacros.h
├── CMakeLists.txt
├── LICENSE
├── OpenCL.md
├── clearance-performance-notes.md
├── clu
├── 3rdparty
│ └── CL
│ │ └── opencl.hpp
├── CMakeLists.txt
├── clu.cpp
├── clu.h
├── cluBuffer.h
├── cluConfig.in.h
├── cluConstraint.cpp
├── cluConstraint.h
├── cluKernel.cpp
├── cluKernel.h
├── cluProgram.cpp
└── cluProgram.h
├── cmake
├── ClangTidy.cmake
├── FindLASZIP.cmake
├── FindLIBLAS.cmake
├── FindOctomap.cmake
├── FindTBB.cmake
├── Findglm.cmake
├── LeakTrack.cmake
├── OhmCuda.cmake
├── OhmGTest.cmake
├── TextFileResource.cmake
├── TextFileResource.py
├── compilerSetup.cmake
├── doxyfile.in
├── doxygen.cmake
├── ohm-config.cmake
├── ohm-packages.cmake
├── tidy
│ ├── clang-tidy-names.yaml
│ └── wrap-clang-tidy.py
└── utils.cmake
├── docs
├── docglossary.md
├── docmain.h
├── docusage.md
├── docutils.md
├── docvoxellayers.md
└── gpu
│ ├── docgpualgorithm.md
│ └── docgpudetail.md
├── gputil
├── CMakeLists.txt
├── cl
│ ├── gpuApiExceptionCode.cpp
│ ├── gpuBuffer.cpp
│ ├── gpuBufferDetail.h
│ ├── gpuDevice.cpp
│ ├── gpuDeviceDetail.h
│ ├── gpuEvent.cpp
│ ├── gpuEventDetail.h
│ ├── gpuKernel.cpp
│ ├── gpuKernel2.h
│ ├── gpuKernelDetail.h
│ ├── gpuPinnedBuffer.cpp
│ ├── gpuPlatform2.h
│ ├── gpuProgram.cpp
│ ├── gpuProgramDetail.h
│ ├── gpuQueue.cpp
│ └── gpuQueueDetail.h
├── cuda
│ ├── cutil_atomic.h
│ ├── cutil_decl.h
│ ├── cutil_importcl.h
│ ├── cutil_math.h
│ ├── gpuApiExceptionCode.cpp
│ ├── gpuBuffer.cpp
│ ├── gpuBufferDetail.h
│ ├── gpuDevice.cpp
│ ├── gpuDeviceDetail.h
│ ├── gpuDirtyRegion
│ ├── gpuEvent.cpp
│ ├── gpuEventDetail.h
│ ├── gpuKernel.cpp
│ ├── gpuKernel2.h
│ ├── gpuKernelDetail.h
│ ├── gpuMemRegion.cpp
│ ├── gpuMemRegion.h
│ ├── gpuPinnedBuffer.cpp
│ ├── gpuPlatform2.h
│ ├── gpuProgram.cpp
│ ├── gpuProgramDetail.h
│ ├── gpuQueue.cpp
│ ├── gpuQueueDetail.h
│ └── ref.h
├── gpuApiException.cpp
├── gpuApiException.h
├── gpuBuffer.h
├── gpuConfig.in.h
├── gpuDevice.h
├── gpuDeviceInfo.h
├── gpuEvent.h
├── gpuEventList.cpp
├── gpuEventList.h
├── gpuException.cpp
├── gpuException.h
├── gpuKernel.h
├── gpuPinMode.h
├── gpuPinnedBuffer.h
├── gpuPlatform.h
├── gpuProgram.h
├── gpuQueue.h
├── gpuThrow.cpp
├── gpuThrow.h
├── gpuVersion.h
└── gpu_ext.h
├── logutil
├── CMakeLists.txt
├── LogUtil.cpp
├── LogUtil.h
├── LogUtilConfig.in.h
├── Logger.cpp
├── Logger.h
└── LoggerDetail.h
├── ohm.natvis
├── ohm
├── Aabb.h
├── CMakeLists.txt
├── CalculateSegmentKeys.cpp
├── CalculateSegmentKeys.h
├── ClearingPattern.cpp
├── ClearingPattern.h
├── CompareMaps.cpp
├── CompareMaps.h
├── CopyUtil.cpp
├── CopyUtil.h
├── CovarianceVoxel.cpp
├── CovarianceVoxel.h
├── CovarianceVoxelCompute.h
├── DataType.cpp
├── DataType.h
├── DebugDraw.cpp
├── DebugDraw.h
├── DebugIDs.h
├── DefaultLayer.cpp
├── DefaultLayer.h
├── Density.cpp
├── Density.h
├── Key.cpp
├── Key.h
├── KeyHash.cpp
├── KeyHash.h
├── KeyList.cpp
├── KeyList.h
├── KeyRange.cpp
├── KeyRange.h
├── KeyStream.h
├── LineKeysQuery.cpp
├── LineKeysQuery.h
├── LineQuery.cpp
├── LineQuery.h
├── LineWalk.h
├── LineWalkCompute.h
├── MapChunk.cpp
├── MapChunk.h
├── MapChunkFlag.h
├── MapCoord.h
├── MapFlag.cpp
├── MapFlag.h
├── MapInfo.cpp
├── MapInfo.h
├── MapLayer.cpp
├── MapLayer.h
├── MapLayout.cpp
├── MapLayout.h
├── MapLayoutMatch.h
├── MapProbability.h
├── MapRegion.cpp
├── MapRegion.h
├── MapRegionCache.cpp
├── MapRegionCache.h
├── MapSerialise.cpp
├── MapSerialise.h
├── Mapper.cpp
├── Mapper.h
├── MappingProcess.cpp
├── MappingProcess.h
├── Mutex.cpp
├── Mutex.h
├── NdtMap.cpp
├── NdtMap.h
├── NdtMode.cpp
├── NdtMode.h
├── NearestNeighbours.cpp
├── NearestNeighbours.h
├── OccupancyMap.cpp
├── OccupancyMap.h
├── OccupancyType.cpp
├── OccupancyType.h
├── OccupancyUtil.cpp
├── OccupancyUtil.h
├── OhmConfig.in.h
├── Query.cpp
├── Query.h
├── QueryFlag.h
├── RayFilter.cpp
├── RayFilter.h
├── RayFlag.h
├── RayMapper.cpp
├── RayMapper.h
├── RayMapperNdt.cpp
├── RayMapperNdt.h
├── RayMapperOccupancy.cpp
├── RayMapperOccupancy.h
├── RayMapperSecondarySample.cpp
├── RayMapperSecondarySample.h
├── RayMapperTrace.cpp
├── RayMapperTrace.h
├── RayMapperTsdf.cpp
├── RayMapperTsdf.h
├── RayPattern.cpp
├── RayPattern.h
├── RayPatternConical.cpp
├── RayPatternConical.h
├── RaysQuery.cpp
├── RaysQuery.h
├── Stream.cpp
├── Stream.h
├── Trace.cpp
├── Trace.h
├── Voxel.cpp
├── Voxel.h
├── VoxelBlock.cpp
├── VoxelBlock.h
├── VoxelBlockCompressionQueue.cpp
├── VoxelBlockCompressionQueue.h
├── VoxelBuffer.cpp
├── VoxelBuffer.h
├── VoxelData.h
├── VoxelIncident.h
├── VoxelIncidentCompute.h
├── VoxelLayout.cpp
├── VoxelLayout.h
├── VoxelMean.h
├── VoxelMeanCompute.h
├── VoxelOccupancy.h
├── VoxelOccupancyCompute.h
├── VoxelSecondarySample.h
├── VoxelTouchTime.h
├── VoxelTouchTimeCompute.h
├── VoxelTsdf.cpp
├── VoxelTsdf.h
├── VoxelTsdfCompute.h
├── private
│ ├── ClearingPatternDetail.h
│ ├── LineKeysQueryDetail.h
│ ├── LineQueryDetail.h
│ ├── MapLayerDetail.h
│ ├── MapLayoutDetail.h
│ ├── MapperDetail.h
│ ├── NdtMapDetail.h
│ ├── NearestNeighboursDetail.h
│ ├── OccupancyMapDetail.cpp
│ ├── OccupancyMapDetail.h
│ ├── OccupancyQueryAlg.h
│ ├── QueryDetail.h
│ ├── RayPatternDetail.h
│ ├── RaysQueryDetail.h
│ ├── SerialiseUtil.h
│ ├── VoxelAlgorithms.cpp
│ ├── VoxelAlgorithms.h
│ ├── VoxelBlockCompressionQueueDetail.h
│ └── VoxelLayoutDetail.h
└── serialise
│ ├── MapSerialiseV0.1.cpp
│ ├── MapSerialiseV0.1.h
│ ├── MapSerialiseV0.2.cpp
│ ├── MapSerialiseV0.2.h
│ ├── MapSerialiseV0.4.cpp
│ ├── MapSerialiseV0.4.h
│ ├── MapSerialiseV0.5.cpp
│ ├── MapSerialiseV0.5.h
│ ├── MapSerialiseV0.cpp
│ └── MapSerialiseV0.h
├── ohmapp
├── CMakeLists.txt
├── DataSource.cpp
├── DataSource.h
├── MapHarness.cpp
├── MapHarness.h
├── OhmAppConfig.in.h
├── OhmAppCpu.cpp
├── OhmAppCpu.h
├── OhmAppGpu.cpp
├── OhmAppGpu.h
├── OhmAppGpuConfig.in.h
├── SlamIOSource.cpp
├── SlamIOSource.h
└── ohmappmain.inl
├── ohmgpu
├── CMakeLists.txt
├── ClearanceProcess.cpp
├── ClearanceProcess.h
├── GpuCache.cpp
├── GpuCache.h
├── GpuCachePostSyncHandler.h
├── GpuCacheStats.h
├── GpuKey.h
├── GpuLayerCache.cpp
├── GpuLayerCache.h
├── GpuLayerCacheParams.cpp
├── GpuLayerCacheParams.h
├── GpuMap.cpp
├── GpuMap.h
├── GpuNdtMap.cpp
├── GpuNdtMap.h
├── GpuTransformSamples.cpp
├── GpuTransformSamples.h
├── GpuTsdfMap.cpp
├── GpuTsdfMap.h
├── LineKeysQueryGpu.cpp
├── LineKeysQueryGpu.h
├── LineQueryGpu.cpp
├── LineQueryGpu.h
├── OhmGpu.cpp
├── OhmGpu.h
├── OhmGpuConfig.in.h
├── RayItem.h
├── RaysQueryGpu.cpp
├── RaysQueryGpu.h
├── gpu
│ ├── AdjustNdt.cl
│ ├── AdjustOccupancy.cl
│ ├── CovarianceHitNdt.cl
│ ├── CovarianceHitNdt.cu
│ ├── CovarianceHitNdt_h.cl
│ ├── LineKeys.cl
│ ├── LineKeys.cu
│ ├── LineWalk.cl
│ ├── LineWalkMarkers.cl
│ ├── RaysQuery.cl
│ ├── RaysQuery.cu
│ ├── RaysQueryResult.h
│ ├── RegionUpdate.cl
│ ├── RegionUpdate.cu
│ ├── RegionUpdateNdt.cu
│ ├── Regions.cl
│ ├── RoiRangeFill.cl
│ ├── RoiRangeFill.cu
│ ├── TransformSamples.cl
│ ├── TransformSamples.cu
│ ├── Traversal.cl
│ ├── TsdfUpdate.cl
│ ├── TsdfUpdate.cu
│ ├── VoxelIncident.cl
│ └── VoxelMean.cl
└── private
│ ├── ClearanceProcessDetail.cpp
│ ├── ClearanceProcessDetail.h
│ ├── GpuMapDetail.cpp
│ ├── GpuMapDetail.h
│ ├── GpuNdtMapDetail.h
│ ├── GpuProgramRef.cpp
│ ├── GpuProgramRef.h
│ ├── GpuTransformSamplesDetail.h
│ ├── GpuTsdfMapDetail.h
│ ├── LineKeysQueryDetailGpu.h
│ ├── LineQueryDetailGpu.h
│ ├── RaysQueryDetailGpu.cpp
│ ├── RaysQueryDetailGpu.h
│ ├── RoiRangeFill.cpp
│ └── RoiRangeFill.h
├── ohmheightmap
├── 3rdparty
│ └── delaunator.hpp
├── CMakeLists.txt
├── Heightmap.cpp
├── Heightmap.h
├── HeightmapMesh.cpp
├── HeightmapMesh.h
├── HeightmapMode.cpp
├── HeightmapMode.h
├── HeightmapSerialise.cpp
├── HeightmapSerialise.h
├── HeightmapUtil.cpp
├── HeightmapUtil.h
├── HeightmapVoxel.cpp
├── HeightmapVoxel.h
├── HeightmapVoxelType.h
├── OhmHeightmapConfig.in.h
├── PlaneFillLayeredWalker.cpp
├── PlaneFillLayeredWalker.h
├── PlaneFillWalker.cpp
├── PlaneFillWalker.h
├── PlaneWalkVisitMode.h
├── PlaneWalker.cpp
├── PlaneWalker.h
├── TriangleEdge.h
├── TriangleNeighbours.h
├── UpAxis.h
└── private
│ ├── HeightmapDetail.cpp
│ ├── HeightmapDetail.h
│ ├── HeightmapOperations.cpp
│ └── HeightmapOperations.h
├── ohmheightmapimage
├── CMakeLists.txt
├── HeightmapImage.cpp
├── HeightmapImage.h
└── OhmHeightmapImageConfig.in.h
├── ohmtools
├── CMakeLists.txt
├── OhmCloud.cpp
├── OhmCloud.h
├── OhmGen.cpp
├── OhmGen.h
└── OhmToolsConfig.in.h
├── ohmutil
├── 3rdparty
│ ├── cxxopts
│ │ └── cxxopts.hpp
│ └── ska
│ │ ├── bytell_hash_map.hpp
│ │ ├── flat_hash_map.hpp
│ │ ├── ska_sort.hpp
│ │ └── unordered_map.hpp
├── CMakeLists.txt
├── Colour.cpp
├── Colour.h
├── GlmStream.h
├── OhmUtilConfig.in.h
├── Options.h
├── PlyMesh.cpp
├── PlyMesh.h
├── PlyPointStream.cpp
├── PlyPointStream.h
├── Profile.cpp
├── Profile.h
├── ProfileMarker.cpp
├── ProfileMarker.h
├── ProgressMonitor.cpp
├── ProgressMonitor.h
├── SafeIO.cpp
├── SafeIO.h
├── ScopedTimeDisplay.cpp
├── ScopedTimeDisplay.h
├── VectorHash.h
└── p2p.h
├── package.xml
├── readme.md
├── scripts
├── ohm-parse-timing.py
├── ohm-timing-run.py
└── setup.cfg
├── slamio
├── CMakeLists.txt
├── DataChannel.h
├── PointCloudReader.cpp
├── PointCloudReader.h
├── PointCloudReaderPdal.cpp
├── PointCloudReaderPdal.h
├── PointCloudReaderPly.cpp
├── PointCloudReaderPly.h
├── PointCloudReaderTraj.cpp
├── PointCloudReaderTraj.h
├── PointCloudReaderXyz.cpp
├── PointCloudReaderXyz.h
├── Points.cpp
├── Points.h
├── SlamCloudLoader.cpp
├── SlamCloudLoader.h
├── SlamIO.cpp
├── SlamIO.h
├── SlamIOConfig.in.h
├── pdal
│ ├── PointStream.cpp
│ └── PointStream.h
└── rply
│ ├── LICENSE
│ ├── rply.c
│ ├── rply.h
│ └── rplyfile.h
├── tests
├── CMakeLists.txt
├── data
│ ├── CMakeLists.txt
│ ├── readme.md
│ ├── test-map.0.1.0.ohm
│ ├── test-map.0.2.0.ohm
│ ├── test-map.0.3.0.ohm
│ ├── test-map.0.4.0.ohm
│ └── test-map.0.ohm
├── gputiltest
│ ├── CMakeLists.txt
│ ├── GpuBufferTest.cpp
│ ├── GpuDeviceTest.cpp
│ ├── KernelTest.cpp
│ ├── TestMain.cpp
│ ├── cuda
│ │ └── matrix_kernel.cu
│ └── matrix.cl
├── ohmtest
│ ├── CMakeLists.txt
│ ├── CompressionTests.cpp
│ ├── CopyTests.cpp
│ ├── IncidentsTests.cpp
│ ├── KeyTests.cpp
│ ├── LayoutTests.cpp
│ ├── LineQueryTests.cpp
│ ├── LineWalkTests.cpp
│ ├── MapTests.cpp
│ ├── MathsTests.cpp
│ ├── NdtTests.cpp
│ ├── OhmConfig.in.h
│ ├── OhmTestConfig.in.h
│ ├── RayPatternTests.cpp
│ ├── RayValidation.cpp
│ ├── RayValidation.h
│ ├── RaysQueryTests.cpp
│ ├── SecondarySampleTests.cpp
│ ├── SerialisationTests.cpp
│ ├── TestMain.cpp
│ ├── TouchTimeTests.cpp
│ ├── TraversalTests.cpp
│ ├── TsdfTests.cpp
│ └── VoxelMeanTests.cpp
├── ohmtestcommon
│ ├── CMakeLists.txt
│ ├── CovarianceTestUtil.cpp
│ ├── CovarianceTestUtil.h
│ ├── OhmTestUtil.cpp
│ ├── OhmTestUtil.h
│ ├── RayPatternTestUtil.h
│ ├── TraversalTest.cpp
│ ├── TraversalTest.h
│ ├── WalkSegmentKeysLegacy.cpp
│ └── WalkSegmentKeysLegacy.h
├── ohmtestgpu
│ ├── CMakeLists.txt
│ ├── GpuCopyTests.cpp
│ ├── GpuIncidentsTests.cpp
│ ├── GpuLineKeysTests.cpp
│ ├── GpuLineQueryTests.cpp
│ ├── GpuMapTest.cpp
│ ├── GpuMapperTests.cpp
│ ├── GpuNdtTests.cpp
│ ├── GpuRangesTests.cpp
│ ├── GpuRayPatternTests.cpp
│ ├── GpuRaysQueryTests.cpp
│ ├── GpuSerialisationTests.cpp
│ ├── GpuTestMain.cpp
│ ├── GpuTests.cpp
│ ├── GpuTouchTimeTests.cpp
│ ├── GpuTraversalTests.cpp
│ ├── GpuTsdfTests.cpp
│ └── GpuVoxelMeanTests.cpp
├── ohmtestheightmap
│ ├── CMakeLists.txt
│ └── HeightmapTests.cpp
└── slamiotest
│ ├── CMakeLists.txt
│ ├── Loader.cpp
│ └── SlamCloudLoader.cpp
├── todo.md
├── utils
├── CMakeLists.txt
├── ohm2ply
│ ├── CMakeLists.txt
│ └── ohm2ply.cpp
├── ohmcmp
│ ├── CMakeLists.txt
│ └── ohmcmp.cpp
├── ohmfilter
│ ├── CMakeLists.txt
│ └── ohmfilter.cpp
├── ohmheightmap
│ ├── CMakeLists.txt
│ └── ohmheightmap.cpp
├── ohmhm2img
│ ├── CMakeLists.txt
│ └── ohmhm2img.cpp
├── ohminfo
│ ├── CMakeLists.txt
│ └── ohminfo.cpp
├── ohmpop
│ ├── CMakeLists.txt
│ ├── OhmPopConfig.in.h
│ ├── ohmpopmaincpu.cpp
│ └── ohmpopmaingpu.cpp
├── ohmprob
│ ├── CMakeLists.txt
│ └── ohmprob.cpp
├── ohmquery
│ ├── CMakeLists.txt
│ ├── OhmQueryConfig.in.h
│ └── ohmquery.cpp
└── ohmsubmap
│ ├── CMakeLists.txt
│ └── ohmsubmap.cpp
└── vcpkg.json
/.gitignore:
--------------------------------------------------------------------------------
1 | .vscode/
2 | .vs/
3 | build*/
4 | # Visual Studio output directory for CMake projects.
5 | out/
6 | vcpkg_installed/
7 |
--------------------------------------------------------------------------------
/clu/cluConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef CLUCONFIG_H
6 | #define CLUCONFIG_H
7 |
8 | #ifndef _USE_MATH_DEFINES
9 | #define _USE_MATH_DEFINES
10 | #endif // _USE_MATH_DEFINES
11 | #ifndef NOMINMAX
12 | #define NOMINMAX
13 | #endif // NOMINMAX
14 |
15 | #ifdef _MSC_VER
16 | // Avoid dubious security warnings for plenty of legitimate code
17 | #ifndef _SCL_SECURE_NO_WARNINGS
18 | #define _SCL_SECURE_NO_WARNINGS
19 | #endif // _SCL_SECURE_NO_WARNINGS
20 | #ifndef _CRT_SECURE_NO_WARNINGS
21 | #define _CRT_SECURE_NO_WARNINGS
22 | #endif // _CRT_SECURE_NO_WARNINGS
23 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
24 | #endif // _MSC_VER
25 |
26 | // clang-format off
27 | #define CL_HPP_MINIMUM_OPENCL_VERSION 120
28 | #define CL_HPP_TARGET_OPENCL_VERSION @CLU_TARGET_OPENCL_VERSION@
29 | // clang-format on
30 |
31 | #endif // CLUCONFIG_H
32 |
--------------------------------------------------------------------------------
/cmake/FindLASZIP.cmake:
--------------------------------------------------------------------------------
1 | # This module searches LASzip and defines
2 | # LASZIP_LIBRARIES - link libraries
3 | # LASZIP_FOUND, if false, do not try to link
4 | # LASZIP_INCLUDE_DIR, where to find the headers
5 | #
6 | # $LASZIP_DIR is an environment variable that would
7 | # correspond to the ./configure --prefix=$LASZIP_DIR
8 |
9 | find_path(LASZIP_INCLUDE_DIR laszip.hpp HINTS ENV LASZIP_DIR PATH_SUFFIXES include/laszip)
10 |
11 | find_library(LASZIP_LIBRARY_DEBUG NAMES laszipd HINTS ENV LASZIP_DIR PATH_SUFFIXES lib)
12 | find_library(LASZIP_LIBRARY_RELEASE NAMES laszip HINTS ENV LASZIP_DIR PATH_SUFFIXES lib)
13 |
14 | find_file(LASZIP_RUNTIME_DEBUG NAMES laszipd${CMAKE_SHARED_LIBRARY_SUFFIX} HINTS ENV LASZIP_DIR PATH_SUFFIXES lib bin)
15 | find_file(LASZIP_RUNTIME_RELEASE NAMES laszip${CMAKE_SHARED_LIBRARY_SUFFIX} HINTS ENV LASZIP_DIR PATH_SUFFIXES lib bin)
16 |
17 | set(LASZIP_RUNTIME_DIRS)
18 | if(LASZIP_RUNTIME_DEBUG)
19 | get_filename_component(RUNTIME_DIR "${LASZIP_RUNTIME_DEBUG}" DIRECTORY)
20 | list(APPEND LASZIP_RUNTIME_DIRS "${RUNTIME_DIR}")
21 | endif(LASZIP_RUNTIME_DEBUG)
22 | if(LASZIP_RUNTIME_RELEASE)
23 | get_filename_component(RUNTIME_DIR "${LASZIP_RUNTIME_RELEASE}" DIRECTORY)
24 | list(APPEND LASZIP_RUNTIME_DIRS "${RUNTIME_DIR}")
25 | endif(LASZIP_RUNTIME_RELEASE)
26 | if(LASZIP_RUNTIME_DIRS)
27 | list(REMOVE_DUPLICATES LASZIP_RUNTIME_DIRS)
28 | endif(LASZIP_RUNTIME_DIRS)
29 | set(LASZIP_RUNTIME_DIRS "${LASZIP_RUNTIME_DIRS}" CACHE PATH "LASZIP runtime directories")
30 |
31 | if(LASZIP_LIBRARY_DEBUG)
32 | list(APPEND LASZIP_LIBRARIES debug ${LASZIP_LIBRARY_DEBUG})
33 | if(LASZIP_LIBRARY_RELEASE)
34 | list(APPEND LASZIP_LIBRARIES optimized ${LASZIP_LIBRARY_RELEASE})
35 | endif(LASZIP_LIBRARY_RELEASE)
36 | else(LASZIP_LIBRARY_DEBUG)
37 | list(APPEND LASZIP_LIBRARIES ${LASZIP_LIBRARY_RELEASE})
38 | endif(LASZIP_LIBRARY_DEBUG)
39 |
40 | # handle the QUIETLY and REQUIRED arguments and set LASZIP_FOUND to TRUE if
41 | # all listed variables are TRUE
42 | include(FindPackageHandleStandardArgs)
43 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(LASZIP REQUIRED_VARS LASZIP_LIBRARIES LASZIP_INCLUDE_DIR)
44 |
45 | if(LASZIP_FOUND)
46 | mark_as_advanced(LASZIP_INCLUDE_DIR LASZIP_LIBRARIES LASZIP_RUNTIME_DIRS LASZIP_RUNTIME_DEBUG LASZIP_RUNTIME_RELEASE)
47 | endif(LASZIP_FOUND)
48 |
--------------------------------------------------------------------------------
/cmake/Findglm.cmake:
--------------------------------------------------------------------------------
1 |
2 | # FindGLM (OpenGL Mathematics library)
3 | # --------
4 | #
5 | # try to find include files.
6 | #
7 | # IMPORTED Targets
8 | # ^^^^^^^^^^^^^^^^
9 | #
10 | # This module defines the :prop_tgt:`IMPORTED` targets:
11 | #
12 | # ``glm::glm``
13 | # Defined if the system has OpenGL Mathematics headers.
14 | #
15 | # Result Variables
16 | # ^^^^^^^^^^^^^^^^
17 | #
18 | # This module sets the following variables:
19 | #
20 | # ::
21 | #
22 | # GLM_INCLUDE_DIRS, where to find glm/glm.hpp, etc.
23 | # glm_FOUND, If false, do not try to use GLM.
24 | # glm::glm, import target
25 |
26 | # Try config file first. This particular file is really only for supporting older GLM such as found on Ubuntu 18.04.
27 | # Newer GLM with config.cmake files is strongly recommended.
28 | find_package(glm QUIET CONFIG)
29 | if(NOT glm_FOUND)
30 | find_path(GLM_INCLUDE_DIRS NAMES glm/glm.hpp
31 | PATHS $ENV{GLM_ROOT_PATH}/include ${GLM_ROOT_PATH}/include
32 | /usr/local/include
33 | )
34 |
35 | mark_as_advanced(
36 | GLM_INCLUDE_DIRS
37 | )
38 | endif()
39 |
40 | # Note: glmConfig.cmake on Ubuntu 18.04 does not provide a target glm::glm.
41 | # 20.04 does.
42 | if(NOT TARGET glm::glm)
43 | add_library(glm::glm INTERFACE IMPORTED)
44 | set_target_properties(glm::glm PROPERTIES
45 | INTERFACE_INCLUDE_DIRECTORIES "${GLM_INCLUDE_DIRS}"
46 | )
47 | else()
48 | get_target_property(GLM_INCLUDE_DIRS glm::glm INTERFACE_INCLUDE_DIRECTORIES)
49 | endif()
50 |
51 | include(FindPackageHandleStandardArgs)
52 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(glm REQUIRED_VARS GLM_INCLUDE_DIRS)
53 |
--------------------------------------------------------------------------------
/cmake/ohm-packages.cmake:
--------------------------------------------------------------------------------
1 | # Copyright (c) 2017
2 | # Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | # ABN 41 687 119 230
4 | #
5 | # Author: Kazys Stepanas
6 |
7 | # Configure variables and include packages.
8 |
9 | # Find packages
10 | find_package(Threads)
11 | find_package(glm REQUIRED)
12 |
13 | if(OHM_FEATURE_THREADS)
14 | find_package(TBB CONFIG)
15 | endif(OHM_FEATURE_THREADS)
16 |
17 | if(NOT OHM_BUILD_SHARED)
18 | # We only need to propagate these packages for static ohm builds.
19 | if(OHM_TES_DEBUG)
20 | find_package(3es)
21 | endif(OHM_TES_DEBUG)
22 |
23 | if(OHM_FEATURE_PDAL)
24 | find_package(PDAL REQUIRED)
25 | endif(OHM_FEATURE_PDAL)
26 |
27 | find_package(ZLIB)
28 | endif(NOT OHM_BUILD_SHARED)
29 |
--------------------------------------------------------------------------------
/docs/docglossary.md:
--------------------------------------------------------------------------------
1 |
9 |
10 |
11 |
12 | @page docglossary OHM Glossary
13 |
14 |
15 | # OHM Glossary
16 |
17 | | Term | Description |
18 | | --------- | ------------------------------------------------------------------------------- |
19 | | atomic | Generally, atomic CPU/GPU instructions, executed as a single operation. |
20 | | CAS | Atomic Compare and Swap operation |
21 | | Chunk | Related to region, normally focusing on the memory layout of the region. |
22 | | GP GPU | General Purpose GPU programming |
23 | | NDT | Normal distribution transform, normally implying NDT use in occupancy maps. |
24 | | Occupancy | Probabilistic determination of whether a voxel is occupied, free or unobserved. |
25 | | Region | A region of space containing a fixed number of voxels. |
26 | | TSDF | Truncated signed distance fields. |
27 | | Voxel | A 3D cube (normally) division of space. |
28 |
--------------------------------------------------------------------------------
/docs/docmain.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 |
5 | // Author: Kazys Stepanas
6 | #ifndef DOCMAIN_H
7 | #define DOCMAIN_H
8 |
9 | /*!
10 | @mainpage Occupancy Homogeneous Map documentation
11 |
12 | The ohm library is a probabilistic voxel occupancy map supporting fast GPU based population and operations and normal
13 | distribution transform semantics. The ohm library defines an occupancy map consisting of regions or chunks of
14 | homogeneous voxels, arranged in contiguous memory blocks. This homogeneous voxel layout, rather than an octree layout,
15 | supports fast GPU based map population using OpenCL and CUDA.
16 |
17 | - @subpage docglossary
18 | - @subpage docusage
19 | - @subpage docutils
20 | - @subpage docgpualgorithm
21 | - @subpage docgpudetail
22 | - @subpage docvoxellayers
23 |
24 | */
25 |
26 | #endif // DOCMAIN_H
27 |
--------------------------------------------------------------------------------
/gputil/cl/gpuApiExceptionCode.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "gpuApiException.h"
7 |
8 | #include
9 |
10 | namespace gputil
11 | {
12 | const char *ApiException::errorCodeString(int error_code)
13 | {
14 | return clu::errorCodeString(error_code);
15 | }
16 | } // namespace gputil
--------------------------------------------------------------------------------
/gputil/cl/gpuBufferDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUBUFFERDETAIL_H
8 | #define GPUBUFFERDETAIL_H
9 |
10 | #include "gpuConfig.h"
11 |
12 | #include
13 |
14 | #include "gputil/gpuDevice.h"
15 |
16 | #include
17 |
18 | namespace gputil
19 | {
20 | class Event;
21 | class Queue;
22 |
23 | struct BufferDetail
24 | {
25 | Device device;
26 | cl::Buffer buffer;
27 | size_t requested_size = 0;
28 | unsigned flags = 0;
29 | unsigned request_flags = 0;
30 | };
31 |
32 | uint8_t *pin(BufferDetail &imp, PinMode mode);
33 | /// Unpin memory. Supports asynchronous unpinning by providing a queue, in which case
34 | /// a @p completion event is also recommended.
35 | void unpin(BufferDetail &imp, void *pinned_ptr, Queue *queue = nullptr, Event *block_on = nullptr,
36 | Event *completion = nullptr);
37 | } // namespace gputil
38 |
39 | #endif // GPUBUFFERDETAIL_H
40 |
--------------------------------------------------------------------------------
/gputil/cl/gpuDeviceDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUDEVICEDETAIL_H
8 | #define GPUDEVICEDETAIL_H
9 |
10 | #include "gpuConfig.h"
11 |
12 | #include "gputil/gpuDeviceInfo.h"
13 | #include "gputil/gpuQueue.h"
14 |
15 | #include
16 |
17 | namespace gputil
18 | {
19 | struct DeviceDetail
20 | {
21 | cl::Context context;
22 | cl::Device device;
23 | /// The default queue object. We need a Queue object rather than just a cl::CommandQueue because we add data members.
24 | Queue default_queue;
25 | DeviceInfo info;
26 | std::string description;
27 | std::string search_paths;
28 | std::string extensions; ///< OpenCL supported extension string.
29 | unsigned debug = 0;
30 | };
31 | } // namespace gputil
32 |
33 | #endif // GPUDEVICEDETAIL_H
34 |
--------------------------------------------------------------------------------
/gputil/cl/gpuEventDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUEVENTDETAIL_H
7 | #define GPUEVENTDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include
12 |
13 | namespace gputil
14 | {
15 | struct EventDetail
16 | {
17 | cl_event event;
18 | };
19 | } // namespace gputil
20 |
21 | #endif // GPUEVENTDETAIL_H
22 |
--------------------------------------------------------------------------------
/gputil/cl/gpuKernelDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUKERNELDETAIL_H
7 | #define GPUKERNELDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "../gpuDevice.h"
12 | #include "../gpuProgram.h"
13 |
14 | #include
15 |
16 | #include
17 |
18 | namespace gputil
19 | {
20 | struct KernelDetail
21 | {
22 | clu::Kernel kernel;
23 | Program program;
24 | std::vector> local_mem_args;
25 | bool auto_error_checking = true;
26 | };
27 | } // namespace gputil
28 |
29 | #endif // GPUKERNELDETAIL_H
30 |
--------------------------------------------------------------------------------
/gputil/cl/gpuPlatform2.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUPLATFORM2_H
7 | #define GPUPLATFORM2_H
8 |
9 | #if defined(__APPLE__) || defined(__MACOSX)
10 | #include
11 | #else
12 | #include
13 | #endif // !__APPLE__
14 |
15 | namespace gputil
16 | {
17 | typedef cl_char char1; // NOLINT
18 | typedef cl_char2 char2; // NOLINT
19 | typedef cl_char3 char3; // NOLINT
20 | typedef cl_char4 char4; // NOLINT
21 | typedef cl_uchar uchar; // NOLINT
22 | typedef cl_uchar uchar1; // NOLINT
23 | typedef cl_uchar2 uchar2; // NOLINT
24 | typedef cl_uchar3 uchar3; // NOLINT
25 | typedef cl_uchar4 uchar4; // NOLINT
26 | typedef short short1; // NOLINT
27 | typedef cl_short2 short2; // NOLINT
28 | typedef cl_short3 short3; // NOLINT
29 | typedef cl_short4 short4; // NOLINT
30 | typedef cl_ushort ushort; // NOLINT
31 | typedef cl_ushort ushort1; // NOLINT
32 | typedef cl_ushort2 ushort2; // NOLINT
33 | typedef cl_ushort3 ushort3; // NOLINT
34 | typedef cl_ushort4 ushort4; // NOLINT
35 | typedef cl_int int1; // NOLINT
36 | typedef cl_int2 int2; // NOLINT
37 | typedef cl_int3 int3; // NOLINT
38 | typedef cl_int4 int4; // NOLINT
39 | typedef cl_uint uint; // NOLINT
40 | typedef cl_uint uint1; // NOLINT
41 | typedef cl_uint2 uint2; // NOLINT
42 | typedef cl_uint3 uint3; // NOLINT
43 | typedef cl_uint4 uint4; // NOLINT
44 | typedef cl_long long1; // NOLINT
45 | typedef cl_long2 long2; // NOLINT
46 | typedef cl_long3 long3; // NOLINT
47 | typedef cl_long4 long4; // NOLINT
48 | typedef cl_ulong ulong; // NOLINT
49 | typedef cl_ulong ulong1; // NOLINT
50 | typedef cl_ulong2 ulong2; // NOLINT
51 | typedef cl_ulong3 ulong3; // NOLINT
52 | typedef cl_ulong4 ulong4; // NOLINT
53 | typedef cl_float float1; // NOLINT
54 | typedef cl_float2 float2; // NOLINT
55 | typedef cl_float3 float3; // NOLINT
56 | typedef cl_float4 float4; // NOLINT
57 | typedef cl_double double1; // NOLINT
58 | typedef cl_double2 double2; // NOLINT
59 | } // namespace gputil
60 |
61 | #endif // GPUPLATFORM2_H
62 |
--------------------------------------------------------------------------------
/gputil/cl/gpuProgramDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUPROGRAMDETAIL_H
7 | #define GPUPROGRAMDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "gpuDevice.h"
12 |
13 | #include
14 |
15 | #include
16 |
17 | namespace gputil
18 | {
19 | struct ProgramDetail
20 | {
21 | cl::Program program;
22 | Device device;
23 | std::string program_name;
24 | };
25 | } // namespace gputil
26 |
27 | #endif // GPUPROGRAMDETAIL_H
28 |
--------------------------------------------------------------------------------
/gputil/cl/gpuQueueDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUQUEUEDETAIL_H
7 | #define GPUQUEUEDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include
12 |
13 | namespace gputil
14 | {
15 | struct QueueDetail
16 | {
17 | cl::CommandQueue queue;
18 | bool force_synchronous = false;
19 | };
20 | } // namespace gputil
21 |
22 | #endif // GPUQUEUEDETAIL_H
23 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuApiExceptionCode.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include "gputil/gpuApiException.h"
8 |
9 | #include
10 |
11 | namespace gputil
12 | {
13 | const char *ApiException::errorCodeString(int error_code)
14 | {
15 | return cudaGetErrorString(static_cast(error_code));
16 | }
17 | } // namespace gputil
18 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuBufferDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUBUFFERDETAIL_H
8 | #define GPUBUFFERDETAIL_H
9 |
10 | #include "gputil/gpuDevice.h"
11 |
12 | #include "gputil/cuda/gpuMemRegion.h"
13 |
14 | #include
15 |
16 | namespace gputil
17 | {
18 | enum PinStatus : unsigned
19 | {
20 | kPinnedForRead = (1u << 0u),
21 | kPinnedForWrite = (1u << 1u)
22 | };
23 |
24 | struct BufferDetail
25 | {
26 | void *device_mem = nullptr;
27 | /// Host mapped memory for pinning. Maintained by gpuPinnedBuffer code.
28 | void *mapped_mem = nullptr;
29 | size_t alloc_size = 0;
30 | unsigned flags = 0;
31 | unsigned pinned_status = 0;
32 | Device device;
33 | /// List of dirty regions which need to be copied from mapped_mep to device_mem.
34 | std::vector dirty_write;
35 | };
36 |
37 | bool bufferCopy(void *dst, const void *src, size_t byte_count, cudaMemcpyKind kind, Queue *queue, Event *block_on,
38 | Event *completion);
39 | uint8_t *pin(BufferDetail &buf, PinMode mode);
40 | void unpin(BufferDetail &imp, void *pinned_ptr, PinMode mode, Queue *queue, Event *block_on, Event *completion);
41 | } // namespace gputil
42 |
43 | #endif // GPUBUFFERDETAIL_H
44 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuDeviceDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUDEVICEDETAIL_H
8 | #define GPUDEVICEDETAIL_H
9 |
10 | #include "gpuConfig.h"
11 |
12 | #include "gputil/gpuDeviceInfo.h"
13 | #include "gputil/gpuQueue.h"
14 |
15 | #include
16 |
17 | #include
18 |
19 | namespace gputil
20 | {
21 | struct DeviceDetail
22 | {
23 | int device = -1;
24 | std::string name;
25 | DeviceInfo info;
26 | Queue default_queue;
27 | };
28 | } // namespace gputil
29 |
30 | #endif // GPUDEVICEDETAIL_H
31 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuDirtyRegion:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/csiro-robotics/ohm/4e2e76945e550eb8a96cef9c4434e7c1811dd030/gputil/cuda/gpuDirtyRegion
--------------------------------------------------------------------------------
/gputil/cuda/gpuEventDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUEVENTDETAIL_H
7 | #define GPUEVENTDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "gputil/cuda/ref.h"
12 |
13 | #include
14 |
15 | #include
16 |
17 | namespace gputil
18 | {
19 | void destroyEvent(cudaEvent_t event);
20 |
21 | struct EventDetail : public Ref
22 | {
23 | inline EventDetail(cudaEvent_t obj, unsigned initial_ref_count, const ReleaseFunc &release)
24 | : Ref(obj, initial_ref_count, release)
25 | {}
26 |
27 | explicit inline EventDetail(Ref &&other) noexcept
28 | : Ref(std::move(other))
29 | {}
30 |
31 | inline EventDetail(const Ref &other) = delete;
32 |
33 | protected:
34 | inline ~EventDetail() = default;
35 | };
36 | } // namespace gputil
37 |
38 | #endif // GPUEVENTDETAIL_H
39 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuKernelDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUKERNELDETAIL_H
7 | #define GPUKERNELDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "gputil/gpuDevice.h"
12 | #include "gputil/gpuProgram.h"
13 |
14 | #include
15 |
16 | #include
17 |
18 | namespace gputil
19 | {
20 | struct KernelDetail
21 | {
22 | const void *cuda_kernel_function = nullptr;
23 | OptimalGroupSizeCalculation optimal_group_size_calc;
24 | size_t arg_count = 0u;
25 | std::vector> local_mem_args;
26 | size_t maximum_potential_workgroup_size = 0;
27 | Program program;
28 | bool auto_error_checking = true;
29 | };
30 | } // namespace gputil
31 |
32 | #endif // GPUKERNELDETAIL_H
33 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuMemRegion.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "gpuMemRegion.h"
7 |
8 | namespace gputil
9 | {
10 | void MemRegion::mergeRegionList(std::vector ®ions)
11 | {
12 | if (regions.empty())
13 | {
14 | return;
15 | }
16 |
17 | // Process the dirty list, copying regions back to the device.
18 | // First sort and merge the dirty list.
19 | std::sort(regions.begin(), regions.end());
20 |
21 | // Merge regions in the dirty list.
22 | auto merge_iter = regions.begin();
23 | for (auto next_iter = merge_iter + 1; merge_iter != regions.end() && next_iter != regions.end();)
24 | {
25 | if (!merge_iter->overlaps(*next_iter))
26 | {
27 | // Move on to the next block.
28 | merge_iter = next_iter;
29 | ++next_iter;
30 | }
31 | else
32 | {
33 | // Merge the two items.
34 | merge_iter->merge(*next_iter);
35 | // Zero out the merged item.
36 | next_iter->byte_count = 0;
37 | // Next item
38 | ++next_iter;
39 | }
40 | }
41 | }
42 | } // namespace gputil
43 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuMemRegion.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MEM_REGION_H
7 | #define MEM_REGION_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include
12 | #include
13 | #include
14 |
15 | namespace gputil
16 | {
17 | class MemRegion
18 | {
19 | public:
20 | size_t offset;
21 | size_t byte_count;
22 |
23 | inline bool operator<(const MemRegion &other) const
24 | {
25 | return offset < other.offset || offset == other.offset && byte_count < other.byte_count;
26 | }
27 |
28 |
29 | inline bool overlaps(const MemRegion &other) const
30 | {
31 | const size_t start_a = offset;
32 | const size_t end_a = offset + byte_count;
33 | const size_t start_b = other.offset;
34 | const size_t end_b = other.offset + other.byte_count - 1;
35 |
36 | // Overlap if any end point falls within the other region.
37 | // Only test non-zero regions.
38 | return byte_count && other.byte_count && (start_a <= start_b && start_b <= end_a) ||
39 | (start_b <= start_a && start_a <= end_b) || (start_a <= end_b && end_b <= end_a) ||
40 | (start_b <= end_a && end_a <= end_b);
41 | }
42 |
43 | inline MemRegion &merge(const MemRegion &other)
44 | {
45 | const size_t end_a = offset + byte_count;
46 | const size_t end_b = other.offset + other.byte_count;
47 | offset = std::min(offset, other.offset);
48 | byte_count = std::max(end_a, end_b) - offset;
49 | return *this;
50 | }
51 |
52 | /// Sorts and merged the list @p regions. The merged regions are reduced to zero size, but left in the list.
53 | /// @param regions The list to sort and merge.
54 | static void mergeRegionList(std::vector ®ions);
55 | };
56 | } // namespace gputil
57 |
58 | #endif // MEM_REGION_H
59 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuPlatform2.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUPLATFORM2_H
7 | #define GPUPLATFORM2_H
8 |
9 | #include
10 |
11 | namespace gputil
12 | {
13 | using char1 = ::char1; // NOLINT
14 | using char2 = ::char2; // NOLINT
15 | using char3 = ::char3; // NOLINT
16 | using char4 = ::char4; // NOLINT
17 | using uchar = unsigned char; // NOLINT
18 | using uchar1 = ::uchar1; // NOLINT
19 | using uchar2 = ::uchar2; // NOLINT
20 | using uchar3 = ::uchar3; // NOLINT
21 | using uchar4 = ::uchar4; // NOLINT
22 | using short1 = ::short1; // NOLINT
23 | using short2 = ::short2; // NOLINT
24 | using short3 = ::short3; // NOLINT
25 | using short4 = ::short4; // NOLINT
26 | using ushort = unsigned short; // NOLINT
27 | using ushort1 = ::ushort1; // NOLINT
28 | using ushort2 = ::ushort2; // NOLINT
29 | using ushort3 = ::ushort3; // NOLINT
30 | using ushort4 = ::ushort4; // NOLINT
31 | using int1 = ::int1; // NOLINT
32 | using int2 = ::int2; // NOLINT
33 | using int3 = ::int3; // NOLINT
34 | using int4 = ::int4; // NOLINT
35 | using uint = unsigned int; // NOLINT
36 | using uint1 = ::uint1; // NOLINT
37 | using uint2 = ::uint2; // NOLINT
38 | using uint3 = ::uint3; // NOLINT
39 | using uint4 = ::uint4; // NOLINT
40 | using long1 = ::longlong1; // NOLINT
41 | using long2 = ::longlong2; // NOLINT
42 | using long3 = ::longlong3; // NOLINT
43 | using long4 = ::longlong4; // NOLINT
44 | using ulong1 = ::ulonglong1; // NOLINT
45 | using ulong2 = ::ulonglong2; // NOLINT
46 | using ulong3 = ::ulonglong3; // NOLINT
47 | using ulong4 = ::ulonglong4; // NOLINT
48 | using float1 = ::float1; // NOLINT
49 | using float2 = ::float2; // NOLINT
50 | using float3 = ::float3; // NOLINT
51 | using float4 = ::float4; // NOLINT
52 | using double1 = ::double1; // NOLINT
53 | using double2 = ::double2; // NOLINT
54 | } // namespace gputil
55 |
56 | #endif // GPUPLATFORM2_H
57 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuProgram.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "gpuProgram.h"
7 |
8 | #include "gputil/cuda/gpuProgramDetail.h"
9 |
10 | namespace gputil
11 | {
12 | Program::Program() = default;
13 |
14 |
15 | Program::Program(Device &device, const char *program_name)
16 | : imp_(std::make_unique())
17 | {
18 | imp_->device = device;
19 | imp_->program_name = program_name;
20 | }
21 |
22 |
23 | Program::Program(Program &&other) noexcept
24 | : imp_(std::move(other.imp_))
25 | {}
26 |
27 |
28 | Program::Program(const Program &other)
29 | {
30 | *this = other;
31 | }
32 |
33 |
34 | Program::~Program() = default;
35 |
36 |
37 | bool Program::isValid() const
38 | {
39 | return imp_ != nullptr && imp_->device.isValid();
40 | }
41 |
42 | const char *Program::programName() const
43 | {
44 | return imp_->program_name.c_str();
45 | }
46 |
47 | Device Program::device()
48 | {
49 | return (imp_) ? imp_->device : Device();
50 | }
51 |
52 | // Lint(KS): API implementation compatibility requirement. Cannot make static
53 | // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
54 | int Program::buildFromFile(const char * /*file_name*/, const BuildArgs & /*build_args*/)
55 | {
56 | return 0;
57 | }
58 |
59 | // Lint(KS): API implementation compatibility requirement. Cannot make static
60 | // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
61 | int Program::buildFromSource(const char * /*source*/, size_t /*source_length*/, const BuildArgs & /*build_args*/)
62 | {
63 | return 0;
64 | }
65 |
66 | Program &Program::operator=(const Program &other)
67 | {
68 | if (this != &other)
69 | {
70 | if (other.imp_)
71 | {
72 | if (!imp_)
73 | {
74 | imp_ = std::make_unique();
75 | }
76 | imp_->device = other.imp_->device;
77 | imp_->program_name = other.imp_->program_name;
78 | }
79 | else
80 | {
81 | imp_.reset();
82 | }
83 | }
84 | return *this;
85 | }
86 |
87 | Program &Program::operator=(Program &&other) noexcept
88 | {
89 | imp_ = std::move(other.imp_);
90 | return *this;
91 | }
92 | } // namespace gputil
93 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuProgramDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUPROGRAMDETAIL_H
7 | #define GPUPROGRAMDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "gputil/gpuDevice.h"
12 |
13 | #include
14 |
15 | namespace gputil
16 | {
17 | struct ProgramDetail
18 | {
19 | Device device;
20 | std::string program_name;
21 | };
22 | } // namespace gputil
23 |
24 | #endif // GPUPROGRAMDETAIL_H
25 |
--------------------------------------------------------------------------------
/gputil/cuda/gpuQueueDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUQUEUEDETAIL_H
7 | #define GPUQUEUEDETAIL_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "gputil/gpuApiException.h"
12 | #include "gputil/gpuThrow.h"
13 |
14 | #include
15 |
16 | #include
17 |
18 | namespace gputil
19 | {
20 | struct QueueDetail
21 | {
22 | cudaStream_t queue = nullptr;
23 | bool force_synchronous = false;
24 |
25 | inline ~QueueDetail()
26 | {
27 | if (queue)
28 | {
29 | cudaError_t err = cudaStreamDestroy(queue);
30 | if (err != cudaSuccess)
31 | {
32 | // Dangerous to throw from destructor, so just log on exceptions.
33 | gputil::log(gputil::ApiException(err, nullptr, __FILE__, __LINE__));
34 | }
35 | }
36 | }
37 | };
38 | } // namespace gputil
39 |
40 | #endif // GPUQUEUEDETAIL_H
41 |
--------------------------------------------------------------------------------
/gputil/gpuApiException.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "gpuApiException.h"
7 |
8 | #include
9 | #include
10 |
11 | namespace gputil
12 | {
13 | ApiException::ApiException(const int error_code, const char *const msg, const char *const filename,
14 | const int line_number)
15 | : error_code_(error_code)
16 | {
17 | if (msg)
18 | {
19 | setMessage(msg, (filename == nullptr) ? "" : filename, line_number);
20 | }
21 | else
22 | {
23 | std::ostringstream str;
24 | str << "API error (" << error_code << ") " << errorCodeString(error_code);
25 | setMessage(str.str(), (filename == nullptr) ? "" : filename, line_number);
26 | }
27 | }
28 |
29 |
30 | ApiException::ApiException(ApiException &&other) noexcept
31 | : Exception(std::move(other))
32 | , error_code_(other.error_code_)
33 | {}
34 |
35 |
36 | ApiException::ApiException(const ApiException &other) noexcept
37 | : Exception(other)
38 | , error_code_(other.error_code_)
39 | {}
40 | } // namespace gputil
41 |
--------------------------------------------------------------------------------
/gputil/gpuApiException.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUGPUAPICHECK_H
8 | #define GPUGPUAPICHECK_H
9 |
10 | #include "gpuConfig.h"
11 |
12 | #include "gpuException.h"
13 |
14 | namespace gputil
15 | {
16 | /// Raised due to an exception in the underlying GPU SDK.
17 | class gputilAPI ApiException : public Exception // NOLINT(cppcoreguidelines-special-member-functions)
18 | {
19 | public:
20 | /// Construct an API exception.
21 | /// @param error_code The underlying SDK error code - e.g., a @c cudaError_t .
22 | /// @param msg Optional test for the api error. When null, the @c errorCodeString() for @p error_code is used.
23 | /// @param filename Optional name of file the exception is thrown from.
24 | /// @param line_number Optional line number where the exception is thrown.
25 | ApiException(int error_code, const char *msg, const char *filename = nullptr, int line_number = 0);
26 |
27 | /// @overload
28 | explicit inline ApiException(int error_code)
29 | : ApiException(error_code, nullptr)
30 | {}
31 |
32 | /// Move constructor.
33 | /// @param other Object to move.
34 | ApiException(ApiException &&other) noexcept;
35 |
36 | /// Copy constructor.
37 | /// @param other Object to copy.
38 | ApiException(const ApiException &other) noexcept;
39 |
40 | /// A helper function which converts the @p error_code into a string message.
41 | /// @param error_code The GPU API error code to retrieve a message for - e.g., a @c cudaError_t .
42 | /// @return A string which identifies the @p error_code . Some codes may be unknown.
43 | static const char *errorCodeString(int error_code);
44 |
45 | /// Query the error code value for the exception.
46 | /// @return The underlying SDK error code - e.g., a @c cudaError_t .
47 | inline int errorCode() const { return error_code_; }
48 |
49 | private:
50 | int error_code_;
51 | };
52 | } // namespace gputil
53 |
54 | #endif // GPUGPUAPICHECK_H
55 |
--------------------------------------------------------------------------------
/gputil/gpuConfig.in.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUCONFIG_H
8 | #define GPUCONFIG_H
9 |
10 | #include "gputilExport.h"
11 |
12 | #ifndef _USE_MATH_DEFINES
13 | #define _USE_MATH_DEFINES
14 | #endif // _USE_MATH_DEFINES
15 | #ifndef NOMINMAX
16 | #define NOMINMAX
17 | #endif // NOMINMAX
18 | #ifndef NOMINMAX
19 | #define NOMINMAX
20 | #endif // NOMINMAX
21 | #include
22 |
23 | // clang-format off
24 | #define GPUTIL_NONE 0
25 | #define GPUTIL_OPENCL 1
26 | #define GPUTIL_CUDA 2
27 | // clang-format on
28 |
29 | #endif // GPUCONFIG_H
30 |
--------------------------------------------------------------------------------
/gputil/gpuDeviceInfo.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUDEVICEINFO_H
7 | #define GPUDEVICEINFO_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include "gpuVersion.h"
12 |
13 | #include
14 |
15 | namespace gputil
16 | {
17 | /// Enumeration of accelerator types.
18 | enum DeviceType : uint16_t
19 | {
20 | kDeviceNull, ///< Invalid
21 | kDeviceGpu, ///< A GPU accelerator device.
22 | kDeviceCpu, ///< A CPU accelerator device
23 | kDeviceOther ///< Some other accelerator type.
24 | };
25 |
26 | /// Structure detailing information about a GPU or accelerator device. Available details may vary depending on the
27 | /// GPU SDK.
28 | struct DeviceInfo
29 | {
30 | /// The device name - e.g., the name of the GPU card.
31 | std::string name;
32 | /// Details of the accelerator platform - e.g., OpenCL <version> or CUDA.
33 | std::string platform;
34 | /// Accelerator API version - e.g., OpenCL 1.2 vs 2.0
35 | Version version;
36 | /// The accelerator type of the default.
37 | DeviceType type = DeviceType::kDeviceNull;
38 |
39 | /// Equality operator.
40 | /// @param other The structure to compare against.
41 | /// @return True if @c this exactly matches @p other .
42 | inline bool operator==(const DeviceInfo &other) const
43 | {
44 | return version == other.version && type == other.type && name == other.name && platform == other.platform;
45 | }
46 | };
47 | } // namespace gputil
48 |
49 | #endif // GPUDEVICEINFO_H
50 |
--------------------------------------------------------------------------------
/gputil/gpuException.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include "gpuException.h"
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | namespace gputil
14 | {
15 | #ifdef WIN32
16 | #define strncpy(dst, msg, len) strncpy_s(dst, len + 1, msg, len)
17 | #endif // WIN32
18 |
19 | Exception::Exception(const std::string &msg, const std::string &filename, int line_number)
20 | {
21 | setMessage(msg, filename, line_number);
22 | }
23 |
24 |
25 | Exception::Exception(Exception &&other) noexcept
26 | : message_(std::move(other.message_))
27 | {}
28 |
29 |
30 | Exception::Exception(const Exception &other) noexcept
31 | : message_(other.message_)
32 | {}
33 |
34 |
35 | Exception::~Exception() = default;
36 |
37 |
38 | const char *Exception::what() const noexcept
39 | {
40 | return message_.c_str();
41 | }
42 |
43 |
44 | void Exception::setMessage(const std::string &message, const std::string &filename, int line_number)
45 | {
46 | std::string str;
47 | if (!message.empty() || !filename.empty())
48 | {
49 | std::ostringstream sstr;
50 | if (!filename.empty())
51 | {
52 | sstr << filename;
53 |
54 | if (line_number > 0)
55 | {
56 | sstr << '(' << line_number << "):";
57 | }
58 | sstr << ' ';
59 | }
60 | if (!message.empty())
61 | {
62 | sstr << message;
63 | }
64 | str = sstr.str();
65 | }
66 |
67 | message_ = std::move(str);
68 | }
69 | } // namespace gputil
70 |
--------------------------------------------------------------------------------
/gputil/gpuException.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUEXCEPTION_H
8 | #define GPUEXCEPTION_H
9 |
10 | #include "gpuConfig.h"
11 | #include "gpuThrow.h"
12 |
13 | #include
14 | #include
15 |
16 | namespace gputil
17 | {
18 | /// Base class for exceptions thrown from this library.
19 | class gputilAPI Exception : public std::exception // NOLINT(cppcoreguidelines-special-member-functions)
20 | {
21 | public:
22 | /// Construct an exception.
23 | /// @param msg Optional exception message.
24 | /// @param filename Optional filename where the exception was thrown.
25 | /// @param line_number Optional line number where the exception was thrown.
26 | Exception(const std::string &msg = std::string(), // NOLINT(google-explicit-constructor)
27 | const std::string &filename = std::string(), int line_number = 0);
28 |
29 | /// Move constructor.
30 | /// @param other The object to move.
31 | Exception(Exception &&other) noexcept;
32 |
33 | /// Copy constructor.
34 | /// @param other The object to copy.
35 | Exception(const Exception &other) noexcept;
36 |
37 | /// Destructor.
38 | ~Exception() override;
39 |
40 | /// Query the exception message. This is a combination of the constructed message, filename and line number.
41 | /// @return The exception message.
42 | const char *what() const noexcept override;
43 |
44 | protected:
45 | /// Build an exception message from the given base @p message , @p filename and @p line_number .
46 | ///
47 | /// Formatted as `(): ` where possible. Optional items may be omitted.
48 | ///
49 | /// @param message The base exception message.
50 | /// @param filename Optional filename where the exception was thrown.
51 | /// @param line_number Optional line number where the exception was thrown.
52 | void setMessage(const std::string &message, const std::string &filename = std::string(), int line_number = 0);
53 |
54 | private:
55 | std::string message_;
56 | };
57 | } // namespace gputil
58 |
59 | #endif // GPUEXCEPTION_H
60 |
--------------------------------------------------------------------------------
/gputil/gpuPinMode.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUPINMODE_H
7 | #define GPUPINMODE_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | namespace gputil
12 | {
13 | // Pinning functions.
14 | enum PinMode
15 | {
16 | /// Null pinning mode - invalid.
17 | kPinNone = 0,
18 | /// Pin for reading on CPU.
19 | kPinRead,
20 | /// Pin for writing from CPU.
21 | kPinWrite,
22 | /// Pin for read/write.
23 | kPinReadWrite
24 | };
25 | } // namespace gputil
26 |
27 | #endif // GPUPINMODE_H
28 |
--------------------------------------------------------------------------------
/gputil/gpuPlatform.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUPLATFORM_H
7 | #define GPUPLATFORM_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #if GPUTIL_TYPE == GPUTIL_OPENCL
12 | #include "cl/gpuPlatform2.h"
13 | #elif GPUTIL_TYPE == GPUTIL_CUDA
14 | #include "cuda/gpuPlatform2.h"
15 | #else // GPUTIL_TYPE == ???
16 | #error Unknown GPU base API
17 | #endif // GPUTIL_TYPE
18 |
19 | #endif // GPUPLATFORM_H
20 |
--------------------------------------------------------------------------------
/gputil/gpuThrow.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include "gpuThrow.h"
8 |
9 | #include "gpuException.h"
10 |
11 | #include
12 |
13 | namespace gputil
14 | {
15 | void log(const Exception &e)
16 | {
17 | std::cerr << "GPU Exception: " << e.what() << std::endl;
18 | }
19 |
20 |
21 | void log(const Exception &e, const char *file, int line)
22 | {
23 | std::cerr << file << "(" << line << "): GPU exception: " << e.what() << std::endl;
24 | }
25 | } // namespace gputil
26 |
--------------------------------------------------------------------------------
/gputil/gpuThrow.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef GPUTHROW_H
8 | #define GPUTHROW_H
9 |
10 | #include "gpuConfig.h"
11 |
12 | #define GPU_EXCEPTIONS 1
13 |
14 | #if GPU_EXCEPTIONS
15 | // Return statement present to prevent compilation errors when switching GPU_EXCEPTIONS.
16 | #define GPUTHROW(e, r) \
17 | { \
18 | throw(e); \
19 | return r; \
20 | }
21 | #define GPUTHROW2(e) throw e
22 | #else // GPU_EXCEPTIONS
23 | #define GPUTHROW(e, r) \
24 | { \
25 | gputil::log(e); \
26 | return r; \
27 | }
28 | #define GPUTHROW2(e) \
29 | { \
30 | gputil::log(e); \
31 | return; \
32 | }
33 | #endif // GPU_EXCEPTIONS
34 |
35 | #define GPUAPICHECK(err, expect, r) \
36 | if ((err) != (expect)) \
37 | { \
38 | GPUTHROW(gputil::ApiException(err, nullptr, __FILE__, __LINE__), r); \
39 | }
40 | #define GPUAPICHECK2(err, expect) \
41 | if ((err) != (expect)) \
42 | { \
43 | GPUTHROW2(gputil::ApiException(err, nullptr, __FILE__, __LINE__)); \
44 | }
45 |
46 | namespace gputil
47 | {
48 | class Exception;
49 |
50 | void gputilAPI log(const Exception &e);
51 | void gputilAPI log(const Exception &e, const char *file, int line);
52 | } // namespace gputil
53 |
54 | #endif // GPUTHROW_H
55 |
--------------------------------------------------------------------------------
/gputil/gpuVersion.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUVERSION_H
7 | #define GPUVERSION_H
8 |
9 | #include "gpuConfig.h"
10 |
11 | #include
12 |
13 | namespace gputil
14 | {
15 | /// Version number structure for the GPU device/API split as `..`.
16 | struct Version
17 | {
18 | uint16_t major = 0; ///< Major version part.
19 | uint16_t minor = 0; ///< Minor version part.
20 | uint16_t patch = 0; ///< Patch version part.
21 |
22 | /// Equality operator.
23 | /// @param other The object to compare against.
24 | /// @return True if the two versions exactly match.
25 | inline bool operator==(const Version &other) const
26 | {
27 | return major == other.major && minor == other.minor && patch == other.patch;
28 | }
29 | };
30 | } // namespace gputil
31 |
32 | #endif // GPUVERSION_H
33 |
--------------------------------------------------------------------------------
/logutil/CMakeLists.txt:
--------------------------------------------------------------------------------
1 |
2 | include(GenerateExportHeader)
3 | include(TextFileResource)
4 |
5 | configure_file(LogUtilConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/logutil/LogUtilConfig.h")
6 |
7 | set(SOURCES
8 | Logger.cpp
9 | Logger.h
10 | LoggerDetail.h
11 | LogUtil.cpp
12 | LogUtil.h
13 | )
14 |
15 | set(PUBLIC_HEADERS
16 | "${CMAKE_CURRENT_BINARY_DIR}/logutil/LogUtilConfig.h"
17 | "${CMAKE_CURRENT_BINARY_DIR}/logutil/LogUtilExport.h"
18 | Logger.h
19 | LoggerDetail.h
20 | LogUtil.h
21 | )
22 |
23 | add_library(logutil ${SOURCES})
24 | clang_tidy_target(logutil)
25 |
26 | target_include_directories(logutil
27 | PUBLIC
28 | $
29 | $
30 | $
31 | $
32 | PRIVATE
33 | $
34 | )
35 |
36 | generate_export_header(logutil
37 | EXPORT_MACRO_NAME logutil_API
38 | EXPORT_FILE_NAME logutil/LogUtilExport.h
39 | STATIC_DEFINE LOGUTIL_STATIC)
40 |
41 | install(TARGETS logutil EXPORT ${CMAKE_PROJECT_NAME}-config-targets
42 | LIBRARY DESTINATION lib
43 | ARCHIVE DESTINATION lib
44 | RUNTIME DESTINATION bin
45 | INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/logutil
46 | )
47 |
48 | install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/logutil)
49 |
50 | source_group("source" REGULAR_EXPRESSION ".*$")
51 |
--------------------------------------------------------------------------------
/logutil/LogUtilConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef LOGUTILCONFIG_H
6 | #define LOGUTILCONFIG_H
7 |
8 | #include "LogUtilExport.h"
9 |
10 | #ifndef _USE_MATH_DEFINES
11 | #define _USE_MATH_DEFINES
12 | #endif // _USE_MATH_DEFINES
13 | #ifndef NOMINMAX
14 | #define NOMINMAX
15 | #endif // NOMINMAX
16 |
17 | #ifdef _MSC_VER
18 | // Avoid dubious security warnings for plenty of legitimate code
19 | #ifndef _SCL_SECURE_NO_WARNINGS
20 | #define _SCL_SECURE_NO_WARNINGS
21 | #endif // _SCL_SECURE_NO_WARNINGS
22 | #ifndef _CRT_SECURE_NO_WARNINGS
23 | #define _CRT_SECURE_NO_WARNINGS
24 | #endif // _CRT_SECURE_NO_WARNINGS
25 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
26 | #endif // _MSC_VER
27 |
28 | #endif // LOGUTILCONFIG_H
29 |
--------------------------------------------------------------------------------
/logutil/Logger.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "Logger.h"
7 |
8 | #include
9 |
10 | namespace
11 | {
12 | logutil::LogOStream g_default_logger;
13 | logutil::LogInterface *g_current_logger = &g_default_logger;
14 | } // namespace
15 |
16 | namespace logutil
17 | {
18 | LogInterface::LogInterface(LogLevel level) noexcept
19 | : level_(level)
20 | {}
21 |
22 |
23 | LogInterface::~LogInterface() = default;
24 |
25 |
26 | LogOStream::LogOStream(LogLevel level) noexcept
27 | : LogInterface(level)
28 | {}
29 |
30 |
31 | void LogOStream::message(LogLevel level, const char *msg)
32 | {
33 | if (int(level) <= int(this->level()))
34 | {
35 | switch (level)
36 | {
37 | default:
38 | case LogLevel::kTrace:
39 | std::clog << msg << std::flush;
40 | break;
41 | case LogLevel::kInfo:
42 | std::cout << msg << std::flush;
43 | break;
44 | case LogLevel::kWarn:
45 | std::cout << msg << std::flush;
46 | break;
47 | case LogLevel::kError:
48 | case LogLevel::kFatal:
49 | std::cerr << msg << std::flush;
50 | break;
51 | }
52 | }
53 | }
54 |
55 |
56 | LogInterface *logger()
57 | {
58 | return g_current_logger;
59 | }
60 |
61 |
62 | LogInterface *setLogger(LogInterface *logger)
63 | {
64 | auto *previous = g_current_logger;
65 | g_current_logger = logger;
66 | return previous;
67 | }
68 |
69 |
70 | LogInterface *defaultLogger()
71 | {
72 | return &g_default_logger;
73 | }
74 | } // namespace logutil
75 |
--------------------------------------------------------------------------------
/logutil/LoggerDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef LOGUTIL_LOGGERDETAIL_H
7 | #define LOGUTIL_LOGGERDETAIL_H
8 |
9 | #include "LogUtilConfig.h"
10 |
11 | #include
12 | #include
13 | #include
14 | #include
15 |
16 | namespace logutil
17 | {
18 | namespace logger_detail
19 | {
20 | /// Prepare the @p out stream for logging, imbuing the locale.
21 | ///
22 | /// Other effects:
23 | /// - Set @c std::boolalpha
24 | ///
25 | /// @param out The stream to prepare.
26 | inline void prepareStream(std::ostream &out)
27 | {
28 | out.imbue(std::locale(""));
29 | out << std::boolalpha;
30 | }
31 |
32 | template
33 | inline void message(std::ostream &stream, const T &value)
34 | {
35 | stream << value;
36 | }
37 |
38 | template
39 | inline void message(std::ostream &stream, const T &value, Args... args)
40 | {
41 | stream << value;
42 | message(stream, args...);
43 | }
44 | } // namespace logger_detail
45 | } // namespace logutil
46 |
47 | #endif // LOGUTIL_LOGGERDETAIL_H
48 |
--------------------------------------------------------------------------------
/ohm/CalculateSegmentKeys.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "CalculateSegmentKeys.h"
7 |
8 | #include "KeyList.h"
9 | #include "LineWalk.h"
10 |
11 | namespace ohm
12 | {
13 | size_t calculateSegmentKeys(KeyList &keys, const OccupancyMap &map, const glm::dvec3 &start_point,
14 | const glm::dvec3 &end_point, bool include_end_point)
15 | {
16 | keys.clear();
17 | return walkSegmentKeys(LineWalkContext(map,
18 | [&keys](const Key &key, double enter_range, double exit_range) {
19 | (void)enter_range; // Unused
20 | (void)exit_range; // Unused
21 |
22 | keys.add(key);
23 | return true;
24 | }),
25 | start_point, end_point, (include_end_point) ? 0u : kExcludeEndVoxel);
26 | }
27 | } // namespace ohm
28 |
--------------------------------------------------------------------------------
/ohm/CalculateSegmentKeys.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef CALCULATESEGMENTKEYS_H
7 | #define CALCULATESEGMENTKEYS_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | class KeyList;
18 | class OccupancyMap;
19 |
20 | /// This populates a @c KeyList with the voxel @c Key values intersected by a line segment.
21 | ///
22 | /// This utility function leverages @c walkSegmentKeys() in order to calculate the set of voxel @c Key values
23 | /// intersected by the line segment @p start_point to @p end_point .
24 | ///
25 | /// @param[out] keys Populates with the keys intersected by the specified line segment.
26 | /// @param map The occupancy map to calculate key segments for.
27 | /// @param start_point The coordinate of the line segment start or sensor position. Global map frame.
28 | /// @param end_point The coordinate of the line segment end or sample position. Global map frame.
29 | /// @param include_end_point True to include the voxel containing the @p end_point , false to omit this voxel,
30 | /// even when the @p start_point is in the same voxel (this case would generate an empty list).
31 | size_t ohm_API calculateSegmentKeys(KeyList &keys, const OccupancyMap &map, const glm::dvec3 &start_point,
32 | const glm::dvec3 &end_point, bool include_end_point = true);
33 | } // namespace ohm
34 |
35 | #endif // CALCULATESEGMENTKEYS_H
36 |
--------------------------------------------------------------------------------
/ohm/ClearingPattern.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "ClearingPattern.h"
7 | #include "private/ClearingPatternDetail.h"
8 |
9 | #include "OccupancyMap.h"
10 | #include "RayPattern.h"
11 |
12 | namespace ohm
13 | {
14 | ClearingPattern::ClearingPattern(const RayPattern *pattern, bool take_ownership)
15 | : imp_(new ClearingPatternDetail)
16 | {
17 | imp_->pattern = pattern;
18 | imp_->has_pattern_ownership = take_ownership;
19 | imp_->ray_flags = kDefaultRayFlags;
20 | }
21 |
22 | ClearingPattern::~ClearingPattern()
23 | {
24 | if (imp_->has_pattern_ownership)
25 | {
26 | delete imp_->pattern;
27 | }
28 | }
29 |
30 | const RayPattern *ClearingPattern::pattern() const
31 | {
32 | return imp_->pattern;
33 | }
34 |
35 | bool ClearingPattern::hasPatternOwnership() const
36 | {
37 | return imp_->has_pattern_ownership;
38 | }
39 |
40 | unsigned ClearingPattern::rayFlags() const
41 | {
42 | // Must not use kRfReverseWalk for clearing patterns and ray queries or we cannot stop on the first obstruction.
43 | return imp_->ray_flags & ~kRfReverseWalk;
44 | }
45 |
46 | void ClearingPattern::setRayFlags(unsigned ray_flags)
47 | {
48 | imp_->ray_flags = ray_flags;
49 | }
50 |
51 | const glm::dvec3 *ClearingPattern::lastRaySet(size_t *element_count) const
52 | {
53 | *element_count = imp_->ray_set.size();
54 | return imp_->ray_set.data();
55 | }
56 |
57 |
58 | const glm::dvec3 *ClearingPattern::buildRaySet(size_t *element_count, const glm::dvec3 &position,
59 | const glm::dquat &rotation)
60 | {
61 | imp_->pattern->buildRays(&imp_->ray_set, position, rotation);
62 | *element_count = imp_->ray_set.size();
63 | return imp_->ray_set.data();
64 | }
65 |
66 |
67 | const glm::dvec3 *ClearingPattern::buildRaySet(size_t *element_count, const glm::dmat4 &pattern_transform)
68 | {
69 | imp_->pattern->buildRays(&imp_->ray_set, pattern_transform);
70 | *element_count = imp_->ray_set.size();
71 | return imp_->ray_set.data();
72 | }
73 | } // namespace ohm
74 |
--------------------------------------------------------------------------------
/ohm/DataType.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "DataType.h"
7 |
8 | #include
9 |
10 | namespace ohm
11 | {
12 | const char *DataType::name(unsigned type)
13 | {
14 | static const std::array names = //
15 | {
16 | "", //
17 | "int8_t", //
18 | "uint8_t", //
19 | "int16_t", //
20 | "uint16_t", //
21 | "int32_t", //
22 | "uint32_t", //
23 | "int64_t", //
24 | "uint64_t", //
25 | "float", //
26 | "double", //
27 | "struct" //
28 | };
29 |
30 | if (type < kUser)
31 | {
32 | if (type < names.size())
33 | {
34 | return names[type];
35 | }
36 |
37 | return "";
38 | }
39 |
40 | return "user";
41 | }
42 |
43 |
44 | size_t DataType::size(unsigned type)
45 | {
46 | static const std::array sizes = //
47 | {
48 | 0u, // unknown
49 | 1u, // int8
50 | 1u, // uint8
51 | 2u, // int16
52 | 2u, // uin16
53 | 4u, // int32
54 | 4u, // uint32
55 | 8u, // int64
56 | 8u, // uint64
57 | 4u, // float
58 | 8u, // double
59 | 0u // struct
60 | };
61 |
62 | if (type < kUser)
63 | {
64 | if (type < sizes.size())
65 | {
66 | return sizes[type];
67 | }
68 |
69 | return 0u;
70 | }
71 |
72 | return 0u;
73 | }
74 | } // namespace ohm
75 |
--------------------------------------------------------------------------------
/ohm/DataType.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_DATATYPE_H
7 | #define OHM_DATATYPE_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | /// Data type enumeration, particularly for @c VoxelLayout data.
14 | struct ohm_API DataType
15 | {
16 | /// Enumeration of the supported data types.
17 | enum Type
18 | {
19 | kUnknown,
20 | kInt8,
21 | kUInt8,
22 | kInt16,
23 | kUInt16,
24 | kInt32,
25 | kUInt32,
26 | kInt64,
27 | kUInt64,
28 | kFloat,
29 | kDouble,
30 | // NYI: May implement in future to support more interesting voxel structures.
31 | kStruct,
32 |
33 | kUser = 256
34 | };
35 |
36 | /// Returns a string representation of @p type.
37 | /// @param type The data type value to convert to string.
38 | /// @return A string name for @p type or "<invalid>" if @p type is invalid. Any user type is "user".
39 | static const char *name(unsigned type);
40 |
41 | /// Returns the byte size of @p type if known.
42 | /// @param type The data type value to return a byte size for.
43 | /// @return The byte size of @p type or zero if the size is unknown, variable or @p type is invalid.
44 | static size_t size(unsigned type);
45 | };
46 | } // namespace ohm
47 |
48 | #endif // OHM_DATATYPE_H
49 |
--------------------------------------------------------------------------------
/ohm/DebugDraw.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "OhmConfig.h"
7 |
8 | namespace ohm
9 | {
10 | class OccupancyMap;
11 |
12 | /// Debug draw the @p map using 3rd Eye Scene (if available).
13 | ///
14 | /// Requires @c ohm::trace::available()
15 | /// @param map The map to draw.
16 | /// @return True if a 3es is available and a connection is present.
17 | bool ohm_API debugDraw(const ohm::OccupancyMap &map);
18 |
19 | /// Debug draw @p map via 3es with Ndt TM data. Fails if required layers are not available.
20 | /// @param map The map to draw.
21 | /// @return True if a 3es is available and a connection is present.
22 | bool ohm_API debugDrawNdtTm(const ohm::OccupancyMap &map);
23 |
24 | /// Debug draw @p map via 3es with Ndt occupancy data. Fails if required layers are not available.
25 | /// @param map The map to draw.
26 | /// @return True if a 3es is available and a connection is present.
27 | bool ohm_API debugDrawNdtOm(const ohm::OccupancyMap &map);
28 |
29 | /// Debug draw @p map via 3es using only occupancy and voxel mean data. Fails if occupancy layer is not available.
30 | /// @param map The map to draw.
31 | /// @return True if a 3es is available and a connection is present.
32 | bool ohm_API debugDrawOccupancy(const ohm::OccupancyMap &map);
33 | } // namespace ohm
34 |
--------------------------------------------------------------------------------
/ohm/DebugIDs.h:
--------------------------------------------------------------------------------
1 | //
2 | // author Kazys Stepanas
3 | //
4 | #ifndef DEBUG_IDS_H_
5 | #define DEBUG_IDS_H_
6 |
7 | namespace ohm
8 | {
9 | enum class Category
10 | {
11 | kDefault,
12 | kMap,
13 | // Parent category for below.
14 | kPopulate,
15 | kRays,
16 | kFreeCells,
17 | kOccupiedCells,
18 | kInfo
19 | };
20 |
21 | enum Resource
22 | {
23 | kMap = 1,
24 | kMapMesh = 1, // OK to be the same as kMap. One is object, the other is resource.
25 | };
26 | } // namespace ohm
27 |
28 | #endif // DEBUG_IDS_H_
--------------------------------------------------------------------------------
/ohm/Density.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "Density.h"
7 |
8 | #include "Key.h"
9 | #include "MapLayout.h"
10 | #include "OccupancyMap.h"
11 |
12 | #include
13 |
14 | namespace ohm
15 | {
16 | float voxelDensity(const OccupancyMap &map, const Key &key)
17 | {
18 | if (!key.isNull())
19 | {
20 | Voxel traversal_voxel(&map, map.layout().traversalLayer());
21 | Voxel mean_voxel(&map, map.layout().meanLayer());
22 | if (traversal_voxel.isLayerValid() && mean_voxel.isLayerValid())
23 | {
24 | setVoxelKey(key, traversal_voxel, mean_voxel);
25 | return voxelDensity(traversal_voxel, mean_voxel);
26 | }
27 | }
28 | return 0.0f;
29 | }
30 | } // namespace ohm
31 |
--------------------------------------------------------------------------------
/ohm/Key.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Author: Kazys Stepanas
3 | // Copyright (c) CSIRO 2017
4 | //
5 | #include "Key.h"
6 |
7 | #include
8 |
9 | #define INVALID_VALUE std::numeric_limits::lowest()
10 |
11 | namespace ohm
12 | {
13 | const int16_t Key::kInvalidValue = INVALID_VALUE;
14 | const Key Key::kNull(glm::ivec3(INVALID_VALUE), 0, 0, 0); // NOLINT
15 |
16 | size_t Key::Hash::operator()(const Key &key) const
17 | {
18 | const unsigned one_byte_shift = 8u;
19 | const unsigned two_byte_shift = 16u;
20 | glm::u32vec3 hash;
21 | hash.x = key.region_key_.x | key.region_key_.z << two_byte_shift; // NOLINT(hicpp-signed-bitwise)
22 | hash.y = key.region_key_.y;
23 | hash.z =
24 | key.local_[0] | key.local_[1] << one_byte_shift | key.local_[2] << two_byte_shift; // NOLINT(hicpp-signed-bitwise)
25 | return vhash::hashBits(hash.x, hash.y, hash.z);
26 | }
27 |
28 | unsigned Key::regionHash() const
29 | {
30 | const glm::u32vec3 rk = region_key_;
31 | return vhash::hashBits(rk.x, rk.y, rk.z);
32 | }
33 |
34 |
35 | void Key::clampToAxis(int axis, const Key &min_bounds, const Key &max_bounds)
36 | {
37 | if (region_key_[axis] < min_bounds.region_key_[axis])
38 | {
39 | // Below lower bounds region. Clamp region and local.
40 | region_key_[axis] = min_bounds.region_key_[axis];
41 | local_[axis] = min_bounds.local_[axis];
42 | }
43 | else if (region_key_[axis] == min_bounds.region_key_[axis] && local_[axis] < min_bounds.local_[axis])
44 | {
45 | // Below lower bounds local. Clamp local.
46 | local_[axis] = min_bounds.local_[axis];
47 | }
48 |
49 | if (region_key_[axis] > max_bounds.region_key_[axis])
50 | {
51 | // Above upper bounds region. Clamp region and local.
52 | region_key_[axis] = max_bounds.region_key_[axis];
53 | local_[axis] = max_bounds.local_[axis];
54 | }
55 | else if (region_key_[axis] == max_bounds.region_key_[axis] && local_[axis] > max_bounds.local_[axis])
56 | {
57 | // Above upper bounds local. Clamp local.
58 | local_[axis] = max_bounds.local_[axis];
59 | }
60 | }
61 | } // namespace ohm
62 |
--------------------------------------------------------------------------------
/ohm/KeyHash.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | //
7 | // Source file to make sure KeyHash symbols are DLL exported.
8 | #include "KeyHash.h"
9 |
--------------------------------------------------------------------------------
/ohm/KeyHash.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef KEYHASH_H
7 | #define KEYHASH_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "Key.h"
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | /// Hashing function for @c Key objects.
18 | struct ohm_API KeyHash
19 | {
20 | /// Hash operator.
21 | /// @param key The key to hash.
22 | /// @return A 32-bit hash value for @p key.
23 | inline uint32_t operator()(const Key &key) const
24 | {
25 | const auto ®ion = key.regionKey();
26 | const auto &local = key.localKey();
27 | return vhash::hashBits(region.x | (local.x << 16), region.y | (local.y << 16), region.z | (local.z << 16));
28 | }
29 | };
30 | } // namespace ohm
31 |
32 | #endif // KEYHASH_H
33 |
--------------------------------------------------------------------------------
/ohm/KeyList.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Author: Kazys Stepanas
3 | // Copyright (c) CSIRO 2017
4 | //
5 | #include "KeyList.h"
6 |
7 | #include
8 | #include
9 |
10 | namespace ohm
11 | {
12 | KeyList::KeyList(size_t initial_count)
13 | {
14 | if (initial_count)
15 | {
16 | keys_.resize(initial_count);
17 | }
18 | }
19 |
20 |
21 | KeyList::~KeyList() = default;
22 |
23 |
24 | void KeyList::reserve(size_t capacity)
25 | {
26 | keys_.reserve(capacity);
27 | }
28 |
29 |
30 | void KeyList::resize(size_t count)
31 | {
32 | keys_.resize(count);
33 | }
34 |
35 |
36 | void KeyList::emplace_back(const Key &key) // NOLINT
37 | {
38 | keys_.emplace_back(key);
39 | }
40 |
41 |
42 | Key &KeyList::add()
43 | {
44 | keys_.emplace_back(Key::kNull);
45 | return keys_.back();
46 | }
47 | } // namespace ohm
48 |
--------------------------------------------------------------------------------
/ohm/KeyStream.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_KEYSTREAM_H_
7 | #define OHM_KEYSTREAM_H_
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "Key.h"
12 |
13 | // Deprecated. Stream operator now appears in Key.h
14 |
15 | #endif // OHM_KEYSTREAM_H_
16 |
--------------------------------------------------------------------------------
/ohm/MapChunkFlag.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPCHUNKFLAG_H_
7 | #define MAPCHUNKFLAG_H_
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | enum MapChunkFlag
14 | {
15 | MainMemDirty = (1 << 0),
16 | GpuMemDirty = (1 << 1)
17 | };
18 | }
19 |
20 | #endif // MAPCHUNKFLAG_H_
21 |
--------------------------------------------------------------------------------
/ohm/MapFlag.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "MapFlag.h"
7 |
8 | #include
9 | #include
10 |
11 | namespace
12 | {
13 | const std::array kMapFlagNames = //
14 | { "VoxelMean", "Compressed", "Traversal" };
15 | } // namespace
16 |
17 | namespace ohm
18 | {
19 | const char *mapFlagToString(MapFlag flag)
20 | {
21 | unsigned bit = 1;
22 | for (unsigned i = 0; i < kMapFlagNames.size(); ++i, bit <<= 1u)
23 | {
24 | if (unsigned(flag) & bit)
25 | {
26 | return kMapFlagNames[i];
27 | }
28 | }
29 |
30 | return "None";
31 | }
32 |
33 |
34 | MapFlag mapFlagFromString(const char *str)
35 | {
36 | std::string name(str);
37 | unsigned bit = 1;
38 | for (unsigned i = 0; i < kMapFlagNames.size(); ++i, bit <<= 1u)
39 | {
40 | if (name == kMapFlagNames[i])
41 | {
42 | return MapFlag(bit);
43 | }
44 | }
45 |
46 | return MapFlag::kNone;
47 | }
48 |
49 | } // namespace ohm
50 |
--------------------------------------------------------------------------------
/ohm/MapLayoutMatch.h:
--------------------------------------------------------------------------------
1 | //
2 | // Author: Kazys Stepanas
3 | // Copyright (c) CSIRO 2020
4 | //
5 | #ifndef MAPLAYOUTMATCH_H
6 | #define MAPLAYOUTMATCH_H
7 |
8 | #include "OhmConfig.h"
9 |
10 | namespace ohm
11 | {
12 | /// Return values for @c MapLayout, @p MapLayer and @p VoxelLayout checks.
13 | enum class MapLayoutMatch : int
14 | {
15 | /// Layers are different and do not match.
16 | kDifferent = 0,
17 | /// The layout matches, but some of the layer and voxel member names may differ.
18 | kEquivalent,
19 | /// The layer match exactly.
20 | kExact
21 | };
22 | } // namespace ohm
23 |
24 | #endif // MAPLAYOUTMATCH_H
25 |
--------------------------------------------------------------------------------
/ohm/MapProbability.h:
--------------------------------------------------------------------------------
1 | //
2 | // @author Kazys Stepanas
3 | //
4 | // Copyright (c) 2015 CSIRO
5 | //
6 | #ifndef OHM_PROBABILITY_H
7 | #define OHM_PROBABILITY_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 | #include
13 |
14 | namespace ohm
15 | {
16 | /// Calculate a probability from its log value.
17 | /// @param value The log probability value.
18 | /// @return A real probability value [0, 1].
19 | template // NOLINT(readability-identifier-naming)
20 | inline real valueToProbability(real value)
21 | {
22 | // Ensure -infinity yields a zero probability.
23 | // Not all environments respect the sign of an infinity. Some will and yield 0 and 1
24 | // for -infinity and +infinity respectively, other's yield 1. We explicitly correct this.
25 | return (value == -std::numeric_limits::infinity()) ? real{ 0 } :
26 | real{ 1 } - (real{ 1 } / (real{ 1 } + std::exp(value)));
27 | }
28 |
29 | /// Convert a probability to a storable value. Inverse of @p valueToProbability().
30 | /// @param probability The probability to convert
31 | /// @return The value associated with @p probability."ohmconfig.h"
32 | template // NOLINT(readability-identifier-naming)
33 | inline real probabilityToValue(real probability)
34 | {
35 | return std::log(probability / (real{ 1 } - probability));
36 | }
37 | } // namespace ohm
38 |
39 | #endif // OHM_PROBABILITY_H
40 |
--------------------------------------------------------------------------------
/ohm/MapRegionCache.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "MapRegionCache.h"
7 |
8 | namespace ohm
9 | {
10 | struct MapRegionCacheDetail
11 | {
12 | // empty
13 | };
14 |
15 | MapRegionCache::MapRegionCache() = default;
16 |
17 | MapRegionCache::~MapRegionCache() = default;
18 |
19 | } // namespace ohm
20 |
--------------------------------------------------------------------------------
/ohm/MappingProcess.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "MappingProcess.h"
7 |
8 | namespace ohm
9 | {
10 | MappingProcess::MappingProcess() = default;
11 |
12 | MappingProcess::~MappingProcess() = default;
13 | } // namespace ohm
14 |
--------------------------------------------------------------------------------
/ohm/MappingProcess.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_MAPPROCESS_H
7 | #define OHM_MAPPROCESS_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | class OccupancyMap;
14 |
15 | /// Return value for @c Mapper::update() and @c MappingProcess::update(). Values must be ordered
16 | /// such that more significant results come later. This supports a simple amalgamation of results
17 | /// in @c Mapper::update() by simply using the maximum value of the aggregate processes.
18 | enum MappingProcessResult
19 | {
20 | /// Everything up to date or nothing to do.
21 | kMprUpToDate,
22 | /// Some progress made. More to be done.
23 | kMprProgressing
24 | };
25 |
26 | /// Base class for processes to be added to the @p Mapper for processing during map update.
27 | ///
28 | /// @note This class and all derivations are experimental.
29 | ///
30 | /// @todo Features to consider:
31 | /// - Update period and scheduling
32 | /// - Update only on dirty?
33 | class ohm_API MappingProcess
34 | {
35 | public:
36 | /// Constructor.
37 | MappingProcess();
38 | /// Virtual destructor.
39 | virtual ~MappingProcess();
40 |
41 | /// Request a pause or unpause the process.
42 | /// @param pause True to pause, false to unpause.
43 | inline void pause(bool pause = true) { paused_ = pause; }
44 | /// Query if the process is currently paused.
45 | /// @return True if paused.
46 | inline bool paused() const { return paused_; }
47 |
48 | /// Request a reset of the process. Must drop and reinitialise all data.
49 | virtual void reset() = 0;
50 |
51 | /// Called to update the process on the given map and limited processing time.
52 | /// @param map The map to target.
53 | /// @param time_slice Time processing limit for the update (seconds).
54 | virtual int update(OccupancyMap &map, double time_slice) = 0;
55 |
56 | private:
57 | bool paused_ = false;
58 | };
59 | } // namespace ohm
60 |
61 | #endif // OHM_MAPPROCESS_H
62 |
--------------------------------------------------------------------------------
/ohm/Mutex.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "Mutex.h"
7 |
8 | // Nothing.
9 |
--------------------------------------------------------------------------------
/ohm/Mutex.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMMUTEX_H
7 | #define OHMMUTEX_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | #ifdef OHM_FEATURE_THREADS
14 | #include
15 | namespace ohm
16 | {
17 | // tbb::mutex has been deprecated in favour of std::mutex
18 | using Mutex = std::mutex;
19 | using SpinMutex = tbb::spin_mutex;
20 | } // namespace ohm
21 | #else // OHM_FEATURE_THREADS
22 | namespace ohm
23 | {
24 | using Mutex = std::mutex;
25 | using SpinMutex = std::mutex;
26 | } // namespace ohm
27 | #endif // OHM_FEATURE_THREADS
28 |
29 | #endif // OHMMUTEX_H
30 |
--------------------------------------------------------------------------------
/ohm/NdtMode.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "NdtMode.h"
7 |
8 | #include
9 |
10 | namespace ohm
11 | {
12 | const std::array &ndtModeNames()
13 | {
14 | static const std::array mode_names = { "none", "occupancy", "traversability" };
15 | return mode_names;
16 | }
17 |
18 | std::string ndtModeToString(NdtMode mode)
19 | {
20 | if (unsigned(mode) < ndtModeNames().size())
21 | {
22 | return ndtModeNames()[int(mode)];
23 | }
24 |
25 | return "";
26 | }
27 |
28 |
29 | NdtMode ndtModeFromString(const std::string &str)
30 | {
31 | for (size_t i = 0; i < ndtModeNames().size(); ++i)
32 | {
33 | if (str == ndtModeNames()[i])
34 | {
35 | return NdtMode(i);
36 | }
37 | }
38 |
39 | return NdtMode::kNone;
40 | }
41 | } // namespace ohm
42 |
--------------------------------------------------------------------------------
/ohm/NdtMode.h:
--------------------------------------------------------------------------------
1 | //
2 | // Author: Kazys Stepanas
3 | // Copyright (c) CSIRO 2021
4 | //
5 | #ifndef OHM_NDTMODE_H
6 | #define OHM_NDTMODE_H
7 |
8 | #include "OhmConfig.h"
9 |
10 | #include
11 | namespace ohm
12 | {
13 | /// NDT mapping mode
14 | enum class NdtMode
15 | {
16 | kNone, ///< Ndt not in use.
17 | /// Ndt occupancy map mode. This uses the occupancy, mean and covariance layers.
18 | kOccupancy,
19 | /// Ndt traversability map. A super set of @c Mode::kOccupancy which adds intensity and covariance hit/miss count.
20 | kTraversability ///< Ndt traversability mode.
21 | };
22 |
23 | /// Convert an @c NdtMode value to a display string. @c "" on failure.
24 | std::string ohm_API ndtModeToString(NdtMode mode);
25 | /// Convert a string value to an @c NdtMode or @c NdtMode::kNone on failure. Inverse of @c ndtModeToString()
26 | NdtMode ohm_API ndtModeFromString(const std::string &str);
27 | } // namespace ohm
28 |
29 | #endif // OHM_NDTMODE_H
30 |
--------------------------------------------------------------------------------
/ohm/OccupancyType.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "OccupancyType.h"
7 |
8 | #include
9 |
10 | namespace ohm
11 | {
12 | const char *occupancyTypeToString(int occupancy_type)
13 | {
14 | static const std::array type_names = //
15 | {
16 | "null", //
17 | "uncertain", //
18 | "free", //
19 | "occupied" //
20 | };
21 |
22 | const int index = occupancy_type - ohm::kNull;
23 | if (index >= 0 && unsigned(index) < type_names.size())
24 | {
25 | return type_names[index];
26 | }
27 |
28 | return "";
29 | }
30 | } // namespace ohm
31 |
--------------------------------------------------------------------------------
/ohm/OccupancyType.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OCCUPANCYTYPE_H
7 | #define OCCUPANCYTYPE_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | /// An enumeration of the types of @c Voxel states available.
14 | enum OccupancyType
15 | {
16 | /// Invalid/null voxel.
17 | kNull = -2,
18 | /// Unobserved: no data recorded or available for the voxel.
19 | kUnobserved = -1,
20 | /// Know to be empty or free (traversable).
21 | kFree = 0,
22 | /// Occupied voxel.
23 | kOccupied = 1
24 | };
25 |
26 | const char ohm_API *occupancyTypeToString(int occupancy_type);
27 | } // namespace ohm
28 |
29 | #endif // OCCUPANCYTYPE_H_
30 |
--------------------------------------------------------------------------------
/ohm/OccupancyUtil.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "OccupancyUtil.h"
7 |
8 | #ifdef TES_ENABLE
9 | namespace ohm
10 | {
11 | tes::Server *g_tes = nullptr;
12 | } // namespace ohm
13 | #endif // TES_ENABLE
14 |
--------------------------------------------------------------------------------
/ohm/OccupancyUtil.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OCCUPANCYUTIL
7 | #define OCCUPANCYUTIL
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "Key.h"
12 |
13 | #include
14 |
15 | #include
16 |
17 |
18 | namespace ohm
19 | {
20 | // From GLM 0.9.9.1, numbering starting including a patch number.
21 | // So 0.9.8 was version 98, 0.9.9 was 99 and 0.9.9.1 was 991
22 | // This assumes version 1.0 will be 1000
23 | #if GLM_VERSION < 99
24 | using GlmQualifier = glm::precision;
25 | #else // GLM_VERSION
26 | using GlmQualifier = glm::qualifier;
27 | #endif // GLM_VERSION
28 |
29 | template
30 | inline T volumeOf(const glm::tvec3 &expanse)
31 | {
32 | return expanse.x * expanse.y * expanse.z;
33 | }
34 | } // namespace ohm
35 |
36 | #endif // OCCUPANCYUTIL
37 |
--------------------------------------------------------------------------------
/ohm/OhmConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMCONFIG_H
6 | #define OHMCONFIG_H
7 |
8 | #include "OhmExport.h"
9 |
10 | #ifndef _USE_MATH_DEFINES
11 | #define _USE_MATH_DEFINES
12 | #endif // _USE_MATH_DEFINES
13 | #ifndef NOMINMAX
14 | #define NOMINMAX
15 | #endif // NOMINMAX
16 |
17 | #ifdef _MSC_VER
18 | // Avoid dubious security warnings for plenty of legitimate code
19 | #ifndef _SCL_SECURE_NO_WARNINGS
20 | #define _SCL_SECURE_NO_WARNINGS
21 | #endif // _SCL_SECURE_NO_WARNINGS
22 | #ifndef _CRT_SECURE_NO_WARNINGS
23 | #define _CRT_SECURE_NO_WARNINGS
24 | #endif // _CRT_SECURE_NO_WARNINGS
25 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
26 | #endif // _MSC_VER
27 |
28 | #include
29 |
30 | // clang-format off
31 |
32 | // Enable various validation tests throughout this library.
33 | #cmakedefine OHM_VALIDATION
34 | #cmakedefine OHM_FEATURE_THREADS
35 | #cmakedefine OHM_PROFILE
36 | #cmakedefine OHM_EMBED_GPU_CODE
37 | #cmakedefine OHM_FEATURE_EIGEN
38 |
39 | #ifdef OHM_PROFILE
40 | #define PROFILING 1
41 | #endif // OHM_PROFILE
42 |
43 | #cmakedefine TES_ENABLE
44 | #ifdef TES_ENABLE
45 | namespace tes
46 | {
47 | class Server;
48 | } // namespace tes
49 | namespace ohm
50 | {
51 | /// Debug visualisation server pointer. Must be set by the executable to enable its use in this library.
52 | /// That is, this is considered a borrowed pointer in this library.
53 | extern tes::Server ohm_API *g_tes; // Symbol defined in occupancyutil.cpp.
54 | } // namespace ohm
55 | #endif // TES_ENABLE
56 |
57 | #include
58 |
59 | /// Enable experimental parts of GLM, like `glm::length2()` (length squared)
60 | #define GLM_ENABLE_EXPERIMENTAL
61 |
62 | // clang-format on
63 | #endif // OHMCONFIG_H
64 |
--------------------------------------------------------------------------------
/ohm/Query.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "Query.h"
7 |
8 | #include "private/QueryDetail.h"
9 |
10 | #include "QueryFlag.h"
11 |
12 | namespace ohm
13 | {
14 | Query::Query(QueryDetail *detail)
15 | : imp_(detail)
16 | {
17 | if (!imp_)
18 | {
19 | imp_ = new QueryDetail;
20 | }
21 | }
22 |
23 |
24 | Query::~Query()
25 | {
26 | delete imp_;
27 | imp_ = nullptr;
28 | }
29 |
30 |
31 | const OccupancyMap *Query::map() const
32 | {
33 | return imp_->map;
34 | }
35 |
36 |
37 | void Query::setMap(OccupancyMap *map)
38 | {
39 | imp_->map = map;
40 | onSetMap();
41 | }
42 |
43 |
44 | unsigned Query::queryFlags() const
45 | {
46 | return imp_->query_flags;
47 | }
48 |
49 |
50 | void Query::setQueryFlags(unsigned flags)
51 | {
52 | imp_->query_flags = flags;
53 | }
54 |
55 |
56 | size_t Query::numberOfResults() const
57 | {
58 | return imp_->number_of_results;
59 | }
60 |
61 |
62 | const Key *Query::intersectedVoxels() const
63 | {
64 | return (!imp_->intersected_voxels.empty()) ? imp_->intersected_voxels.data() : nullptr;
65 | }
66 |
67 |
68 | const double *Query::ranges() const
69 | {
70 | return (!imp_->ranges.empty()) ? imp_->ranges.data() : nullptr;
71 | }
72 |
73 |
74 | bool Query::execute()
75 | {
76 | reset(false);
77 | return onExecute();
78 | }
79 |
80 |
81 | bool Query::executeAsync()
82 | {
83 | return onExecuteAsync();
84 | }
85 |
86 |
87 | void Query::reset(bool hard_reset)
88 | {
89 | wait();
90 | imp_->intersected_voxels.clear();
91 | imp_->ranges.clear();
92 | imp_->number_of_results = 0u;
93 | onReset(hard_reset);
94 | }
95 |
96 |
97 | bool Query::wait(unsigned timeout_ms)
98 | {
99 | return onWaitAsync(timeout_ms);
100 | }
101 |
102 |
103 | bool Query::onWaitAsync(unsigned /*timeout_ms*/)
104 | {
105 | return false;
106 | }
107 | } // namespace ohm
108 |
--------------------------------------------------------------------------------
/ohm/RayMapper.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "RayMapper.h"
7 |
8 | namespace ohm
9 | {
10 | RayMapper::RayMapper() = default;
11 |
12 | RayMapper::~RayMapper() = default;
13 | } // namespace ohm
14 |
--------------------------------------------------------------------------------
/ohm/RayPatternConical.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef RAYPATTERNCONICAL_H
7 | #define RAYPATTERNCONICAL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "RayPattern.h"
12 |
13 | namespace ohm
14 | {
15 | /// A conical @c RayPattern construction
16 | class ohm_API RayPatternConical : public RayPattern
17 | {
18 | public:
19 | /// Constructor.
20 | /// @param cone_axis The cone's primary axis/directory - expected to be a unit vector.
21 | /// @param cone_angle The angle between the cone's axis and the cone wall (radians).
22 | /// @param range Length of the rays in the cone. This makes the cone bottom spherical.
23 | /// @param angular_resolution Approximate angular separation between rays (radians).
24 | /// @param min_range Controls the near point of each line segment. Zero starts all rays at the apex, while
25 | /// increasing this value moves the starting points out. Must be less than @p range .
26 | RayPatternConical(const glm::dvec3 &cone_axis, double cone_angle, double range, double angular_resolution,
27 | double min_range = 0);
28 | };
29 | } // namespace ohm
30 |
31 | #endif // RAYPATTERNCONICAL_H
32 |
--------------------------------------------------------------------------------
/ohm/Trace.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "Trace.h"
7 |
8 | #include <3esservermacros.h>
9 |
10 | void ohm::trace::init(const std::string &file_stream)
11 | {
12 | (void)file_stream;
13 | // Initialise TES
14 | TES_SETTINGS(settings, tes::SF_Compress | tes::SF_Collate);
15 | // Initialise server info.
16 | TES_SERVER_INFO(info, tes::XYZ);
17 | TES_STMT(info.defaultFrameTime = 1);
18 | // Create the server. Use tesServer declared globally above.
19 | TES_SERVER_CREATE(ohm::g_tes, settings, &info);
20 |
21 | // Start the server and wait for the connection monitor to start.
22 | TES_SERVER_START(ohm::g_tes, tes::ConnectionMonitor::Asynchronous);
23 |
24 | TES_IF(!file_stream.empty()) { TES_LOCAL_FILE_STREAM(ohm::g_tes, file_stream.c_str()); }
25 | TES_SERVER_START_WAIT(g_tes, 1000);
26 |
27 | TES_CATEGORY(g_tes, "Map", kTcMap, 0, true);
28 | TES_CATEGORY(g_tes, "Rays", kTcRays, kTcMap, true);
29 | TES_CATEGORY(g_tes, "Voxels", kTcVoxels, kTcMap, true);
30 | TES_CATEGORY(g_tes, "Ndt", kTcNdt, kTcMap, true);
31 | TES_CATEGORY(g_tes, "Heightmap", kTcHeightmap, 0, true);
32 | TES_CATEGORY(g_tes, "Voxels", kTcHmVoxel, kTcHeightmap, true);
33 | TES_CATEGORY(g_tes, "Surface", kTcHmSurface, kTcHmVoxel, true);
34 | TES_CATEGORY(g_tes, "Vacant", kTcHmVacant, kTcHmVoxel, true);
35 | TES_CATEGORY(g_tes, "Virtual", kTcHmVirtualSurface, kTcHmVoxel, true);
36 | TES_CATEGORY(g_tes, "Clearance", kTcHmClearance, kTcHeightmap, false);
37 | TES_CATEGORY(g_tes, "Visit", kTcHmVisit, kTcHeightmap, true);
38 | TES_CATEGORY(g_tes, "Info", kTcHmInfo, kTcHeightmap, true);
39 | }
40 |
41 |
42 | void ohm::trace::done()
43 | {
44 | TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
45 | TES_SERVER_STOP(ohm::g_tes);
46 | }
47 |
48 |
49 | bool ohm::trace::available()
50 | {
51 | #ifdef TES_ENABLE
52 | return true;
53 | #else // TES_ENABLE
54 | return false;
55 | #endif // TES_ENABLE
56 | }
57 |
--------------------------------------------------------------------------------
/ohm/Voxel.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "Voxel.h"
7 |
--------------------------------------------------------------------------------
/ohm/VoxelData.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMVOXELDATA_H
7 | #define OHMVOXELDATA_H
8 |
9 | // A convenience header for including supporting data and functions for all known @c ohm voxel types.
10 |
11 | #include "CovarianceVoxel.h"
12 | #include "DefaultLayer.h"
13 | #include "OccupancyType.h"
14 | #include "Voxel.h"
15 | #include "VoxelMean.h"
16 | #include "VoxelOccupancy.h"
17 | #include "VoxelSecondarySample.h"
18 | #include "VoxelTsdf.h"
19 |
20 | #endif // OHMVOXELDATA_H
21 |
--------------------------------------------------------------------------------
/ohm/VoxelIncident.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_VOXEL_INCIDENT_H
7 | #define OHM_VOXEL_INCIDENT_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | namespace ohm
14 | {
15 | #include "VoxelIncidentCompute.h"
16 | }
17 |
18 | #endif // OHM_VOXEL_INCIDENT_H
19 |
--------------------------------------------------------------------------------
/ohm/VoxelTouchTime.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_VOXEL_TOUCH_TIME_H
7 | #define OHM_VOXEL_TOUCH_TIME_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | #include "VoxelTouchTimeCompute.h"
14 | }
15 |
16 | #endif // OHM_VOXEL_TOUCH_TIME_H
17 |
--------------------------------------------------------------------------------
/ohm/VoxelTouchTimeCompute.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_VOXEL_TOUCH_TIME_COMPUTE_H
7 | #define OHM_VOXEL_TOUCH_TIME_COMPUTE_H
8 |
9 | // Do not include config header. This can be used from GPU code.
10 |
11 | #if !GPUTIL_DEVICE
12 | #define OHM_DEVICE_HOST
13 | #else // GPUTIL_DEVICE
14 | #define OHM_DEVICE_HOST __device__ __host__
15 | #endif // GPUTIL_DEVICE
16 |
17 | /// Encode as milliseconds
18 | #define OHM_VOXEL_TOUCH_TIME_SCALE 0.001
19 |
20 | /// Encode a voxel timestamp. Encoded as milliseconds since @p timebase .
21 | /// @param timebase The base time to encode relative to.
22 | /// @param timestamp The timestamp to encode.
23 | /// @return Milliseconds elapsed between @p timebase and @p timestamp .
24 | inline unsigned OHM_DEVICE_HOST encodeVoxelTouchTime(double timebase, double timestamp)
25 | {
26 | return (unsigned)((timestamp - timebase) / OHM_VOXEL_TOUCH_TIME_SCALE);
27 | }
28 |
29 | /// Decode a voxel timestamp. Decoded as milliseconds since @p timebase .
30 | /// @param timebase The base time to decode relative to.
31 | /// @param touch_time Encoded time - milliseconds elapsed since @p timebase .
32 | /// @return The decoded tiemstamp.
33 | inline double OHM_DEVICE_HOST decodeVoxelTouchTime(double timebase, unsigned touch_time)
34 | {
35 | return touch_time * OHM_VOXEL_TOUCH_TIME_SCALE + timebase;
36 | }
37 |
38 | #undef OHM_DEVICE_HOST
39 |
40 | #endif // OHM_VOXEL_TOUCH_TIME_COMPUTE_H
41 |
--------------------------------------------------------------------------------
/ohm/VoxelTsdf.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "VoxelTsdf.h"
7 |
8 | #include "MapInfo.h"
9 |
10 | namespace ohm
11 | {
12 | void updateMapInfo(MapInfo &info, const TsdfOptions &tsdf_options)
13 | {
14 | info.set(MapValue("tsdf-max-weight", tsdf_options.max_weight));
15 | info.set(MapValue("tsdf-default-truncation-distance", tsdf_options.default_truncation_distance));
16 | info.set(MapValue("tsdf-dropoff-epsilon", tsdf_options.dropoff_epsilon));
17 | info.set(MapValue("tsdf-sparsity-compensation-factor", tsdf_options.sparsity_compensation_factor));
18 | }
19 |
20 |
21 | void fromMapInfo(TsdfOptions &tsdf_options, const MapInfo &info)
22 | {
23 | tsdf_options.max_weight =
24 | static_cast(info.get("tsdf-max-weight", MapValue("tsdf-max-weight", tsdf_options.max_weight)));
25 | tsdf_options.default_truncation_distance = static_cast(
26 | info.get("tsdf-default-truncation-distance",
27 | MapValue("tsdf-default-truncation-distance", tsdf_options.default_truncation_distance)));
28 | tsdf_options.dropoff_epsilon = static_cast(
29 | info.get("tsdf-dropoff-epsilon", MapValue("tsdf-dropoff-epsilon", tsdf_options.dropoff_epsilon)));
30 | tsdf_options.sparsity_compensation_factor = static_cast(
31 | info.get("tsdf-sparsity-compensation-factor",
32 | MapValue("tsdf-sparsity-compensation-factor", tsdf_options.sparsity_compensation_factor)));
33 | }
34 | } // namespace ohm
35 |
--------------------------------------------------------------------------------
/ohm/VoxelTsdf.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef VOXELTSDF_H
7 | #define VOXELTSDF_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "Voxel.h"
12 |
13 | #include
14 |
15 | /// @defgroup voxeltsdf Voxel Truncated Signed Distance Fields
16 | /// These functions are used to manipulate the voxel TSDF values. @c VoxelTsdf allows a voxel to store
17 | /// a weight and distance value for the TSDF algorithm.
18 | ///
19 | /// The TSDF algorithm used here is based on Voxblox TSDF and is licensed under BSD 3-clause license.
20 | ///
21 |
22 | namespace ohm
23 | {
24 | class MapInfo;
25 |
26 | /// TSDF mapping options.
27 | struct TsdfOptions
28 | {
29 | /// Maximum TSDF voxel weight.
30 | float max_weight = 1e4f;
31 | /// Default truncation distance.
32 | float default_truncation_distance = 0.1f;
33 | /// Dropoff for the dropoff adjustment. Zero or negative to disable.
34 | float dropoff_epsilon = 0.0f;
35 | /// Compensation for sparse data sets. Zero or negative to disable.
36 | float sparsity_compensation_factor = 1.0;
37 | };
38 |
39 | /// Write the given tsdf options to @c MapInfo .
40 | void ohm_API updateMapInfo(MapInfo &info, const TsdfOptions &tsdf_options);
41 |
42 | /// Read the given tsdf options from @c MapInfo .
43 | void ohm_API fromMapInfo(TsdfOptions &tsdf_options, const MapInfo &info);
44 |
45 | #include "VoxelTsdfCompute.h"
46 | } // namespace ohm
47 |
48 | #endif // VOXELTSDF_H
49 |
--------------------------------------------------------------------------------
/ohm/private/ClearingPatternDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef CLEARINGPATTERNDETAIL_H
7 | #define CLEARINGPATTERNDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | class RayPattern;
18 |
19 | struct ClearingPatternDetail
20 | {
21 | std::vector ray_set;
22 | const RayPattern *pattern = nullptr;
23 | unsigned ray_flags = 0u; // Default value should be ClearingPattern::kDefaultFlags.
24 | bool has_pattern_ownership = false;
25 | };
26 | } // namespace ohm
27 |
28 | #endif // CLEARINGPATTERNDETAIL_H
29 |
--------------------------------------------------------------------------------
/ohm/private/LineKeysQueryDetail.h:
--------------------------------------------------------------------------------
1 |
2 | // Copyright (c) 2017
3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
4 | // ABN 41 687 119 230
5 | //
6 | // Author: Kazys Stepanas
7 | #ifndef OHM_LINEKEYSQUERYDETAIL_H_
8 | #define OHM_LINEKEYSQUERYDETAIL_H_
9 |
10 | #include "OhmConfig.h"
11 |
12 | #include "QueryDetail.h"
13 |
14 | namespace ohm
15 | {
16 | /// Pimpl data for @c LineKeysQuery
17 | struct ohm_API LineKeysQueryDetail : QueryDetail
18 | {
19 | std::vector rays; ///< Ray origin/end point pairs to query for.
20 | /// Results vector, identifying the offsets for each ray into @c intersected_voxels where the results for that ray
21 | /// begin.
22 | std::vector result_indices;
23 | /// Results vector, identifying the number of voxels for each ray in @c intersected_voxels.
24 | std::vector result_counts;
25 | };
26 | } // namespace ohm
27 |
28 | #endif // OHM_LINEKEYSQUERYDETAIL_H_
29 |
--------------------------------------------------------------------------------
/ohm/private/LineQueryDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_LINEQUERYDETAIL_H
7 | #define OHM_LINEQUERYDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "QueryDetail.h"
12 |
13 | #include "ohm/KeyList.h"
14 |
15 | namespace ohm
16 | {
17 | class ClearanceProcess;
18 |
19 | struct ohm_API LineQueryDetail : QueryDetail
20 | {
21 | glm::dvec3 start_point = glm::dvec3(0);
22 | glm::dvec3 end_point = glm::dvec3(0);
23 | // Internal: calculated on execute.
24 | glm::dvec3 segment_dir = glm::dvec3(0);
25 | glm::dvec3 axis_scaling = glm::dvec3(1, 1, 1);
26 | KeyList segment_keys;
27 | // Internal: calculated on execute.
28 | double segment_length = 0;
29 | /// Range reported for unobstructed voxels.
30 | float default_range = -1;
31 | float search_radius = 0;
32 | };
33 | } // namespace ohm
34 |
35 | #endif // OHM_LINEQUERYDETAIL_H
36 |
--------------------------------------------------------------------------------
/ohm/private/MapLayerDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_MAPLAYERDETAIL_H
7 | #define OHM_MAPLAYERDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | namespace ohm
14 | {
15 | struct VoxelLayoutDetail;
16 |
17 | struct ohm_API MapLayerDetail
18 | {
19 | std::string name;
20 | VoxelLayoutDetail *voxel_layout = nullptr;
21 | uint16_t layer_index = 0;
22 | uint16_t subsampling = 0;
23 | unsigned flags = 0;
24 | };
25 | } // namespace ohm
26 |
27 | #endif // OHM_MAPLAYERDETAIL_H
28 |
--------------------------------------------------------------------------------
/ohm/private/MapLayoutDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_MAPLAYOUTDETAIL_H
7 | #define OHM_MAPLAYOUTDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "ohm/MapLayer.h"
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | struct ohm_API MapLayoutDetail
18 | {
19 | std::vector layers;
20 | int occupancy_layer = -1;
21 | int mean_layer = -1;
22 | int traversal_layer = -1;
23 | int covariance_layer = -1;
24 | int clearance_layer = -1;
25 | int intensity_layer = -1;
26 | int hit_miss_count_layer = -1;
27 |
28 | inline ~MapLayoutDetail() { clear(); }
29 |
30 | inline void clear()
31 | {
32 | for (MapLayer *layer : layers)
33 | {
34 | delete layer;
35 | }
36 | layers.clear();
37 | occupancy_layer = mean_layer = traversal_layer = covariance_layer = clearance_layer = intensity_layer =
38 | hit_miss_count_layer = -1;
39 | }
40 | };
41 | } // namespace ohm
42 |
43 | #endif // OHM_MAPLAYOUTDETAIL_H
44 |
--------------------------------------------------------------------------------
/ohm/private/MapperDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPPERDETAIL_H_
7 | #define MAPPERDETAIL_H_
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | namespace ohm
14 | {
15 | class MappingProcess;
16 | class OccupancyMap;
17 |
18 | struct ohm_API MapperDetail
19 | {
20 | std::vector processes;
21 | OccupancyMap *map = nullptr;
22 | unsigned next_process = 0;
23 | };
24 | } // namespace ohm
25 |
26 | #endif // MAPPERDETAIL_H_
27 |
--------------------------------------------------------------------------------
/ohm/private/NdtMapDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef NDTMAPDETAIL_H
7 | #define NDTMAPDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "MapProbability.h"
12 | #include "NdtMode.h"
13 |
14 | namespace ohm
15 | {
16 | class OccupancyMap;
17 |
18 | /// Internal details associated with a @c NdtMap extension to an @c OccupancyMap .
19 | struct NdtMapDetail
20 | {
21 | /// The target occupancy map.
22 | OccupancyMap *map = nullptr;
23 | /// Range sensor noise estimate
24 | float sensor_noise = 0.05f; // NOLINT(readability-magic-numbers)
25 | /// Number of samples required before using NDT logic in a miss integration.
26 | unsigned sample_threshold = 3;
27 | /// Rate at which ray intersections with NDT ellipsoids errode voxels. Range [0, 1] with 1 yielding stronger
28 | /// effects. Initialise to zero here. Should be reset on NDT map construction to match the normal occupancy miss
29 | /// value by default, otherwise we will yield stronger erosion from NDT misses.
30 | ///
31 | /// See NdtMap::ndtAdaptationRateFromMissProbability()
32 | float adaptation_rate = 0; // NOLINT(readability-magic-numbers)
33 | /// Low probability value threshold used to re-initialise covariance matrix and mean.
34 | /// Used with @c reinitialise_covariance_point_count in @c calculateHitWithCovariance()
35 | float reinitialise_covariance_threshold = probabilityToValue(0.2f); // NOLINT(readability-magic-numbers)
36 | /// Upper point count limit required to reinitialise the covariance matrix. Used with
37 | /// @c reinitialise_covariance_threshold in @c calculateHitWithCovariance()
38 | unsigned reinitialise_covariance_point_count = 100;
39 | /// Covariance of intensity (for NDT-TM) for initialisation upon receipt of first hit.
40 | float initial_intensity_covariance = 1.0f;
41 | /// The NDT mapping mode. Must not be @c kNone in the @c NdtMap usage case.
42 | NdtMode mode = NdtMode::kOccupancy;
43 | /// True if @p map is a borrowed pointer, false to take ownership and delete it.
44 | bool borrowed_map = false;
45 | /// Debug tracing enabled? Requires 3es
46 | bool trace = false;
47 | };
48 | } // namespace ohm
49 |
50 | #endif // NDTMAPDETAIL_H
51 |
--------------------------------------------------------------------------------
/ohm/private/NearestNeighboursDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_NEARESTNEIGHBOURSDETAIL_H
7 | #define OHM_NEARESTNEIGHBOURSDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "QueryDetail.h"
12 |
13 | namespace ohm
14 | {
15 | struct ohm_API NearestNeighboursDetail : QueryDetail
16 | {
17 | glm::dvec3 near_point = glm::dvec3(0);
18 | float search_radius = 0;
19 | };
20 | } // namespace ohm
21 |
22 | #endif // OHM_NEARESTNEIGHBOURSDETAIL_H
23 |
--------------------------------------------------------------------------------
/ohm/private/QueryDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_QUERYDETAIL_H
7 | #define OHM_QUERYDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "ohm/Key.h"
12 |
13 | #include
14 | #include
15 |
16 | namespace ohm
17 | {
18 | class OccupancyMap;
19 |
20 | /// Pimpl data for @c Query objects .
21 | struct ohm_API QueryDetail
22 | {
23 | /// The map to perform the query on.
24 | OccupancyMap *map = nullptr;
25 | /// The voxels intersected by the query, identified by their @c Key (if used).
26 | std::vector intersected_voxels;
27 | /// Distances associated with the query results (if used).
28 | std::vector ranges;
29 | /// Number of results in @c intersected_voxels and/or @c ranges .
30 | size_t number_of_results = 0;
31 | /// @c QueryFlag values for the query.
32 | unsigned query_flags = 0;
33 |
34 | /// Virtual destructor.
35 | virtual ~QueryDetail() = default;
36 | };
37 |
38 | /// Query result helper identifying the closest result index.
39 | struct ohm_API ClosestResult
40 | {
41 | size_t index = 0; ///< Index into the results arrays.
42 | double range = std::numeric_limits::max(); ///< (Closest) range value.
43 | };
44 | } // namespace ohm
45 |
46 | #endif // OHM_QUERYDETAIL_H
47 |
--------------------------------------------------------------------------------
/ohm/private/RayPatternDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef RAYPATTERNDETAIL_H
7 | #define RAYPATTERNDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | struct RayPatternDetail
18 | {
19 | /// Ray start/end point pairs in sensor space.
20 | std::vector sample_pairs;
21 | };
22 | } // namespace ohm
23 |
24 | #endif // RAYPATTERNDETAIL_H
25 |
--------------------------------------------------------------------------------
/ohm/private/RaysQueryDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_RAYSQUERYDETAIL_H
7 | #define OHM_RAYSQUERYDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "QueryDetail.h"
12 |
13 | #include
14 |
15 | #include
16 |
17 | namespace ohm
18 | {
19 | struct ohm_API RaysQueryDetail : QueryDetail
20 | {
21 | /// Set of origin/end point pairs to lookup in the map.
22 | std::vector rays_in;
23 | std::vector unobserved_volumes_out;
24 | std::vector terminal_states_out;
25 | double volume_coefficient = 1.0f;
26 | int occupancy_layer = -1; ///< Cached occupancy layer index.
27 | glm::u8vec3 occupancy_dim{ 0, 0, 0 }; ///< Cached occupancy layer voxel dimensions. Voxel mean must exactly match.
28 | bool valid_layers = false; ///< Has layer validation passed?
29 | };
30 | } // namespace ohm
31 |
32 | #endif // OHM_RAYSQUERYDETAIL_H
33 |
--------------------------------------------------------------------------------
/ohm/private/SerialiseUtil.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef SERIALISEUTIL_H
7 | #define SERIALISEUTIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "ohm/Stream.h"
12 |
13 | namespace ohm
14 | {
15 | /// Explicitly typed stream writing, uncompressed.
16 | template
17 | inline bool writeUncompressed(OutputStream &stream, const S &val)
18 | {
19 | const T val2 = static_cast(val);
20 | return stream.writeUncompressed(&val2, unsigned(sizeof(val2))) == sizeof(val2);
21 | }
22 |
23 |
24 | /// Explicitly typed stream writing.
25 | template
26 | inline bool write(OutputStream &stream, const S &val)
27 | {
28 | const T val2 = static_cast(val);
29 | return stream.write(&val2, unsigned(sizeof(val2))) == sizeof(val2);
30 | }
31 |
32 |
33 | /// Explicitly typed stream reading, uncompressed.
34 | template
35 | inline bool readRaw(InputStream &stream, S &val)
36 | {
37 | T val2{ 0 };
38 | if (stream.readRaw(&val2, unsigned(sizeof(val2))) != sizeof(val2))
39 | {
40 | return false;
41 | }
42 | val = static_cast(val2);
43 | return true;
44 | }
45 |
46 |
47 | /// Explicitly typed stream reading.
48 | template
49 | inline bool read(InputStream &stream, S &val)
50 | {
51 | T val2{ 0 };
52 | if (stream.read(&val2, unsigned(sizeof(val2))) != sizeof(val2))
53 | {
54 | return false;
55 | }
56 | val = static_cast(val2);
57 | return true;
58 | }
59 | } // namespace ohm
60 |
61 | #endif // SERIALISEUTIL_H
62 |
--------------------------------------------------------------------------------
/ohm/private/VoxelLayoutDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHM_VOXELLAYOUTDETAIL_H
7 | #define OHM_VOXELLAYOUTDETAIL_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 | #include
13 |
14 | namespace ohm
15 | {
16 | struct ohm_API VoxelMember
17 | {
18 | /// Voxel member name field. Sized to set the overall structure size to 64 bytes.
19 | std::array name; // NOLINT(readability-magic-numbers)
20 | uint64_t clear_value;
21 | uint16_t type;
22 | uint16_t offset;
23 | };
24 |
25 | struct ohm_API VoxelLayoutDetail
26 | {
27 | std::vector members;
28 | uint16_t next_offset = 0u;
29 | uint16_t voxel_byte_size = 0u;
30 | };
31 | } // namespace ohm
32 |
33 | #endif // OHM_VOXELLAYOUTDETAIL_H
34 |
--------------------------------------------------------------------------------
/ohm/serialise/MapSerialiseV0.1.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPSERIALISEV0_1_H
7 | #define MAPSERIALISEV0_1_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include
12 |
13 | namespace ohm
14 | {
15 | class InputStream;
16 | struct MapChunk;
17 | struct MapVersion;
18 | struct OccupancyMapDetail;
19 | class SerialiseProgress;
20 |
21 | using ChunkFunc = std::function;
22 |
23 | namespace v0_1
24 | {
25 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
26 | size_t region_count);
27 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
28 | size_t region_count, const ChunkFunc &load_chunk);
29 | int loadLayout(InputStream &stream, OccupancyMapDetail &map);
30 | int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail);
31 | } // namespace v0_1
32 | } // namespace ohm
33 |
34 | #endif // MAPSERIALISEV0_1_H
35 |
--------------------------------------------------------------------------------
/ohm/serialise/MapSerialiseV0.2.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPSERIALISEV0_2_H
7 | #define MAPSERIALISEV0_2_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "MapSerialiseV0.1.h"
12 |
13 | namespace ohm
14 | {
15 | class InputStream;
16 | struct MapChunk;
17 | class MapInfo;
18 | class MapValue;
19 | struct MapVersion;
20 | struct OccupancyMapDetail;
21 | class SerialiseProgress;
22 |
23 | namespace v0_2
24 | {
25 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
26 | size_t region_count);
27 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
28 | size_t region_count, const ChunkFunc &load_chunk);
29 |
30 | int loadMapInfo(InputStream &in, MapInfo &info);
31 | int loadItem(InputStream &in, MapValue &value);
32 | } // namespace v0_2
33 | } // namespace ohm
34 |
35 | #endif // MAPSERIALISEV0_2_H
36 |
--------------------------------------------------------------------------------
/ohm/serialise/MapSerialiseV0.4.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPSERIALISEV0_4_H
7 | #define MAPSERIALISEV0_4_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "MapSerialiseV0.1.h"
12 |
13 | namespace ohm
14 | {
15 | class InputStream;
16 | struct MapChunk;
17 | struct MapVersion;
18 | struct OccupancyMapDetail;
19 | class SerialiseProgress;
20 |
21 | namespace v0_4
22 | {
23 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
24 | size_t region_count);
25 |
26 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
27 | size_t region_count, const ChunkFunc &load_chunk);
28 |
29 | int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail);
30 | } // namespace v0_4
31 | } // namespace ohm
32 |
33 | #endif // MAPSERIALISEV0_3_H
34 |
--------------------------------------------------------------------------------
/ohm/serialise/MapSerialiseV0.5.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "MapSerialiseV0.5.h"
7 |
--------------------------------------------------------------------------------
/ohm/serialise/MapSerialiseV0.5.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPSERIALISEV0_5_H
7 | #define MAPSERIALISEV0_5_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | #include "MapSerialiseV0.4.h"
12 |
13 | namespace ohm
14 | {
15 | namespace v0_5
16 | {
17 | inline int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
18 | size_t region_count)
19 | {
20 | return v0_4::load(stream, detail, progress, version, region_count);
21 | }
22 | } // namespace v0_5
23 | } // namespace ohm
24 |
25 | #endif // MAPSERIALISEV0_3_H
26 |
--------------------------------------------------------------------------------
/ohm/serialise/MapSerialiseV0.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef MAPSERIALISEV0_H
7 | #define MAPSERIALISEV0_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | class InputStream;
14 | struct MapChunk;
15 | struct MapVersion;
16 | struct OccupancyMapDetail;
17 | class SerialiseProgress;
18 |
19 | namespace v0
20 | {
21 | int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
22 | size_t region_count);
23 | int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail);
24 | } // namespace v0
25 | } // namespace ohm
26 |
27 | #endif // MAPSERIALISEV0_H
28 |
--------------------------------------------------------------------------------
/ohmapp/OhmAppConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMAPPCONFIG_H
6 | #define OHMAPPCONFIG_H
7 |
8 | #include "OhmAppExport.h"
9 |
10 | #include
11 |
12 | #endif // OHMAPPCONFIG_H
13 |
--------------------------------------------------------------------------------
/ohmapp/OhmAppGpuConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMAPPGPUCONFIG_H
6 | #define OHMAPPGPUCONFIG_H
7 |
8 | #include "OhmAppGpuExport.h"
9 |
10 | #include
11 |
12 | #endif // OHMAPPGPUCONFIG_H
13 |
--------------------------------------------------------------------------------
/ohmapp/ohmappmain.inl:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | //
7 | // Provdes a structure for an application which populates a map using MapHarness
8 | #include "MapHarness.h"
9 |
10 | #include
11 | #include
12 | #include
13 | #include
14 |
15 | namespace
16 | {
17 | /// Global cache of @c ohmapp::MapHarness::quitLevelPtr() updated in @c onSignal()
18 | unsigned *g_quit = nullptr;
19 |
20 | /// Handle @c SIGINT and @c SIGTERM by incrementing @c g_quit
21 | void onSignal(int arg)
22 | {
23 | if (arg == SIGINT || arg == SIGTERM)
24 | {
25 | if (g_quit)
26 | {
27 | ++(*g_quit);
28 | }
29 | }
30 | }
31 | } // namespace
32 |
33 | /// Main program implementation.
34 | ///
35 | /// @param argc Command line argument count.
36 | /// @param argv Command line arguments.
37 | /// @param populator Map generation option.
38 | int ohmappMain(int argc, const char *const *argv, ohmapp::MapHarness &populator)
39 | {
40 | g_quit = populator.quitLevelPtr();
41 | signal(SIGINT, onSignal);
42 | signal(SIGTERM, onSignal);
43 |
44 | std::cout.imbue(std::locale(""));
45 |
46 | int exit_code = populator.parseCommandLineOptions(argc, argv);
47 | if (exit_code)
48 | {
49 | return exit_code;
50 | }
51 |
52 | exit_code = populator.run();
53 | return exit_code;
54 | }
55 |
56 | /// Overload using the template type as a @c ohmapp::MapHarness .
57 | /// @param argc Command line argument count.
58 | /// @param argv Command line arguments.
59 | /// @param data_source Data source object.
60 | template
61 | int ohmappMain(int argc, const char *const *argv, std::shared_ptr data_source)
62 | {
63 | OhmPop populator(data_source);
64 | return ohmappMain(argc, argv, populator);
65 | }
66 |
67 | /// Overload using the template types as a @c ohmapp::MapHarness and @c ohmapp::DataSource .
68 | /// @param argc Command line argument count.
69 | /// @param argv Command line arguments.
70 | template
71 | int ohmappMain(int argc, const char *const *argv)
72 | {
73 | return ohmappMain(argc, argv, std::make_shared());
74 | }
75 |
--------------------------------------------------------------------------------
/ohmgpu/GpuCachePostSyncHandler.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUCACHEPOSTSYNCHANDLER_H
7 | #define GPUCACHEPOSTSYNCHANDLER_H
8 |
9 | #include
10 |
11 | #include
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | struct MapChunk;
18 |
19 | using GpuCachePostSyncHandler = std::function;
20 | } // namespace ohm
21 |
22 | #endif // GPUCACHEPOSTSYNCHANDLER_H
23 |
--------------------------------------------------------------------------------
/ohmgpu/GpuCacheStats.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMGPU_GPUCACHESTATS_H
7 | #define OHMGPU_GPUCACHESTATS_H
8 |
9 | #include "OhmGpuConfig.h"
10 |
11 | #include
12 |
13 | namespace ohm
14 | {
15 | /// Running stats on a @c GpuLayerCache .
16 | struct ohmgpu_API GpuCacheStats
17 | {
18 | uint32_t hits = 0; ///< Number of cache hits
19 | uint32_t misses = 0; ///< Number of cache misses.
20 | uint32_t full = 0; ///< Number of misses where the cache was full and something had to be dropped.
21 | };
22 | } // namespace ohm
23 |
24 | #endif // OHMGPU_GPUCACHESTATS_H
25 |
--------------------------------------------------------------------------------
/ohmgpu/GpuLayerCacheParams.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "GpuLayerCacheParams.h"
7 |
--------------------------------------------------------------------------------
/ohmgpu/GpuLayerCacheParams.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPULAYERCACHEPARAMS_H
7 | #define GPULAYERCACHEPARAMS_H
8 |
9 | #include "OhmGpuConfig.h"
10 |
11 | #include "GpuCachePostSyncHandler.h"
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | /// Flags used to create a @c GpuLayerCache.
18 | enum GpuLayerCacheFlag : unsigned
19 | {
20 | /// Will read voxel data from host to GPU memory.
21 | kGcfRead = (1u << 0u),
22 | /// Will write voxel data from GPU to host memory.
23 | kGcfWrite = (1u << 1u),
24 | /// Using buffers mappable to host memory? Can result in faster data transfer.
25 | kGcfMappable = (1u << 2u),
26 |
27 | /// Default creation flags.
28 | kGcfDefaultFlags = kGcfRead | kGcfMappable
29 | };
30 |
31 | /// Parameters used in creating a @c GpuCacheLayer in @c GpuCache::createCache().
32 | struct ohmgpu_API GpuLayerCacheParams
33 | {
34 | /// The size (bytes) of the GPU cache buffer. Use zero to choose the default size specified by the @c GpuCache.
35 | size_t gpu_mem_size = 0;
36 | /// The @c MapLayer index the cache pulls data from.
37 | int map_layer = 0;
38 | /// @c GpuLayerCacheFlag cache creation flags.
39 | unsigned flags = kGcfDefaultFlags;
40 | /// Callback invoked on synchronising memory back to CPU.
41 | /// The calling thread may not be the main thread, so the function must be threadsafe.
42 | GpuCachePostSyncHandler on_sync;
43 |
44 | /// Default constructor.
45 | inline GpuLayerCacheParams() = default;
46 | /// Construct with the given member values.
47 | /// @param mem_size Target GPU cache size.
48 | /// @param layer The @c MapLayer index which the GPU layer cache synchronised with.
49 | /// @param flags @c GpuLayerCacheFlag cache creation flags.
50 | /// @param on_sync Callback to invoke on synchronising back to CPU.
51 | inline GpuLayerCacheParams(size_t mem_size, int layer, unsigned flags,
52 | GpuCachePostSyncHandler on_sync = GpuCachePostSyncHandler())
53 | : gpu_mem_size(mem_size)
54 | , map_layer(layer)
55 | , flags(flags)
56 | , on_sync(std::move(on_sync))
57 | {}
58 | };
59 | } // namespace ohm
60 |
61 | #endif // GPULAYERCACHEPARAMS_H
62 |
--------------------------------------------------------------------------------
/ohmgpu/OhmGpuConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMGPUCONFIG_H
6 | #define OHMGPUCONFIG_H
7 |
8 | #include "OhmGpuExport.h"
9 |
10 | #include
11 |
12 | // clang-format off
13 |
14 | #define OHM_GPU_NONE 0
15 | #define OHM_GPU_OPENCL 1
16 | #define OHM_GPU_CUDA 2
17 |
18 | /// Target OpenCL standard. 'max' => maximum device version (min 1.2)
19 | #define OHM_OPENCL_STD "@OHM_OPENCL_STD@"
20 | /// OpenCL required features to enable OpenCL 2.0 code.
21 | #define OHM_OPENCL_2_FEATURES "@OHM_OPENCL_2_FEATURES@"
22 |
23 | // clang-format on
24 |
25 | #endif // OHMGPUCONFIG_H
26 |
--------------------------------------------------------------------------------
/ohmgpu/RaysQueryGpu.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "OhmGpuConfig.h"
7 |
8 | #include "GpuMap.h"
9 |
10 | #include
11 |
12 | namespace ohm
13 | {
14 | struct RaysQueryDetailGpu;
15 |
16 | /// A GPU implementation of the @c RaysQuery .
17 | ///
18 | /// The GPU overheads mean that this will be slower than the CPU implementation for low counts. GPU begins to outperform
19 | /// CPU when querying rays on the order of 2000 or more with significant benefits starting at 5000-10000 rays with ray
20 | /// lengths approximately 100 times the voxel resolution. Doubling the ray length may require an order of magnitude
21 | /// more sample rays to maintain GPU timing benefits.
22 | ///
23 | /// The actual timing results will vary depending on GPU API, GPU architecture, voxel size, query ray lengths and map
24 | /// environment.
25 | class ohmgpu_API RaysQueryGpu : public RaysQuery
26 | {
27 | protected:
28 | /// Constructor used for inherited objects. This supports deriving @p RaysQueryDetail into
29 | /// more specialised forms.
30 | /// @param detail pimple style data structure. When null, a @c RaysQueryDetail is allocated by
31 | /// this method.
32 | explicit RaysQueryGpu(RaysQueryDetailGpu *detail);
33 |
34 | public:
35 | /// Constructor. The map and rays must be set before using.
36 | RaysQueryGpu();
37 |
38 | /// Destructor.
39 | ~RaysQueryGpu() override;
40 |
41 | protected:
42 | void onSetMap() override;
43 | bool onExecute() override;
44 | bool onExecuteAsync() override;
45 | void onReset(bool hard_reset) override;
46 |
47 | /// Synchronise GPU results
48 | void sync();
49 |
50 | /// Internal pimpl data access.
51 | /// @return Pimpl data pointer.
52 | RaysQueryDetailGpu *imp();
53 | /// Internal pimpl data access.
54 | /// @return Pimpl data pointer.
55 | const RaysQueryDetailGpu *imp() const;
56 | };
57 | } // namespace ohm
58 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/AdjustOccupancy.cl:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef ADJUSTOCCUPANCY_CL
7 | #define ADJUSTOCCUPANCY_CL
8 |
9 | inline __device__ float calculateOccupancyAdjustment(const GpuKey *voxel_key, const GpuKey *end_key, bool is_end_voxel,
10 | bool is_sample_voxel, float voxel_resolution,
11 | LineWalkData *line_data)
12 | {
13 | // We need to handle broken ray segments. They will come through with is_end_voxel true, but is_sample_voxel false.
14 | // We need to make a zero adjustment in that case.
15 | const float adjustment = (!is_sample_voxel || line_data->region_update_flags & kRfEndPointAsFree) ?
16 | line_data->ray_adjustment :
17 | line_data->sample_adjustment;
18 | return (is_end_voxel && !is_sample_voxel) ? 0 : adjustment;
19 | }
20 |
21 | #endif // ADJUSTOCCUPANCY_CL
22 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/CovarianceHitNdt.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | #include "CovarianceHitNdt.cl"
11 |
12 | GPUTIL_CUDA_DEFINE_KERNEL(covarianceHitNdt);
13 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/CovarianceHitNdt_h.cl:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef OHMGPU_COVARIANCE_HIT_HDT_H_
8 | #define OHMGPU_COVARIANCE_HIT_HDT_H_
9 |
10 | #include "CovarianceVoxelCompute.h"
11 | // #include "VoxelMeanCompute.h"
12 |
13 | // #include "Regions.cl"
14 |
15 | typedef struct WorkItem_t
16 | {
17 | CovarianceVoxel cov;
18 | // Voxel mean relative to the voxel centre.
19 | float3 mean;
20 | uint sample_count;
21 | float occupancy;
22 | } WorkItem;
23 |
24 | // forward declarations
25 |
26 | void __device__ collateSample(WorkItem *work_item, float3 sensor, float3 sample, int3 region_dimensions,
27 | float voxel_resolution, float sample_adjustment, float occupied_threshold,
28 | float sensor_noise, float reinitialise_cov_threshold,
29 | unsigned reinitialise_cov_sample_count);
30 |
31 |
32 | void __device__ collateSample(WorkItem *work_item, float3 sensor, float3 sample, int3 region_dimensions,
33 | float voxel_resolution, float sample_adjustment, float occupied_threshold,
34 | float sensor_noise, float reinitialise_cov_threshold,
35 | unsigned reinitialise_cov_sample_count)
36 | {
37 | // The sample is currently relative to the voxel centre of the end voxel. This is the same reference frame we need
38 | // to calculate the quantised mean, so no change is required.
39 | if (calculateHitWithCovariance(&work_item->cov, &work_item->occupancy, sample, work_item->mean,
40 | work_item->sample_count, sample_adjustment, INFINITY, voxel_resolution,
41 | reinitialise_cov_threshold, reinitialise_cov_sample_count))
42 | {
43 | // Covariance matrix has reset. Reset the point count to clear the mean value.
44 | work_item->sample_count = 0;
45 | }
46 |
47 | const float one_on_count_plus_one = 1.0f / (float)(work_item->sample_count + 1);
48 | work_item->mean = (work_item->sample_count * work_item->mean + sample) * one_on_count_plus_one;
49 | ++work_item->sample_count;
50 | }
51 |
52 | #endif // OHMGPU_COVARIANCE_HIT_HDT_H_
53 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/LineKeys.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include
7 | #include
8 |
9 | #include "LineKeys.cl"
10 |
11 | GPUTIL_CUDA_DEFINE_KERNEL(calculateLines);
12 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/LineWalkMarkers.cl:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2022
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef LINE_WALK_MARKERS_CL
7 | #define LINE_WALK_MARKERS_CL
8 |
9 | /// Flags affecting line walking
10 | typedef enum LineWalkFlag
11 | {
12 | /// No options set: default behaviour.
13 | kLineWalkFlagNone = 0u,
14 | /// Skip reporting the voxel containing the start point if different from the end point?
15 | kExcludeStartVoxel = (1u << 0u),
16 | /// Skip reporting the voxel containing the end point if different from the start point.
17 | kExcludeEndVoxel = (1u << 1u),
18 | /// Option for reverse line walking. The line walks the segment in reverse.
19 | kLineWalkFlagReverse = (1u << 2u),
20 | /// Option to force reporting the end voxel last in a reverse walk. This occurs normally when @p kLineWalkFlagReverse
21 | /// is not set.
22 | kLineWalkFlagForReportEndLast = (1u << 3u)
23 | } LineWalkFlag;
24 |
25 | #endif // LINE_WALK_MARKERS_CL
26 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/RaysQuery.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | // Build base without voxel means
11 | #include "RaysQuery.cl"
12 |
13 | GPUTIL_CUDA_DEFINE_KERNEL(raysQuery);
14 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/RaysQueryResult.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMGPU_GPU_RAYQUERY_RESULT_H
7 | #define OHMGPU_GPU_RAYQUERY_RESULT_H
8 |
9 | #ifndef RQ_OccNull
10 | // TODO(KS): include OccupancyType.h or formalise the types.
11 | /// Invalid/null voxel.
12 | #define RQ_OccNull (-2)
13 | /// Unobserved: no data recorded or available for the voxel.
14 | #define RQ_OccUnobserved (-1)
15 | /// Know to be empty or free (traversable).
16 | #define RQ_OccFree (0)
17 | /// Occupied voxel.
18 | #define RQ_OccOccupied (1)
19 | #endif // RQ_OccNull
20 |
21 | /// Structure used to write results for a GpuRayQuery.
22 | typedef struct RaysQueryResult_t // NOLINT(readability-identifier-naming, modernize-use-using)
23 | {
24 | /// Range the ray successfully traverses.
25 | float range;
26 | /// Unobserved volume metric.
27 | float unobserved_volume;
28 | /// Occupancy type of the final voxel.
29 | int voxel_type;
30 | } RaysQueryResult;
31 |
32 | #endif // OHMGPU_GPU_RAYQUERY_RESULT_H
33 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/RegionUpdate.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | // Build base without voxel means
11 | #include "RegionUpdate.cl"
12 |
13 | GPUTIL_CUDA_DEFINE_KERNEL(regionRayUpdateOccupancy);
14 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/RegionUpdateNdt.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | // Build base with voxel means and NDT
11 | #define NDT
12 | #include "RegionUpdate.cl"
13 |
14 | GPUTIL_CUDA_DEFINE_KERNEL(regionRayUpdateNdt);
15 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/RoiRangeFill.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | // Build base without voxel means
11 | #include "RoiRangeFill.cl"
12 |
13 | GPUTIL_CUDA_DEFINE_KERNEL(seedRegionVoxels);
14 | GPUTIL_CUDA_DEFINE_KERNEL(seedFromOuterRegions);
15 | GPUTIL_CUDA_DEFINE_KERNEL(propagateObstacles);
16 | GPUTIL_CUDA_DEFINE_KERNEL(migrateResults);
17 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/TransformSamples.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | #include "TransformSamples.cl"
11 |
12 | GPUTIL_CUDA_DEFINE_KERNEL(transformTimestampedPoints);
13 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/TsdfUpdate.cu:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include
8 | #include
9 |
10 | // Build base without voxel means
11 | #include "TsdfUpdate.cl"
12 |
13 | GPUTIL_CUDA_DEFINE_KERNEL(tsdfRayUpdate);
14 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/VoxelIncident.cl:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #include "VoxelIncidentCompute.h"
8 |
9 | /// Update the voxel mean pattern at @p target_address by including the bit(s) from @p pattern_to_add.
10 | /// This is done using atomic operations.
11 | ///
12 | /// Each bit in the pattern indicates occupancy at a particular voxel mean location.
13 | /// @param voxel The @c VoxelMean to update.
14 | /// @param incident_ray The incident ray from sensor to sample (un-normalised).
15 | /// @param sample_count How many samples in the voxel *before* adding the current one.
16 | __device__ void updateVoxelIncident(__global atomic_uint *voxel, float3 incident_ray, uint sample_count);
17 |
18 | #ifndef VOXEL_INCIDENT_CL
19 | #define VOXEL_INCIDENT_CL
20 |
21 | //------------------------------------------------------------------------------
22 | // Functions
23 | //------------------------------------------------------------------------------
24 |
25 | // Psuedo header guard to prevent function implementation duplication.
26 | inline __device__ void updateVoxelIncident(__global atomic_uint *voxel, float3 incident_ray, uint sample_count)
27 | {
28 | uint old_value;
29 | uint new_value;
30 |
31 | // Few iterations as it's less important to get this right.
32 | const int iteration_limit = 10;
33 | int iteration_count = 0;
34 |
35 | // First update the mean position, then the point count. There is a level of contension here, and the results may be
36 | // somewhat out.
37 | do
38 | {
39 | old_value = gputilAtomicLoadU32(voxel);
40 | new_value = updateIncidentNormal(old_value, incident_ray, sample_count);
41 | ++iteration_count;
42 | } while (!gputilAtomicCasU32(voxel, old_value, new_value) && iteration_limit < iteration_count);
43 | }
44 |
45 | #endif // VOXEL_INCIDENT_CL
46 |
--------------------------------------------------------------------------------
/ohmgpu/gpu/VoxelMean.cl:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | /// Update the voxel mean pattern at @p target_address by including the bit(s) from @p pattern_to_add.
8 | /// This is done using atomic operations.
9 | ///
10 | /// Each bit in the pattern indicates occupancy at a particular voxel mean location.
11 | /// @param voxel The @c VoxelMean to update.
12 | /// @param sample_pos The sample position local to the centre of the voxel in falls in.
13 | /// @param voxel_resolution Voxel size.
14 | __device__ uint updateVoxelMeanPosition(__global VoxelMean *voxel, float3 sample_pos, float voxel_resolution);
15 |
16 | #ifndef VOXEL_MEAN_CL
17 | #define VOXEL_MEAN_CL
18 |
19 | //------------------------------------------------------------------------------
20 | // Functions
21 | //------------------------------------------------------------------------------
22 |
23 | // Psuedo header guard to prevent function implementation duplication.
24 | inline __device__ uint updateVoxelMeanPosition(__global VoxelMean *voxel, float3 sample_pos, float voxel_resolution)
25 | {
26 | uint point_count;
27 | uint old_value;
28 | uint new_value;
29 |
30 | // Few iterations as it's less important to get this right.
31 | const int iteration_limit = 10;
32 | int iteration_count = 0;
33 |
34 | // First update the mean position, then the point count. There is a level of contension here, and the results may be
35 | // somewhat out.
36 | do
37 | {
38 | // point_count = gputilAtomicLoadU32(&voxel->count);
39 | point_count = gputilAtomicLoadU32(&voxel->count);
40 | old_value = gputilAtomicLoadU32(&voxel->coord);
41 | new_value = subVoxelUpdate(old_value, point_count, sample_pos, voxel_resolution);
42 | ++iteration_count;
43 | } while (!gputilAtomicCasU32(&voxel->coord, old_value, new_value) && iteration_limit < iteration_count);
44 |
45 | // Atomic increment for the point count.
46 | return gputilAtomicInc(&voxel->count);
47 | }
48 |
49 | #endif // VOXEL_MEAN_CL
50 |
--------------------------------------------------------------------------------
/ohmgpu/private/ClearanceProcessDetail.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "ClearanceProcessDetail.h"
7 |
8 | #include
9 |
10 | #include
11 |
12 | namespace ohm
13 | {
14 | void ClearanceProcessDetail::getWork(OccupancyMap &map)
15 | {
16 | map.calculateDirtyClearanceExtents(&min_dirty_region, &max_dirty_region, 1);
17 | current_dirty_cursor = min_dirty_region;
18 | }
19 |
20 |
21 | void ClearanceProcessDetail::stepCursor(const glm::i16vec3 &step)
22 | {
23 | if (current_dirty_cursor.x < max_dirty_region.x)
24 | {
25 | current_dirty_cursor.x = std::min(current_dirty_cursor.x + step.x, max_dirty_region.x);
26 | }
27 | else
28 | {
29 | current_dirty_cursor.x = min_dirty_region.x;
30 | if (current_dirty_cursor.y < max_dirty_region.y)
31 | {
32 | current_dirty_cursor.y = std::min(current_dirty_cursor.y + step.y, max_dirty_region.y);
33 | }
34 | else
35 | {
36 | current_dirty_cursor.y = min_dirty_region.y;
37 | if (current_dirty_cursor.z < max_dirty_region.z)
38 | {
39 | current_dirty_cursor.z = std::min(current_dirty_cursor.z + step.z, max_dirty_region.z);
40 | }
41 | else
42 | {
43 | current_dirty_cursor = max_dirty_region + glm::i16vec3(1);
44 | }
45 | }
46 | }
47 | }
48 | } // namespace ohm
49 |
--------------------------------------------------------------------------------
/ohmgpu/private/ClearanceProcessDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMGPU_CLEARANCEPROCESSEDETAIL_H
7 | #define OHMGPU_CLEARANCEPROCESSEDETAIL_H
8 |
9 | #include "OhmGpuConfig.h"
10 |
11 | #include
12 |
13 | #include "RoiRangeFill.h"
14 |
15 | #include
16 |
17 | // #define CACHE_LOCAL_RESULTS
18 | // #define VALIDATE_KEYS
19 |
20 | namespace ohm
21 | {
22 | class OccupancyMap;
23 | struct MapChunk;
24 |
25 | struct ClearanceProcessDetail
26 | {
27 | unsigned query_flags = 0;
28 | glm::vec3 axis_scaling = glm::vec3(1);
29 | glm::i16vec3 min_dirty_region = glm::i16vec3(1);
30 | glm::i16vec3 max_dirty_region = glm::i16vec3(0);
31 | glm::i16vec3 current_dirty_cursor = glm::i16vec3(0);
32 | /// Last value of the @c OccupancyMap::stamp(). Used to see if the cache needs to be cleared.
33 | uint64_t map_stamp = 0;
34 | float search_radius = 0;
35 |
36 | std::unique_ptr gpu_query;
37 |
38 | inline bool haveWork() const
39 | {
40 | return glm::all(glm::lessThanEqual(min_dirty_region, max_dirty_region)) &&
41 | glm::all(glm::lessThanEqual(current_dirty_cursor, max_dirty_region));
42 | }
43 |
44 | inline void resetWorking()
45 | {
46 | min_dirty_region = glm::i16vec3(1);
47 | max_dirty_region = current_dirty_cursor = glm::i16vec3(0);
48 | map_stamp = 0;
49 | }
50 |
51 | void stepCursor(const glm::i16vec3 &step = glm::i16vec3(1));
52 |
53 | void getWork(OccupancyMap &map);
54 | };
55 | } // namespace ohm
56 |
57 | #endif // OHMGPU_CLEARANCEPROCESSEDETAIL_H
58 |
--------------------------------------------------------------------------------
/ohmgpu/private/GpuNdtMapDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUNDTMAPDETAIL_H
7 | #define GPUNDTMAPDETAIL_H
8 |
9 | #include "OhmGpuConfig.h"
10 |
11 | #include "private/GpuMapDetail.h"
12 |
13 | #include
14 | #include
15 |
16 | namespace ohm
17 | {
18 | struct GpuNdtMapDetail : public GpuMapDetail
19 | {
20 | GpuProgramRef *cov_hit_program_ref = nullptr;
21 | gputil::Kernel cov_hit_kernel;
22 | NdtMap ndt_map;
23 |
24 | /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the covariance layer.
25 | int cov_uidx = -1;
26 | /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the intensity layer.
27 | int intensity_uidx = -1;
28 | /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the hit/hiss layer.
29 | int hit_miss_uidx = -1;
30 |
31 | GpuNdtMapDetail(OccupancyMap *map, bool borrowed_map, NdtMode mode)
32 | : GpuMapDetail(map, borrowed_map)
33 | , ndt_map(map, true, mode) // Ensures correct initialisation for NDT operations.
34 | {}
35 | };
36 | } // namespace ohm
37 |
38 | #endif // GPUNDTMAPDETAIL_H
39 |
--------------------------------------------------------------------------------
/ohmgpu/private/GpuTransformSamplesDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "OhmGpuConfig.h"
7 |
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | struct GpuTransformSamplesDetail
18 | {
19 | static const unsigned kUploadEventCount = 4;
20 |
21 | gputil::Buffer transform_positions_buffer;
22 | gputil::Buffer transform_rotations_buffer;
23 | gputil::Buffer transform_times_buffer;
24 | std::array upload_events;
25 | gputil::Device gpu;
26 | gputil::Kernel kernel;
27 | };
28 | } // namespace ohm
29 |
--------------------------------------------------------------------------------
/ohmgpu/private/GpuTsdfMapDetail.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef GPUTSDFMAPDETAIL_H
7 | #define GPUTSDFMAPDETAIL_H
8 |
9 | #include "OhmGpuConfig.h"
10 |
11 | #include "private/GpuMapDetail.h"
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | struct GpuTsdfMapDetail : public GpuMapDetail
18 | {
19 | /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the tsdf layer.
20 | int tsdf_uidx = -1;
21 | /// Tsdf mapping options.
22 | TsdfOptions tsdf_options;
23 |
24 | GpuTsdfMapDetail(OccupancyMap *map, bool borrowed_map)
25 | : GpuMapDetail(map, borrowed_map)
26 | {
27 | // Require original samples for TSDF for the distance calculations.
28 | use_original_ray_buffers = true;
29 | }
30 | };
31 | } // namespace ohm
32 |
33 | #endif // GPUTSDFMAPDETAIL_H
34 |
--------------------------------------------------------------------------------
/ohmgpu/private/LineKeysQueryDetailGpu.h:
--------------------------------------------------------------------------------
1 |
2 | // Copyright (c) 2017
3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
4 | // ABN 41 687 119 230
5 | //
6 | // Author: Kazys Stepanas
7 | #ifndef OHMGPU_LINEKEYSQUERYDETAILGPU_H_
8 | #define OHMGPU_LINEKEYSQUERYDETAILGPU_H_OHMGPU_LINEKEYSQUERYDETAILGPU_HOHMGPU_LINEKEYSQUERYDETAILGPU_H_
9 |
10 | #include "OhmGpuConfig.h"
11 |
12 | #include
13 |
14 | // Include GPU structure definition.
15 | #include "GpuKey.h"
16 |
17 | #include
18 | #include
19 | #include
20 | #include
21 |
22 | #include
23 |
24 | namespace ohm
25 | {
26 | struct LineKeysQueryDetailGpu : public LineKeysQueryDetail
27 | {
28 | gputil::Kernel line_keys_kernel;
29 | gputil::Device gpu;
30 |
31 | gputil::Queue queue;
32 |
33 | gputil::Buffer lines_out;
34 | gputil::Buffer line_points;
35 | unsigned max_keys_per_line = 0;
36 | std::atomic_bool inflight{ false };
37 |
38 | bool gpu_ok = false;
39 | };
40 | } // namespace ohm
41 |
42 | #endif // OHMGPU_LINEKEYSQUERYDETAILGPU_H_
43 |
--------------------------------------------------------------------------------
/ohmgpu/private/LineQueryDetailGpu.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2017
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMGPU_LINEQUERYDETAILGPU_H
7 | #define OHMGPU_LINEQUERYDETAILGPU_H
8 |
9 | #include "OhmGpuConfig.h"
10 |
11 | #include
12 |
13 | #include
14 |
15 | namespace ohm
16 | {
17 | class ClearanceProcess;
18 |
19 | struct LineQueryDetailGpu : LineQueryDetail
20 | {
21 | ClearanceProcess *clearance_calculator = nullptr;
22 | };
23 | } // namespace ohm
24 |
25 | #endif // OHMGPU_LINEQUERYDETAILGPU_H
26 |
--------------------------------------------------------------------------------
/ohmheightmap/HeightmapMode.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "HeightmapMode.h"
7 |
8 | #include
9 |
10 | namespace ohm
11 | {
12 | namespace
13 | {
14 | static const std::array mode_names = //
15 | {
16 | //
17 | "planar", //
18 | "fill", //
19 | "layered-unordered", //
20 | "layered", //
21 | };
22 | }
23 |
24 | std::string heightmapModeToString(HeightmapMode mode)
25 | {
26 | if (size_t(mode) < mode_names.size())
27 | {
28 | return mode_names[int(mode)];
29 | }
30 |
31 | return "";
32 | }
33 | HeightmapMode heightmapModeFromString(const std::string &str, bool *valid_string)
34 | {
35 | int mode_index = 0;
36 | for (const auto &name : mode_names)
37 | {
38 | if (str == name)
39 | {
40 | if (valid_string)
41 | {
42 | *valid_string = true;
43 | }
44 | return HeightmapMode(mode_index);
45 | }
46 | ++mode_index;
47 | }
48 |
49 | if (valid_string)
50 | {
51 | *valid_string = false;
52 | }
53 | return HeightmapMode::kPlanar;
54 | }
55 | } // namespace ohm
56 |
--------------------------------------------------------------------------------
/ohmheightmap/HeightmapMode.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 |
7 | #ifndef OHMHEIGHTMAP_HEIGHTMAPMODE_H
8 | #define OHMHEIGHTMAP_HEIGHTMAPMODE_H
9 |
10 | #include "OhmHeightmapConfig.h"
11 |
12 | #include
13 |
14 | namespace ohm
15 | {
16 | /// Enumeration of the available heightmap generation modes.
17 | enum class HeightmapMode
18 | {
19 | /// Simple planar traversal from the reference position. Heightmap grid cells are only visited once each.
20 | kPlanar,
21 | /// Use a simple flood fill out from the reference position. The resulting heightmap may have better continuity than
22 | /// @c kPlanar. Voxels may be revisited when a lower candidate voxel is found.
23 | kSimpleFill,
24 | /// Use a flood fill which supports layering. The fill attempts to expand on all available surfaces and can generate
25 | /// a multi layer heightmap. The resulting heightmap is 2.5D and each column can contain multiple entries. The height
26 | /// values of entries in each column are unsorted with undefined height ordering.
27 | kLayeredFillUnordered,
28 | /// Same as @c kLayeredFillUnordered except that the resulting heightmap ensures each column of voxels in the
29 | /// generated heightmap are in ascending height order.
30 | kLayeredFill,
31 |
32 | /// First mode value
33 | kFirst = kPlanar,
34 | /// Last mode value.
35 | kLast = kLayeredFill
36 | };
37 |
38 | /// Convert a @c HeightmapMode to a string.
39 | /// @param mode The mode value to convert.
40 | /// @return The string name for @p mode or `""` if @p mode is out of range.
41 | std::string ohmheightmap_API heightmapModeToString(HeightmapMode mode);
42 | /// Convert a string to a @c HeightampMode - reverse of @c heightmapModeToString().
43 | /// @param str The string to convert.
44 | /// @param valid_string Optional - set to true if @p str is a valid mode name, false otherwise.
45 | /// @return The mode matching @p str or @c HeightmapMode::kPlanar when @p str is not a valid mode name.
46 | HeightmapMode ohmheightmap_API heightmapModeFromString(const std::string &str, bool *valid_string = nullptr);
47 | } // namespace ohm
48 |
49 | #endif // OHMHEIGHTMAP_HEIGHTMAPMODE_H
50 |
--------------------------------------------------------------------------------
/ohmheightmap/HeightmapSerialise.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "HeightmapSerialise.h"
7 |
8 | #include "Heightmap.h"
9 |
10 | #include "private/HeightmapDetail.h"
11 |
12 | #include
13 | #include
14 |
15 | namespace
16 | {
17 | struct RegisterExtensionCodes
18 | {
19 | RegisterExtensionCodes()
20 | {
21 | ohm::registerSerialiseExtensionErrorCodeString(int(ohm::kSeHeightmapInfoMismatch), "heightmap info mismatch");
22 | }
23 | };
24 |
25 | const RegisterExtensionCodes s_register_heightmap_errors;
26 | } // namespace
27 |
28 | namespace ohm
29 | {
30 | int load(const std::string &filename, Heightmap &heightmap, SerialiseProgress *progress, MapVersion *version_out)
31 | {
32 | HeightmapDetail &detail = *heightmap.detail();
33 |
34 | int err = load(filename, *detail.heightmap, progress, version_out);
35 | if (err)
36 | {
37 | return err;
38 | }
39 |
40 | // TODO(KS): Set axis from map info.
41 | const MapInfo &info = detail.heightmap->mapInfo();
42 | if (!bool(info.get("heightmap")))
43 | {
44 | return kSeHeightmapInfoMismatch;
45 | }
46 |
47 | detail.fromMapInfo(info);
48 |
49 | return err;
50 | }
51 | } // namespace ohm
52 |
--------------------------------------------------------------------------------
/ohmheightmap/HeightmapSerialise.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMHEIGHTMAP_HEIGHTMAPMAPSERIALISE_H
7 | #define OHMHEIGHTMAP_HEIGHTMAPMAPSERIALISE_H
8 |
9 | #include "OhmHeightmapConfig.h"
10 |
11 | #include
12 |
13 | #include
14 |
15 | #include
16 | #include
17 |
18 | namespace ohm
19 | {
20 | class Heightmap;
21 |
22 | /// An enumeration of potential serialisation errors.
23 | enum HeightmapSerialisationError
24 | {
25 | /// @c MapInfo does not represent a heightmap.
26 | kSeHeightmapInfoMismatch = kSeExtensionCode + 1,
27 | };
28 |
29 | /// Load a save heightmap into a @c Heightmap object. Saving can be done directly on the @c Occupancy map stored in
30 | /// @c Heightmap::heightmap() .
31 | /// @param filename The heightmap file path to load.
32 | /// @param heightmap The heightmap object to load into.
33 | /// @param progress Optional progress reporting interface.
34 | /// @param[out] version_out Set to the map file version number if provided.
35 | int ohmheightmap_API load(const std::string &filename, Heightmap &heightmap, SerialiseProgress *progress = nullptr,
36 | MapVersion *version_out = nullptr);
37 | } // namespace ohm
38 |
39 | #endif // OHMHEIGHTMAP_HEIGHTMAPMAPSERIALISE_H
40 |
--------------------------------------------------------------------------------
/ohmheightmap/HeightmapVoxel.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "HeightmapVoxel.h"
7 |
8 | namespace ohm
9 | {
10 | const char *const HeightmapVoxel::kHeightmapLayer = "heightmap";
11 | const char *const HeightmapVoxel::kHeightmapBuildLayer = "heightmap_build";
12 | } // namespace ohm
13 |
--------------------------------------------------------------------------------
/ohmheightmap/HeightmapVoxelType.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMHEIGHTMAP_HEIGHTMAP_VOXEL_TYPE_H_
7 | #define OHMHEIGHTMAP_HEIGHTMAP_VOXEL_TYPE_H_
8 |
9 | #include "OhmHeightmapConfig.h"
10 |
11 | namespace ohm
12 | {
13 | /// Type enumeration for voxels in a heightmap.
14 | enum class HeightmapVoxelType : uint8_t
15 | {
16 | /// Unkown or unobserved voxel.
17 | kUnknown = 0,
18 | /// A voxel which is being kept forcibly vacant. Normally this represents voxels which have yet to be able to
19 | /// observed at the startup location.
20 | kVacant,
21 | /// Represents a real surface voxel in the heightmap.
22 | kSurface,
23 | /// Represents a virtual surface voxel in the heightmap. Virtual surfaces are created at the interface between
24 | /// free and unknown voxels (free supported by unknown).
25 | kVirtualSurface,
26 | /// A voxel which has been inferred as fatal. This occurs where virtual surfaces appear near the reference point.
27 | kInferredFatal,
28 | /// An otherwise forcibly fatal cost voxel.
29 | kFatal
30 | };
31 | } // namespace ohm
32 |
33 | #endif // OHMHEIGHTMAP_HEIGHTMAP_VOXEL_TYPE_H_
34 |
--------------------------------------------------------------------------------
/ohmheightmap/OhmHeightmapConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMHEIGHTMAPCONFIG_H
6 | #define OHMHEIGHTMAPCONFIG_H
7 |
8 | #include "OhmHeightmapExport.h"
9 |
10 | #ifndef _USE_MATH_DEFINES
11 | #define _USE_MATH_DEFINES
12 | #endif // _USE_MATH_DEFINES
13 | #ifndef NOMINMAX
14 | #define NOMINMAX
15 | #endif // NOMINMAX
16 |
17 | #ifdef _MSC_VER
18 | // Avoid dubious security warnings for plenty of legitimate code
19 | #ifndef _SCL_SECURE_NO_WARNINGS
20 | #define _SCL_SECURE_NO_WARNINGS
21 | #endif // _SCL_SECURE_NO_WARNINGS
22 | #ifndef _CRT_SECURE_NO_WARNINGS
23 | #define _CRT_SECURE_NO_WARNINGS
24 | #endif // _CRT_SECURE_NO_WARNINGS
25 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
26 | #endif // _MSC_VER
27 |
28 | #include
29 |
30 | #include
31 |
32 | #endif // OHMHEIGHTMAPCONFIG_H
33 |
--------------------------------------------------------------------------------
/ohmheightmap/PlaneWalkVisitMode.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2020
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMHEIGHTMAP_PLANEWALKVISITMODE_H
7 | #define OHMHEIGHTMAP_PLANEWALKVISITMODE_H
8 |
9 | #include "OhmHeightmapConfig.h"
10 |
11 | namespace ohm
12 | {
13 | /// Mode selection for handling neighbours in various @c PlaneWalker implementations used by @c Heightmap .
14 | /// Determines now neighbours are handled when visiting a node.
15 | enum class PlaneWalkVisitMode
16 | {
17 | /// Do nothing with neighbours. Not added.
18 | kIgnoreNeighbours,
19 | /// Add (planar) neighbours for processing.
20 | kAddUnvisitedNeighbours,
21 | /// Add (planar) neighbours for processing as long as they have not yet been visited in a 2D sense.
22 | ///
23 | /// This option should be used with @c PlaneFillLayeredWalk when a column cannot find a valid ground candidate. The
24 | /// layered fill can then traverse unobserved space.
25 | kAddUnvisitedColumnNeighbours
26 | };
27 | } // namespace ohm
28 |
29 | #endif // OHMHEIGHTMAP_PLANEWALKVISITMODE_H
30 |
--------------------------------------------------------------------------------
/ohmheightmap/PlaneWalker.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "PlaneWalker.h"
7 |
8 | #include "HeightmapUtil.h"
9 |
10 | #include
11 |
12 | namespace ohm
13 | {
14 | PlaneWalker::PlaneWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key, UpAxis up_axis,
15 | const Key *plane_key_ptr)
16 | : map(map)
17 | , min_ext_key(min_ext_key)
18 | , max_ext_key(max_ext_key)
19 | , plane_key(plane_key_ptr ? *plane_key_ptr : min_ext_key)
20 | , axis_indices(ohm::heightmap::heightmapAxisIndices(up_axis))
21 | {}
22 |
23 |
24 | bool PlaneWalker::begin(Key &key) const
25 | {
26 | key = min_ext_key;
27 | // Flatten the key onto the plane.
28 | key.setRegionAxis(axis_indices[2], plane_key.regionKey()[axis_indices[2]]);
29 | key.setLocalAxis(axis_indices[2], plane_key.localKey()[axis_indices[2]]);
30 | key.clampToAxis(axis_indices[2], min_ext_key, max_ext_key);
31 | return true;
32 | }
33 |
34 |
35 | bool PlaneWalker::walkNext(Key &key) const
36 | {
37 | map.stepKey(key, axis_indices[0], 1);
38 | if (!key.isBounded(axis_indices[0], min_ext_key, max_ext_key))
39 | {
40 | // Finished walking this axis. Reset and walk outer axis.
41 | key.setLocalAxis(axis_indices[0], min_ext_key.localKey()[axis_indices[0]]);
42 | key.setRegionAxis(axis_indices[0], min_ext_key.regionKey()[axis_indices[0]]);
43 |
44 | map.stepKey(key, axis_indices[1], 1);
45 | if (!key.isBounded(axis_indices[1], min_ext_key, max_ext_key))
46 | {
47 | return false;
48 | }
49 | }
50 |
51 | return true;
52 | }
53 | } // namespace ohm
54 |
--------------------------------------------------------------------------------
/ohmheightmap/TriangleEdge.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMHEIGHTMAP_TRIANGLEEDGE_H
7 | #define OHMHEIGHTMAP_TRIANGLEEDGE_H
8 |
9 | #include "OhmHeightmapConfig.h"
10 |
11 | #include
12 |
13 | namespace ohm
14 | {
15 | /// A helper for tracking edges in mesh generation. Stores edge vertices such that v0 < v1 regardless of the input
16 | /// ordering. This allows the edge array to be sorted to clump the edge pairings where two triangles share an edge.
17 | /// Only two triangles can share one edge because we build the mesh without T intersections.
18 | struct ohmheightmap_API TriangleEdge
19 | {
20 | unsigned v0; ///< The lower magnitude vertex index.
21 | unsigned v1; ///< The higher magnitude vertex index.
22 | unsigned triangle; ///< The index of the triangle which generated this edge.
23 | unsigned triangle_edge_index; ///< The index of this edge in @p triangle.
24 |
25 | /// Constructor; v0 and v1 will be reordered such that v0 < v1.
26 | /// @param v0 First vertex index.
27 | /// @param v1 Second vertex index.
28 | /// @param triangle Triangle index.
29 | /// @param edge_index The index of the edge in @p triangle [0, 2].
30 | inline TriangleEdge(unsigned v0, unsigned v1, unsigned triangle, unsigned edge_index)
31 | : triangle(triangle)
32 | , triangle_edge_index(edge_index)
33 | {
34 | this->v0 = std::min(v0, v1);
35 | this->v1 = std::max(v0, v1);
36 | }
37 |
38 | /// Comparison operator for sorting.
39 | inline bool operator<(const TriangleEdge &other) const { return v0 < other.v0 || v0 == other.v0 && v1 < other.v1; }
40 | };
41 | } // namespace ohm
42 |
43 | #endif // OHMHEIGHTMAP_TRIANGLEEDGE_H
44 |
--------------------------------------------------------------------------------
/ohmheightmap/TriangleNeighbours.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMHEIGHTMAP_TRIANGLENEIGHBOURS_H
7 | #define OHMHEIGHTMAP_TRIANGLENEIGHBOURS_H
8 |
9 | #include "OhmHeightmapConfig.h"
10 |
11 | #include
12 | #include
13 |
14 | namespace ohm
15 | {
16 | /// Stores the neighbours information for a triangle.
17 | ///
18 | /// Identifies the @p neighbour triangle indices (multiply by 3 when indexing HeightmapMesh::triangles()) and the
19 | /// edge ID of the shared edge in the neighbour. The index into @c neighbours [0, 2] identifies the edge ID in the
20 | /// current triangle.
21 | struct ohmheightmap_API TriangleNeighbours
22 | {
23 | /// Indices to the neighbouring triangles.
24 | /// ~0u for each open edge (-1).
25 | ///
26 | /// Ordered by the shared edge as {v[0], v[1]}, {v[1], v[2]}, {v[2], v[0]}.
27 | std::array neighbours;
28 | /// Identifies the shared edge indices in each neighbour triangle. For example, @c neighbour_edge_indices[0]
29 | /// contains edge information for neighbour[0]. The value of [0, 2] indicates which vertex pairing in neighbour[0]
30 | /// identifies the shared edge. -1 is used for empty edges.
31 | std::array neighbour_edge_indices;
32 | };
33 | } // namespace ohm
34 |
35 | #endif // OHMHEIGHTMAP_TRIANGLENEIGHBOURS_H
36 |
--------------------------------------------------------------------------------
/ohmheightmap/UpAxis.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef UPAXIS_H
7 | #define UPAXIS_H
8 |
9 | #include "OhmConfig.h"
10 |
11 | namespace ohm
12 | {
13 | /// Up axis identification values. Used for @c Heightmap generation.
14 | ///
15 | /// Documentation for each ID identifies the up axis.
16 | enum class UpAxis : int
17 | {
18 | /// (0, 0, -1)
19 | kNegZ = -3,
20 | /// (0, -1, 0)
21 | kNegY = -2,
22 | /// (-1, 0, 0)
23 | kNegX = -1,
24 | /// (1, 0, 0)
25 | kX,
26 | /// (0, 1, 0)
27 | kY,
28 | /// (0, 0, 1)
29 | kZ,
30 | };
31 | } // namespace ohm
32 |
33 | #endif // UPAXIS_H
34 |
--------------------------------------------------------------------------------
/ohmheightmapimage/OhmHeightmapImageConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMHEIGHTMAPIMAGECONFIG_H
6 | #define OHMHEIGHTMAPIMAGECONFIG_H
7 |
8 | #include "OhmHeightmapUtilExport.h"
9 |
10 | #ifndef _USE_MATH_DEFINES
11 | #define _USE_MATH_DEFINES
12 | #endif // _USE_MATH_DEFINES
13 | #ifndef NOMINMAX
14 | #define NOMINMAX
15 | #endif // NOMINMAX
16 | #include
17 |
18 | #ifdef _MSC_VER
19 | // Avoid dubious security warnings for plenty of legitimate code
20 | #ifndef _SCL_SECURE_NO_WARNINGS
21 | #define _SCL_SECURE_NO_WARNINGS
22 | #endif // _SCL_SECURE_NO_WARNINGS
23 | #ifndef _CRT_SECURE_NO_WARNINGS
24 | #define _CRT_SECURE_NO_WARNINGS
25 | #endif // _CRT_SECURE_NO_WARNINGS
26 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
27 | #endif // _MSC_VER
28 |
29 | // Enable various validation tests throughout this library.
30 | //#cmakedefine OHM_FEATURE_THREADS
31 |
32 | #include "OhmConfig.h"
33 |
34 | #endif // OHMHEIGHTMAPIMAGECONFIG_H
35 |
--------------------------------------------------------------------------------
/ohmtools/CMakeLists.txt:
--------------------------------------------------------------------------------
1 |
2 | include(GenerateExportHeader)
3 |
4 |
5 |
6 | configure_file(OhmToolsConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmtools/OhmToolsConfig.h")
7 |
8 | set(SOURCES
9 | OhmCloud.cpp
10 | OhmCloud.h
11 | OhmGen.cpp
12 | OhmGen.h
13 | OhmToolsConfig.in.h
14 | )
15 |
16 | set(PUBLIC_HEADERS
17 | OhmCloud.h
18 | OhmGen.h
19 | "${CMAKE_CURRENT_BINARY_DIR}/ohmtools/OhmToolsConfig.h"
20 | "${CMAKE_CURRENT_BINARY_DIR}/ohmtools/OhmToolsExport.h"
21 | )
22 |
23 | add_library(ohmtools ${SOURCES})
24 | clang_tidy_target(ohmtools)
25 |
26 | generate_export_header(ohmtools
27 | EXPORT_MACRO_NAME ohmtools_API
28 | EXPORT_FILE_NAME ohmtools/OhmToolsExport.h
29 | STATIC_DEFINE OHMTOOLS_STATIC)
30 |
31 | target_link_libraries(ohmtools
32 | PUBLIC
33 | ohm
34 | ohmheightmap
35 | ohmutil
36 | glm::glm
37 | )
38 |
39 | target_include_directories(ohmtools
40 | PUBLIC
41 | $
42 | $
43 | $
44 | $
45 | )
46 |
47 | install(TARGETS ohmtools EXPORT ${CMAKE_PROJECT_NAME}-config-targets
48 | LIBRARY DESTINATION lib
49 | ARCHIVE DESTINATION lib
50 | RUNTIME DESTINATION bin
51 | INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmtools
52 | )
53 |
54 | install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmtools)
55 |
56 | source_group("source" REGULAR_EXPRESSION ".*$")
57 | # Needs CMake 3.8+:
58 | # source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})
59 |
--------------------------------------------------------------------------------
/ohmtools/OhmToolsConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMTOOLSCONFIG_H
6 | #define OHMTOOLSCONFIG_H
7 |
8 | #include "OhmToolsExport.h"
9 |
10 | #ifndef _USE_MATH_DEFINES
11 | #define _USE_MATH_DEFINES
12 | #endif // _USE_MATH_DEFINES
13 | #ifndef NOMINMAX
14 | #define NOMINMAX
15 | #endif // NOMINMAX
16 | #include
17 |
18 | #ifdef _MSC_VER
19 | // Avoid dubious security warnings for plenty of legitimate code
20 | #ifndef _SCL_SECURE_NO_WARNINGS
21 | #define _SCL_SECURE_NO_WARNINGS
22 | #endif // _SCL_SECURE_NO_WARNINGS
23 | #ifndef _CRT_SECURE_NO_WARNINGS
24 | #define _CRT_SECURE_NO_WARNINGS
25 | #endif // _CRT_SECURE_NO_WARNINGS
26 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
27 | #endif // _MSC_VER
28 |
29 | // Enable various validation tests throughout this library.
30 | //#cmakedefine OHM_FEATURE_THREADS
31 |
32 | #include "OhmConfig.h"
33 |
34 | #endif // OHMTOOLSCONFIG_H
35 |
--------------------------------------------------------------------------------
/ohmutil/OhmUtilConfig.in.h:
--------------------------------------------------------------------------------
1 | //
2 | // Project configuration header. This is a generated header; do not modify
3 | // it directly. Instead, modify the config.h.in version and run CMake again.
4 | //
5 | #ifndef OHMUTILCONFIG_H
6 | #define OHMUTILCONFIG_H
7 |
8 | #include "OhmUtilExport.h"
9 |
10 | #ifndef _USE_MATH_DEFINES
11 | #define _USE_MATH_DEFINES
12 | #endif // _USE_MATH_DEFINES
13 | #ifndef NOMINMAX
14 | #define NOMINMAX
15 | #endif // NOMINMAX
16 |
17 | #ifdef _MSC_VER
18 | // Avoid dubious security warnings for plenty of legitimate code
19 | #ifndef _SCL_SECURE_NO_WARNINGS
20 | #define _SCL_SECURE_NO_WARNINGS
21 | #endif // _SCL_SECURE_NO_WARNINGS
22 | #ifndef _CRT_SECURE_NO_WARNINGS
23 | #define _CRT_SECURE_NO_WARNINGS
24 | #endif // _CRT_SECURE_NO_WARNINGS
25 | //#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
26 | #endif // _MSC_VER
27 |
28 | #include
29 |
30 | /// Enable experimental parts of GLM, like `glm::length2()` (length squared)
31 | #define GLM_ENABLE_EXPERIMENTAL
32 |
33 | #endif // OHMUTILCONFIG_H
34 |
--------------------------------------------------------------------------------
/ohmutil/Options.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMUTIL_OPTIONS_H
7 | #define OHMUTIL_OPTIONS_H
8 |
9 | #ifdef _MSC_VER
10 | #pragma warning(push)
11 | #pragma warning(disable : 4267)
12 | #endif // _MSC_VER
13 |
14 | #include "GlmStream.h"
15 |
16 | #include
17 |
18 | template
19 | inline std::string optStr(const T &value)
20 | {
21 | std::ostringstream str;
22 | str << value;
23 | return str.str();
24 | }
25 |
26 | template
27 | inline std::shared_ptr optVal(T &val)
28 | {
29 | return cxxopts::value(val)->default_value(optStr(val));
30 | }
31 |
32 | inline std::shared_ptr optVal(bool &val)
33 | {
34 | return cxxopts::value(val)->implicit_value("true")->default_value(val ? "true" : "false");
35 | }
36 |
37 | #pragma warning(pop)
38 |
39 | #endif // OHMUTIL_OPTIONS_H
40 |
--------------------------------------------------------------------------------
/ohmutil/ProfileMarker.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "ProfileMarker.h"
7 |
8 | #include "Profile.h"
9 |
10 | namespace ohm
11 | {
12 | ProfileMarker::ProfileMarker(const char *name, bool activate)
13 | : ProfileMarker(name, nullptr, activate)
14 | {}
15 |
16 |
17 | ProfileMarker::ProfileMarker(const char *name, Profile *profile, bool activate)
18 | : name_(name)
19 | , profile_(profile)
20 | , active_(false)
21 | {
22 | restart(activate);
23 | }
24 |
25 |
26 | ProfileMarker::~ProfileMarker()
27 | {
28 | end();
29 | }
30 |
31 |
32 | void ProfileMarker::end()
33 | {
34 | if (active_)
35 | {
36 | // Pop first so this marker isn't a parent of itself.
37 | Profile *profile = (profile_ == nullptr) ? &Profile::instance() : profile_;
38 | profile->pop();
39 | active_ = false;
40 | }
41 | }
42 |
43 |
44 | void ProfileMarker::restart(bool activate)
45 | {
46 | if (!active_ && activate)
47 | {
48 | Profile *profile = (profile_ == nullptr) ? &Profile::instance() : profile_;
49 | active_ = profile->push(name_);
50 | }
51 | }
52 | } // namespace ohm
53 |
--------------------------------------------------------------------------------
/ohmutil/SafeIO.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "SafeIO.h"
7 |
8 | #include
9 |
10 | #if !defined(_MSC_VER)
11 |
12 | int fopen_s(FILE **file, const char *name, const char *mode) // NOLINT
13 | {
14 | *file = fopen(name, mode);
15 | if (*file)
16 | {
17 | return 0;
18 | }
19 | return 1;
20 | }
21 |
22 | #else // !defined(_MSC_VER)
23 |
24 | namespace
25 | {
26 | int avoid_no_symbols_link_warning = 0;
27 | }
28 |
29 | #endif // !defined(_MSC_VER)
30 |
--------------------------------------------------------------------------------
/ohmutil/SafeIO.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2018
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef OHMUTIL_SAFEIO_H
7 | #define OHMUTIL_SAFEIO_H
8 |
9 | #include "OhmUtilExport.h"
10 |
11 | #include
12 |
13 | #if !defined(_MSC_VER)
14 |
15 | #define sprintf_s snprintf
16 | #define sscanf_s sscanf
17 |
18 | int ohmutil_API fopen_s(FILE **file, const char *name, const char *mode); // NOLINT
19 |
20 | #endif // !defined(_MSC_VER)
21 |
22 | #endif // OHMUTIL_SAFEIO_H
23 |
--------------------------------------------------------------------------------
/ohmutil/p2p.h:
--------------------------------------------------------------------------------
1 | //
2 | // author Kazys Stepanas
3 | //
4 | #ifndef OHMUTIL_P2P_H
5 | #define OHMUTIL_P2P_H
6 |
7 | #include
8 |
9 | #ifdef TES_ENABLE
10 | #include "3esvector3.h"
11 |
12 | #include
13 |
14 | inline tes::Vector3f p2p(const glm::vec3 &p)
15 | {
16 | return tes::Vector3f(p.x, p.y, p.z);
17 | }
18 |
19 | inline glm::vec3 p2p(const tes::Vector3f &p)
20 | {
21 | return glm::vec3(p.x, p.y, p.z);
22 | }
23 |
24 | inline const tes::Vector3f *p2pArray(const glm::vec3 *points)
25 | {
26 | static_assert(sizeof(tes::Vector3f) == sizeof(glm::vec3), "tes::Vector3f size does not match glm::vec3 size.");
27 | // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
28 | return reinterpret_cast(points);
29 | }
30 |
31 | inline const glm::vec3 *p2pArray(const tes::Vector3f *points)
32 | {
33 | static_assert(sizeof(tes::Vector3f) == sizeof(glm::vec3), "tes::Vector3f size does not match glm::vec3 size.");
34 | // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
35 | return reinterpret_cast(points);
36 | }
37 |
38 | #endif // TES_ENABLE
39 |
40 | #endif // OHMUTIL_P2P_H
41 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | ohm
4 | 0.5.0
5 | Occupancy Homogeneous Map libraries
6 | Kazys Stepanas
7 | Modified MIT
8 |
9 | libglew-dev
10 | libglfw3-dev
11 | libglm-dev
12 | ocl-icd-opencl-dev
13 |
14 |
15 | cmake
16 |
17 |
18 |
19 |
--------------------------------------------------------------------------------
/scripts/setup.cfg:
--------------------------------------------------------------------------------
1 | [pep8]
2 | max-line-length = 120
3 |
4 | [flake8]
5 | max-line-length = 120
6 |
--------------------------------------------------------------------------------
/slamio/DataChannel.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef SLAMIO_DATACHANNEL_H_
7 | #define SLAMIO_DATACHANNEL_H_
8 |
9 | #include "SlamIOConfig.h"
10 |
11 | namespace slamio
12 | {
13 | enum class DataChannel : unsigned;
14 | }
15 |
16 | slamio::DataChannel operator~(slamio::DataChannel a);
17 | slamio::DataChannel operator|(slamio::DataChannel a, slamio::DataChannel b);
18 | slamio::DataChannel operator|=(slamio::DataChannel &a, slamio::DataChannel b);
19 | slamio::DataChannel operator&(slamio::DataChannel a, slamio::DataChannel b);
20 | slamio::DataChannel operator&=(slamio::DataChannel &a, slamio::DataChannel b);
21 |
22 | namespace slamio
23 | {
24 | enum class DataChannel : unsigned
25 | {
26 | None = 0u,
27 | Time = (1u << 0u),
28 | Position = (1u << 1u),
29 | Normal = (1u << 2u),
30 | ColourRgb = (1u << 3u),
31 | ColourAlpha = (1u << 4u),
32 | Intensity = (1u << 5u),
33 | ReturnNumber = (1u << 6u),
34 |
35 | Colour = ColourRgb | ColourAlpha
36 | };
37 | }
38 |
39 | inline slamio::DataChannel operator~(slamio::DataChannel a)
40 | {
41 | return slamio::DataChannel(~unsigned(a));
42 | }
43 |
44 | inline slamio::DataChannel operator|(slamio::DataChannel a, slamio::DataChannel b)
45 | {
46 | return slamio::DataChannel(unsigned(a) | unsigned(b));
47 | }
48 |
49 | inline slamio::DataChannel operator|=(slamio::DataChannel &a, slamio::DataChannel b)
50 | {
51 | a = a | b;
52 | return a;
53 | }
54 |
55 | inline slamio::DataChannel operator&(slamio::DataChannel a, slamio::DataChannel b)
56 | {
57 | return slamio::DataChannel(unsigned(a) & unsigned(b));
58 | }
59 |
60 | inline slamio::DataChannel operator&=(slamio::DataChannel &a, slamio::DataChannel b)
61 | {
62 | a = a & b;
63 | return a;
64 | }
65 |
66 | #endif // SLAMIO_DATACHANNEL_H_
67 |
--------------------------------------------------------------------------------
/slamio/PointCloudReader.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #include "PointCloudReader.h"
7 |
8 | #include
9 |
10 | namespace slamio
11 | {
12 | const char *const *timeFieldNames(size_t &count)
13 | {
14 | static const std::array names = //
15 | {
16 | //
17 | "gps_time", //
18 | "gpstime", //
19 | "internal_time", //
20 | "internaltime", //
21 | "offset_time", //
22 | "offsettime", //
23 | "timestamp", //
24 | "time" //
25 | };
26 | count = names.size();
27 | return names.data();
28 | }
29 |
30 | const char *const *returnNumberFieldNames(size_t &count)
31 | {
32 | {
33 | static const std::array names = //
34 | {
35 | //
36 | "returnnumber", //
37 | "return_number", //
38 | "returnnum", //
39 | "return_num", //
40 | };
41 | count = names.size();
42 | return names.data();
43 | }
44 | }
45 |
46 | PointCloudReader::~PointCloudReader() = default;
47 | } // namespace slamio
48 |
--------------------------------------------------------------------------------
/slamio/PointCloudReaderTraj.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2021
2 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO)
3 | // ABN 41 687 119 230
4 | //
5 | // Author: Kazys Stepanas
6 | #ifndef SLAMIO_POINTCLOUDREADERTRAJ_H_
7 | #define SLAMIO_POINTCLOUDREADERTRAJ_H_
8 |
9 | #include "SlamIOConfig.h"
10 |
11 | #include "PointCloudReader.h"
12 |
13 | #include
14 | #include
15 | #include