├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── nanomapConfig.cmake ├── config ├── agents │ └── Agent.txt ├── config.txt ├── mapgen │ └── MapGen.txt ├── maps │ └── Map.txt └── sensors │ ├── 640x480.txt │ └── VLP16.txt ├── include └── nanomap │ ├── agent │ └── Agent.h │ ├── allocator │ └── SensorAllocator.h │ ├── config │ └── Config.h │ ├── gpu │ ├── GridData.h │ ├── NodeWorker.h │ ├── PointCloud.h │ ├── Sensor.h │ └── SensorBucket.h │ ├── handler │ ├── BlockWorker.h │ ├── Handler.h │ ├── SimHandler.h │ └── handlerAssert.h │ ├── manager │ ├── Manager.h │ └── SimManager.h │ ├── map │ └── OccupancyMap.h │ ├── mapgen │ └── procedural │ │ └── CaveGen.h │ ├── nanomap.h │ └── sensor │ ├── FrustumData.h │ ├── LaserData.h │ ├── SensorData.h │ └── SharedParameters.h └── src └── nanomap ├── config └── Config.cpp ├── handler ├── BlockWorker.cpp ├── Handler.cpp └── SimHandler.cpp ├── kernels ├── filterCloud.cu ├── frustumCastCloud.cu └── generateCloud.cu ├── mapgen └── procedural │ └── CaveGen.cpp └── test ├── test2DCaveGen.cpp └── test3DCaveGen.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | install/ 3 | cmake/ 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.18) 2 | set(CMAKE_VERBOSE_MAKEFILE ON) 3 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 4 | set (CUDA_TOOLKIT_ROOT_DIR "/usr/local/cuda") 5 | set (CMAKE_CUDA_COMPILER "/usr/local/cuda/bin/nvcc") 6 | set(CMAKE_CUDA_STANDARD 11) 7 | set(CMAKE_CUDA_STANDARD_REQUIRED ON) 8 | Set(CUDA_SEPARABLE_COMPILATION ON) 9 | if(MSVC) 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zi") 11 | else() 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 13 | endif() 14 | project(nanomap_sim) 15 | set(nanomap_VERSION 0.9) 16 | 17 | 18 | if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.18) 19 | # Allow the user to provide CMAKE_CUDA_ARCHITECTURES 20 | if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) 21 | set(CMAKE_CUDA_ARCHITECTURES 72) 22 | endif() 23 | endif() 24 | enable_language(CUDA) 25 | 26 | set(NANOVDB_CUDA_EXTENDED_LAMBDA "--expt-extended-lambda") 27 | if(CUDA_VERSION_MAJOR GREATER_EQUAL 11) 28 | set(NANOVDB_CUDA_EXTENDED_LAMBDA "--extended-lambda") 29 | endif() 30 | 31 | set(CMAKE_CUDA_FLAGS "${NANOVDB_CUDA_EXTENDED_LAMBDA} -use_fast_math -lineinfo -rdc=true ${CMAKE_CUDA_FLAGS}") 32 | 33 | ## DEPENDENCY SETUP 34 | ##BLOSC 35 | set(Blosc_ROOT "$ENV{HOME}/github/third-party/lib/blosc") 36 | 37 | ##EIGEN 38 | ##list(APPEND CMAKE_MODULE_PATH "/usr/local/share/eigen3/cmake") 39 | ##list(APPEND CMAKE_MODULE_PATH "$ENV{HOME}/github/third-party/lib/eigen3/share/eigen3/cmake/") 40 | set(Eigen3_DIR "$ENV{HOME}/github/third-party/src/eigen-3.3.9/build") 41 | ##set(EIGEN3_INCLUDE_DIR "/usr/include/eigen3") 42 | find_package(Eigen3 3.3.9 REQUIRED) 43 | 44 | ##TBB 45 | find_package(TBB REQUIRED) 46 | 47 | ##OPENVDB 48 | set(OpenVDB_LIBRARYDIR "$ENV{HOME}/github/third-party/lib/openvdb/lib") 49 | list(APPEND CMAKE_MODULE_PATH "$ENV{HOME}/github/third-party/lib/openvdb/lib/cmake/OpenVDB") 50 | find_package(OpenVDB REQUIRED) 51 | 52 | ##NANOVDB 53 | set (NanoVDB_INCLUDE_DIR "$ENV{HOME}/github/third-party/lib/openvdb/include") 54 | 55 | ##CUDA 56 | set (CUDA_INCLUDE_DIR "/usr/local/cuda/include") 57 | find_package(CUDA REQUIRED) 58 | 59 | 60 | ############################################################################### 61 | ## NanoMap Library Definition 62 | 63 | set(NANOMAP_KERNEL_FILES 64 | src/nanomap/kernels/generateCloud.cu 65 | src/nanomap/kernels/filterCloud.cu 66 | src/nanomap/kernels/frustumCastCloud.cu 67 | ) 68 | 69 | set(NANOMAP_SRC_FILES 70 | src/nanomap/mapgen/procedural/CaveGen.cpp 71 | src/nanomap/handler/BlockWorker.cpp 72 | src/nanomap/handler/Handler.cpp 73 | src/nanomap/handler/SimHandler.cpp 74 | src/nanomap/config/Config.cpp 75 | ) 76 | 77 | add_library(nanomap 78 | STATIC 79 | ${NANOMAP_KERNEL_FILES} 80 | ${NANOMAP_SRC_FILES} 81 | ) 82 | target_link_libraries(nanomap 83 | PUBLIC 84 | ${OpenVDB_LIBRARIES} 85 | ${CUDA_LIBRARIES} 86 | -ltbb 87 | -lHalf 88 | ) 89 | target_include_directories(nanomap 90 | PUBLIC 91 | ${NanoVDB_INCLUDE_DIR} 92 | $ 93 | ${EIGEN3_INCLUDE_DIR} 94 | ${OpenVDB_INCLUDE_DIR} 95 | ${CUDA_INCLUDE_DIR} 96 | ) 97 | 98 | set_property(TARGET nanomap PROPERTY CUDA_RESOLVE_DEVICE_SYMBOLS ON) 99 | 100 | ############################################################################### 101 | ##Map Generation Executables 102 | 103 | add_executable(test3DCaveGen src/nanomap/test/test3DCaveGen.cpp) 104 | 105 | target_link_libraries(test3DCaveGen PUBLIC nanomap) 106 | 107 | add_executable(test2DCaveGen src/nanomap/test/test2DCaveGen.cpp) 108 | 109 | target_link_libraries(test2DCaveGen PUBLIC nanomap) 110 | 111 | 112 | ############################################################################### 113 | ##Installation instructions 114 | 115 | install(TARGETS nanomap EXPORT nanomaptargets 116 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 117 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 118 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 119 | INCLUDES DESTINATION ${CMAKE_INSTALL_PREFIX}/include 120 | DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 121 | ) 122 | set(NANOMAP_INCLUDE_DIR 123 | $ 124 | ) 125 | install( 126 | DIRECTORY 127 | ${NANOMAP_INCLUDE_DIR} 128 | DESTINATION 129 | ${CMAKE_INSTALL_PREFIX}/include 130 | COMPONENT 131 | Devel 132 | FILES_MATCHING PATTERN "*.h*" 133 | 134 | ) 135 | 136 | include(CMakePackageConfigHelpers) 137 | write_basic_package_version_file( 138 | "${CMAKE_CURRENT_BINARY_DIR}/nanomap/nanomap.cmake" 139 | VERSION ${nanomap_VERSION} 140 | COMPATIBILITY AnyNewerVersion 141 | ) 142 | 143 | export(EXPORT nanomaptargets 144 | FILE "${CMAKE_CURRENT_BINARY_DIR}/nanomap/nanomaptargets.cmake" 145 | NAMESPACE nanomap:: 146 | ) 147 | configure_file(cmake/nanomapConfig.cmake 148 | "${CMAKE_CURRENT_BINARY_DIR}/nanomap/nanomapConfig.cmake" 149 | COPYONLY 150 | ) 151 | 152 | set(ConfigPackageLocation lib/cmake/nanomap) 153 | install(EXPORT nanomaptargets 154 | FILE 155 | nanomaptargets.cmake 156 | NAMESPACE 157 | nanomap:: 158 | DESTINATION 159 | ${CMAKE_INSTALL_PREFIX}/${ConfigPackageLocation} 160 | ) 161 | install( 162 | FILES 163 | cmake/nanomapConfig.cmake 164 | "${CMAKE_CURRENT_BINARY_DIR}/nanomap/nanomapConfig.cmake" 165 | DESTINATION 166 | ${CMAKE_INSTALL_PREFIX}/${ConfigPackageLocation} 167 | COMPONENT 168 | Devel 169 | ) 170 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # NanoMap Library 4 | 5 | The aim of the NanoMap library is to provide accelerated occupancy mapping and simulation capabilities for robotic agents and systems equipped with CUDA capable GPUs. Only frustum style sensors such as RGB-D and Stereo Cameras have GPU accelerated support. LIDAR sensors can still be processed on the CPU, but due to the sparsity of the information they provide, do not benefit from the current methods used to accelerate frustum style sensors. The related publication is located here: https://www.mdpi.com/2072-4292/14/21/5463 6 | 7 | The Bibtex for the citation is: 8 | 9 | `@article{walker2022nanomap, 10 | title={NanoMap: A GPU-Accelerated OpenVDB-Based Mapping and Simulation Package for Robotic Agents}, 11 | author={Walker, Violet and Vanegas, Fernando and Gonzalez, Felipe}, 12 | journal={Remote Sensing}, 13 | volume={14}, 14 | number={21}, 15 | pages={5463}, 16 | year={2022}, 17 | publisher={Multidisciplinary Digital Publishing Institute} 18 | }` 19 | 20 | 21 | ## Disclaimer 22 | 23 | This library is currently in beta and under active development. 24 | 25 | The library is provided by the author “AS IS.” WITHOUT WARRANTY OF ANY KIND, EXPRESS, IMPLIED, IN FACT OR ARISING BY OPERATION OF LAW, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTY OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT AND DATA ACCURACY. 26 | 27 | This document is currently under construction and is subject to change at any time. 28 | 29 | ## Dependencies 30 | These are the main dependencies, currently this is not guaranteed to be an exhaustive list. 31 | 32 | * CUDA >= 10.0 33 | * CUB (This should be a part of the CUDA toolkit, otherwise install it separately) 34 | * Blosc == 1.5.0 35 | * Eigen3 == 3.3.9 36 | * Intel Threaded Building Blocks (apt-get install libtbb-dev) 37 | * OpenVDB built from source with NanoVDB support 38 | 39 | ## Currently Supported and tested Platforms 40 | 41 | * Desktops and Laptops with CUDA capable GPUs running ubuntu 20.04.3 LTS 42 | * Jetson Nano running latest compatible JetPack. (The jetson nano specific code will be uploaded shortly) 43 | 44 | ## Building NanoMap 45 | NanoMap is built using CMAKE >= 3.18. 46 | 47 | Make sure you have built or installed the required dependencies listed above. Currently the CMakeLists.txt uses hardcoded paths to most dependencies, so edit the CMakeLists.txt file to point to the locations where they are built/installed. 48 | 49 | Make a build directory in the NanoMap source folder and from inside run: 50 | 51 | `cmake .. -DCMAKE_INSTALL_PREFIX="Your_Desired_Install_Location"` 52 | 53 | then 54 | 55 | `make install` 56 | 57 | ## Supplemental Packages 58 | 59 | Once installed, the library is ready to use. It should build two executables that can be used to randomly generate simulation environments for use with the simulation components. The library itself is designed to be used by other packages. See nanomap_ros and nanomap_benchmark for more information and examples of usage. 60 | 61 | [nanomap_ros](https://github.com/ViWalkerDev/nanomap_ros) is a ROS package targeted for ROS1 and ROS2 that acts as an interface to use NanoMap with robotic agents. 62 | 63 | [nanomap_benchmark](https://github.com/ViWalkerDev/nanomap_benchmark) is another package used for benchmarking the performance of NanoMap and its different modes of operation against Octomap. 64 | 65 | [nanomap_msgs](https://github.com/ViWalkerDev/nanomap_msgs) is a msg package that is used by nanomap_ros for sending openvdb grid objects between nodes. 66 | 67 | [nanomap_rviz2_plugins](https://github.com/ViWalkerDev/nanomap_rviz2_plugins) is an rviz2 plugin for rendering the openvdb grid messages defined by nanomap_msgs and sent by nanomap_ros. 68 | 69 | ## Performance 70 | [This](https://youtu.be/UBrlLRqY_E4) is a video of the sensor simulation and map generation capabilities provided by NanoMap and the nanomap_ros package. Functionality is basic, but performance is good. The time to generate and then process the pointclouds took between 2-10ms for a frustum sensor configuration with 10m range and 20-40ms for a LIDAR with 20m range. The test was performed on a laptop with a Ryzen 4900HS and RTX 2060 MaxQ GPU. 71 | 72 | On the Jetson Nano, by taking advantage of the GPU, NanoMap can provide a 5x performance improvement over OctoMap at mapping resolutions of 0.1m, the performance difference only grows at higher resolutions. The Jetson Nano using NanoMap is capable of processing a kinect style depth camera sensor input capped at 5m in approximately 10ms. At 0.05m mapping resolution the same sensor can be processed in approximately 30ms. 73 | -------------------------------------------------------------------------------- /cmake/nanomapConfig.cmake: -------------------------------------------------------------------------------- 1 | include("${CMAKE_CURRENT_LIST_DIR}/nanomaptargets.cmake") 2 | -------------------------------------------------------------------------------- /config/agents/Agent.txt: -------------------------------------------------------------------------------- 1 | #agentconfig 2 | Spawn: 3 | Type: 4 | fixed 5 | Position: 6 | 3 0 0 7 | Orientation: 8 | 0 0 0 0 9 | Observations: 10 | NumObs: 11 | 80 12 | LenObs: 13 | 20 14 | CollisionDist: 15 | 0.5 16 | Sensor: 17 | Name: 18 | 640x480 19 | Position: 20 | 0 0 0 21 | Orientation: 22 | 1.0 0.0 0.0 0.0 23 | #endconfig 24 | -------------------------------------------------------------------------------- /config/config.txt: -------------------------------------------------------------------------------- 1 | #config 2 | MappingRes: 3 | 0.1 4 | ProbHitThres: 5 | 0.5 6 | ProbMissThres: 7 | 0.5 8 | GridRes: 9 | 0.1 10 | NodeEdge: 11 | 8 12 | FrustumAllocationFactor: 13 | 0.4 14 | FilterType: 15 | 1 16 | UpdateType: 17 | 0 18 | PrecisionType: 19 | 0 20 | PublishSensor: 21 | 1 22 | SimGrid: 23 | %ABSOLUTE PATH TO SIM GRID HERE 24 | Sensor: 25 | %ABSOLUTE PATH TO SENSOR CONFIG FILE HERE 26 | Agent: 27 | %ABSOLUTE PATH TO AGENT CONFIG FILE HERE 28 | #endconfig 29 | -------------------------------------------------------------------------------- /config/mapgen/MapGen.txt: -------------------------------------------------------------------------------- 1 | #cavegenconfig 2 | file_out map1.vdb 3 | map_size 50 50 3.5 4 | grid_res 0.1 5 | fill_percentage 56 6 | smoothing_count 4 7 | active 4 4 8 | random_seed 1 9 | seed 10 | -------------------------------------------------------------------------------- /config/maps/Map.txt: -------------------------------------------------------------------------------- 1 | #mapconfig 2 | File: 3 | %ABSOLUTE PATH TO VDB FILE FOR SIMULATION 4 | Resolution: 5 | 0.05 6 | #endconfig 7 | -------------------------------------------------------------------------------- /config/sensors/640x480.txt: -------------------------------------------------------------------------------- 1 | #frustumconfig 2 | Name: 3 | 640x480 4 | Id: 5 | 1000 6 | HRes: 7 | 640 8 | VRes: 9 | 480 10 | VFOV: 11 | 43 12 | Rate: 13 | 50 14 | MaxRange: 15 | 10 16 | MinRange: 17 | 0.1 18 | ProbHit: 19 | 0.95 20 | ProbMiss: 21 | 0.3 22 | FrameTransform: 23 | 0.0 -1.0 0.0 24 | 0.0 0.0 1.0 25 | 1.0 0.0 0.0 26 | #endconfig 27 | -------------------------------------------------------------------------------- /config/sensors/VLP16.txt: -------------------------------------------------------------------------------- 1 | #laserconfig 2 | Name: 3 | VLP16 4 | Id: 5 | 1001 6 | AngularHRes: 7 | 0.4 8 | AngularVRes: 9 | 2 10 | HFOV: 11 | 360 12 | VFOV: 13 | 30 14 | Rate: 15 | 20 16 | MaxRange: 17 | 50 18 | MinRange: 19 | 1 20 | ProbHit: 21 | 0.95 22 | ProbMiss: 23 | 0.3 24 | FrameTransform: 25 | 0.0 -1.0 0.0 26 | 0.0 0.0 1.0 27 | 1.0 0.0 0.0 28 | #endconfig 29 | -------------------------------------------------------------------------------- /include/nanomap/agent/Agent.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Agent.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_AGENT_AGENT_H_INCLUDED 9 | #define NANOMAP_AGENT_AGENT_H_INCLUDED 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "nanomap/map/OccupancyMap.h" 19 | #include "nanomap/nanomap.h" 20 | 21 | 22 | /****************************************************************************** 23 | This class defines a basic agent. It is used for single body UAVs with one or 24 | more FIXED sensors and doesn't currently contain more advanced robotics kinematics like 25 | Joint and Link definitions. update the UAV pose, then use the class to update the 26 | sensor poses and pass a map file to the sensors to update the map with the new 27 | sensor views. This also uses a map file to generate observation distances for 28 | deeprl. Rays for an agent are generally loaded from a file and are precomputed. 29 | however, a spherical observation sphere of radius r is generated if file not provided. 30 | *******************************************************************************/ 31 | 32 | namespace nanomap{ 33 | namespace agent{ 34 | 35 | using Map = nanomap::map::Map; 36 | using FloatGrid = openvdb::FloatGrid; 37 | 38 | 39 | using ValueT = float; 40 | using Vec3T = nanovdb::Vec3; 41 | 42 | using EigenVec = Eigen::Matrix; 43 | using EigenMat = Eigen::Matrix; 44 | using Quaternion = Eigen::Quaternion; 45 | 46 | class Agent{ 47 | 48 | public: 49 | Agent(openvdb::FloatGrid::Ptr& simGrid, std::string agentFile, 50 | float mappingRes, float gridRes, float probHitThres, float probMissThres) 51 | :_mappingRes(mappingRes) 52 | ,_gridRes(gridRes) 53 | ,_probHitThres(probHitThres) 54 | ,_probMissThres(probMissThres) 55 | { 56 | _map = std::make_shared(Map(_mappingRes, _probHitThres, _probMissThres)); 57 | std::cout <<"reading in agent config file: " << agentFile << std::endl; 58 | bool isRandom = false; 59 | ValueT x,y,z,qx,qy,qz,qw,lenObs, numObs, collisionDist; 60 | std::string name, line; 61 | openvdb::Vec3f VecSpawn; 62 | Pose pose; 63 | std::ifstream *input = new std::ifstream(agentFile.c_str(), std::ios::in | std::ios::binary); 64 | bool end = false; 65 | *input >> line; 66 | if(line.compare("#agentconfig") != 0){ 67 | std::cout << "Error: first line reads [" << line << "] instead of [#agentconfig]" << std::endl; 68 | delete input; 69 | return; 70 | } 71 | while(input->good()) { 72 | *input >> line; 73 | if(line.compare("Spawn:") == 0){ 74 | *input >> line; 75 | *input >> line; 76 | if(line.compare("Random") == 0){ 77 | _spawnRandom = true; 78 | //VecSpawn = nanomap::util::getAgentSpawn(simGrid, _gridRes); 79 | _pose.position = Eigen::Matrix(0.0,0.0,0.0); 80 | Eigen::Matrix rotation; 81 | //Right 82 | rotation.col(0)=Eigen::Matrix(0.0,-1.0,0.0); 83 | //Forward 84 | rotation.col(1)=Eigen::Matrix(1.0,0.0,0.0); 85 | //Up 86 | rotation.col(2)=Eigen::Matrix(0.0,0.0,1.0); 87 | _pose.orientation = Eigen::Quaternionf(rotation); 88 | }else{ 89 | _spawnRandom = false; 90 | *input>>line; 91 | *input>>x>>y>>z; 92 | *input>>line; 93 | *input>>qx>>qy>>qz>>qw; 94 | _pose.position = Eigen::Vector3f(x,y,z); 95 | Eigen::Matrix rotation; 96 | //Right 97 | rotation.col(0)=Eigen::Matrix(0.0,-1.0,0.0); 98 | //Forward 99 | rotation.col(1)=Eigen::Matrix(1.0,0.0,0.0); 100 | //Up 101 | rotation.col(2)=Eigen::Matrix(0.0,0.0,1.0); 102 | _pose.orientation = Eigen::Quaternionf(rotation); 103 | } 104 | }else if(line.compare("Observations:")==0){ 105 | *input>>line; 106 | *input >> numObs; 107 | *input >> line; 108 | *input >> lenObs; 109 | *input >> line; 110 | *input >> collisionDist; 111 | }else if(line.compare("Sensor:")==0){ 112 | *input>>line; 113 | *input>>name; 114 | *input>>line; 115 | *input>>x>>y>>z; 116 | *input>>line; 117 | *input>>qw>>qx>>qy>>qz; 118 | pose.position = Eigen::Vector3f(x,y,z); 119 | pose.orientation = Eigen::Quaternionf(qw,qx,qy,qz); 120 | _sensorNames.push_back(name); 121 | _sensorOrigins.push_back(pose); 122 | }else if (line.compare("#endconfig")==0){ 123 | break; 124 | } 125 | } 126 | input->close(); 127 | _sensorPoses = _sensorOrigins; 128 | _lenObs = lenObs/_mappingRes; 129 | _numObs = numObs; 130 | _collisionDist = (float)(collisionDist/_gridRes); 131 | _observationRays(3,_numObs); 132 | _observationNorms(2,_numObs); 133 | generateObservationSphere(_numObs, _collisionDist, _lenObs, 134 | _observationRays, _observationNorms); 135 | } 136 | void updatePose(Pose pose){ 137 | _pose = pose; 138 | int index = 0; 139 | Eigen::Matrix defaultFrameTransform; 140 | for(auto itr = _sensorOrigins.begin(); itr != _sensorOrigins.end(); itr++){ 141 | index = std::distance(_sensorOrigins.begin(), itr); 142 | _sensorPoses[index].position = _pose.position; 143 | _sensorPoses[index].orientation = _pose.orientation*((*itr).orientation); 144 | } 145 | } 146 | 147 | Eigen::Vector3f sphericalCoordinate(float x, float y){ 148 | Eigen::Vector3f point; 149 | point(0)= std::cos(x)*std::cos(y); 150 | point(1)= std::sin(x)*std::cos(y); 151 | point(2)= std::sin(y); 152 | return point; 153 | } 154 | 155 | Eigen::Matrix normalisedSphere(int n, float x){ 156 | Eigen::Matrix sphere_points(3,n); 157 | float start = (-1.0+1.0/(n-1.0)); 158 | float increment = (2.0-2.0/(n-1.0))/(n-1.0); 159 | float s,j,k; 160 | for(int i = 0; i < n; i++){ 161 | s = start+i*increment; 162 | j = s*x; 163 | k = (M_PI/2.0)*std::copysign(1.0,s)*(1.0-std::sqrt(1.0-std::abs(s))); 164 | sphere_points.col(i) = sphericalCoordinate(j, k); 165 | sphere_points.col(i).normalize(); 166 | } 167 | return sphere_points; 168 | } 169 | 170 | 171 | 172 | 173 | 174 | Eigen::Matrix generateSphere(int n){ 175 | //std::cout << "1" << std::endl; 176 | 177 | return normalisedSphere(n, (0.1+1.2*n)); 178 | } 179 | 180 | 181 | 182 | 183 | void generateObservationSphere(int numObs, float collisionDist, float lenObs, 184 | Eigen::Matrix& observationRays, 185 | Eigen::Matrix& observationNorms){ 186 | observationRays = generateSphere(numObs); 187 | Eigen::Matrix norms(2,numObs); 188 | for(int x = 0; x < numObs; x++){ 189 | norms.col(x) = Eigen::Matrix(collisionDist,lenObs); 190 | } 191 | observationNorms = norms; 192 | } 193 | 194 | Pose pose(){return _pose;} 195 | 196 | std::vector sensorOrigins(){return _sensorOrigins;} 197 | std::vector sensorPoses(){return _sensorPoses;} 198 | void clearSensors(){_sensorPoses.clear(); 199 | _sensorNames.clear(); 200 | _sensorOrigins.clear();} 201 | 202 | void clearObservationRays(){ _observationNorms.resize(0,0); 203 | _observationRays.resize(0,0);} 204 | Eigen::Matrix observationRays(){return _observationRays;} 205 | Eigen::Matrix observationNorms(){return _observationNorms;} 206 | void resetAgent(){clearSensors(); 207 | clearObservationRays();} 208 | Pose sensorPose(int index){return _sensorPoses[index];} 209 | 210 | std::shared_ptr map(){return _map;} 211 | std::vector sensorNames() {return _sensorNames;} 212 | int getId(){return _agentId;} 213 | std::string getName(){return _agentName;} 214 | bool spawnRandom(){return _spawnRandom;} 215 | protected: 216 | std::string _agentName; 217 | int _agentId; 218 | float _gridRes, _mappingRes, _probHitThres, _probMissThres; 219 | Pose _pose; 220 | int _numObs; 221 | float _lenObs; 222 | float _collisionDist; 223 | bool _spawnRandom; 224 | Eigen::Matrix _observationRays; 225 | Eigen::Matrix _observationNorms; 226 | Eigen::ArrayXd _observations; 227 | std::vector _sensorNames; 228 | std::vector _sensorOrigins; 229 | std::vector _sensorPoses; 230 | std::shared_ptr _map; 231 | 232 | }; 233 | } 234 | } 235 | #endif 236 | -------------------------------------------------------------------------------- /include/nanomap/allocator/SensorAllocator.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SensorAllocator.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_HANDLER_SENSORALLOCATOR_H_INCLUDED 9 | #define NANOMAP_HANDLER_SENSORALLOCATOR_H_INCLUDED 10 | #include "nanomap/handler/handlerAssert.h" 11 | #include "cuda_fp16.h" 12 | #include 13 | #include "nanomap/gpu/NodeWorker.h" 14 | #include "nanomap/gpu/PointCloud.h" 15 | #include "nanomap/gpu/Sensor.h" 16 | #include "nanomap/gpu/GridData.h" 17 | #include "nanomap/gpu/SensorBucket.h" 18 | #include "nanomap/sensor/SensorData.h" 19 | #include "nanomap/config/Config.h" 20 | #include "nanomap/nanomap.h" 21 | 22 | 23 | 24 | namespace nanomap{ 25 | namespace allocator{ 26 | //using BufferT = nanovdb::CudaDeviceBuffer; 27 | using ValueT = float; 28 | using EigenVec = Eigen::Matrix; 29 | class SensorAllocator{ 30 | 31 | public: 32 | 33 | SensorAllocator(std::shared_ptr config) 34 | :_sb(std::make_shared(config->maxPCLSize(), config->frustumLeafBufferSize())), 35 | _cf(config) 36 | { 37 | _sb->setGridRes(_cf->gridRes()); 38 | _sb->setMappingRes(_cf->mappingRes()); 39 | _sb->setFilterType(_cf->filterType()); 40 | _sb->setProcessType(_cf->processType()); 41 | //If using node optimisation for frustum cameras 42 | if(_cf->processType()==1){ //|| _cf->processType() == 2){ 43 | //Arrays used for sorting the active node information. 44 | cudaCheck(cudaMalloc((void**)&((*_sb)._activeLeafNodes), _cf->frustumLeafBufferSize()*sizeof(int))); 45 | //std::cout << "1" << std::endl; 46 | cudaCheck(cudaMalloc((void**)&((*_sb)._activeFlags), _cf->frustumLeafBufferSize()*sizeof(int))); 47 | //std::cout << "1" << std::endl; 48 | cudaCheck(cudaMalloc((void**)&((*_sb)._activeIndices), _cf->frustumLeafBufferSize()*sizeof(int))); 49 | //Set Voxel allocation. This can be changed as needed, and will resize as necessary, 50 | //but resizing is costly, so it is a good number to reduce memory consumption 51 | //While also preventing a lot of resizing when using a frustum style sensor. 52 | //Default factor of 0.4 means that container size for voxel representation set to 2/5th of max observable area for a given sensor. 53 | _frustumVoxelAllocation = (_cf->frustumLeafBufferSize()*(_cf->leafVolume())*_cf->frustumAllocationFactor()); 54 | //Frustum voxel worker contains the probablistic occupancy result of raycasting on GPU 55 | cudaCheck(cudaMalloc((void**)&((*_sb)._devFrustumVoxelWorker), _frustumVoxelAllocation*sizeof(float))); 56 | //Frustum leaf buffer contains the coordinates of active leafNodes. 57 | cudaCheck(cudaMalloc((void**)&((*_sb)._devFrustumLeafBuffer), _cf->frustumLeafBufferSize()*3*sizeof(int))); 58 | cudaCheck(cudaMallocHost((void**)&((*_sb)._hostFrustumLeafBuffer), _cf->frustumLeafBufferSize()*3*sizeof(int))); 59 | //Frustum Voxel buffer is used to transfer contents of the frustum voxel worker to CPU, uses int8_t type to reduce the size of the container, 60 | //And improve memcpy times as this step happens each loop. 61 | cudaCheck(cudaMalloc((void**)&((*_sb)._devFrustumVoxelBuffer), _frustumVoxelAllocation*sizeof(int8_t))); 62 | cudaCheck(cudaMallocHost((void**)&((*_sb)._hostFrustumVoxelBuffer), _frustumVoxelAllocation*sizeof(int8_t))); 63 | } 64 | if(_cf->filterType() == 0){ 65 | //Do Nothing 66 | }else if(_cf->filterType() == 1 && (_cf->processType() == 0 || _cf->processType() == 2)){ 67 | //VOXEL FILTER SETTING 68 | //ONLY USEFUL FOR FRUSTUM CAMERA 69 | //Filtering increases runtime but also increases memory usage. 70 | //This is usually only a problem for sensors with long range, and/or sensors with high grid resolution. 71 | //If you have the memory, the speed ups are 72 | //Generally worth the cost. 73 | //Once the grid resolution becomes too fine to meaningfully recude the input sensor cloud 74 | //the performance benefit is lost, and the overhead causes slowdown. 75 | // Four precision modes are provided 76 | 77 | if(_cf->precisionType() == 0){ 78 | //Precision == 0, provides full xyz precision for discretising rays to a voxel but costs the most memory 79 | _sb->setPrecisionType(_cf->precisionType()); 80 | cudaCheck(cudaMalloc((void**)&((*_sb)._voxelFilterBuffer), _cf->leafVolume()*4*(_cf->frustumLeafBufferSize())*sizeof(float))); 81 | }else if(_cf->precisionType() == 1){ 82 | //Precision == 1, provides (half2 type) xyz offset precision for discretising rays to a voxel. It costs half as much memory as precision 0 83 | //This is not supported on the Jetson Nano as the nano only supports Compute capability 5.4. This does work on the Xavier NX. 84 | //The jetson nano version of this code uses 16 bit integers to store an offset value that has been multiplied by 100. 85 | //This uses half the memory, which is good for a memory constrained platform, but loses some precision. 86 | //Uses 1/2 the memory of Precision = 0; 87 | _sb->setPrecisionType(_cf->precisionType()); 88 | cudaCheck(cudaMalloc((void**)&((*_sb)._voxelFilterBufferHalf2), _cf->leafVolume()*2*(_cf->frustumLeafBufferSize())*sizeof(__half2))); 89 | }else if(_cf->precisionType() == 2){ 90 | //Precision 2 doesn't even bother tracking ray offsets within a voxel, if a ray ends in a voxel, a counter is incremented for that voxel, 91 | //The resultant ray is always directed at the center of the voxel in question. 92 | //This is slightly less accurate, but uses significantly less memory. Great if you don't mind the loss of precision. 93 | //For smaller voxel sizes the loss of precision becomes smaller. 94 | //Uses 1/8th of the memory of Precision = 0; 95 | //Uses 1/4th the memory of Precision = 1; 96 | _sb->setPrecisionType(_cf->precisionType()); 97 | cudaCheck(cudaMalloc((void**)&((*_sb)._voxelFilterBufferSimple), _cf->leafVolume()*(_cf->frustumLeafBufferSize())*sizeof(int))); 98 | }else if(_cf->precisionType() == 3){ 99 | //Precision 3 tracks voxel count only, the same as Precision 2. 100 | //But it uses 1/4th the memory as Precision 2 by condensing 4 counts into a single 32 bit int, 101 | //for some resolutions this can cause bottlenecks for atomicAdd operations due to sharing single ints 102 | _sb->setPrecisionType(_cf->precisionType()); 103 | cudaCheck(cudaMalloc((void**)&((*_sb)._voxelFilterBufferSimpleQuad), _cf->leafVolume()*(std::ceil(_cf->frustumLeafBufferSize()/4))*sizeof(int))); 104 | } 105 | } 106 | cudaDeviceSynchronize(); 107 | _sb->hostGridData()->init(_cf->mappingRes(), _cf->leafEdge(), _cf->frustumLeafBufferSize()); 108 | } 109 | 110 | void update(int pcl_width, int pcl_height, int pcl_step, unsigned char* cloud, 111 | std::shared_ptr sensorData, cudaStream_t s0){ 112 | if(sensorData->sharedParameters()._type == 0){ 113 | _sb->hostSensor()->updateFrustum(sensorData->sharedParameters()); 114 | }else if(sensorData->sharedParameters()._type == 1){ 115 | _sb->hostSensor()->updateLaser(sensorData->sharedParameters()); 116 | } 117 | _sb->hostGridData()->update(sensorData->sharedParameters()._voxelBounds, 118 | sensorData->sharedParameters()._leafBounds, 119 | sensorData->sharedParameters()._leafBufferSize); 120 | cudaCheck(cudaMemcpy(_sb->devSensor(), _sb->hostSensor(), sizeof(nanomap::gpu::Sensor), cudaMemcpyHostToDevice)); 121 | cudaCheck(cudaMemcpy(_sb->devGridData(), _sb->hostGridData(), sizeof(nanomap::gpu::GridData), cudaMemcpyHostToDevice)); 122 | *(_sb->hostFrustumLeafCount()) = 0; 123 | cudaCheck(cudaMemcpy(_sb->devFrustumLeafCount(), _sb->hostFrustumLeafCount(), sizeof(int), cudaMemcpyHostToDevice)); 124 | *(_sb->hostRayCount()) = 0; 125 | cudaCheck(cudaMemcpy(_sb->devRayCount(), _sb->hostRayCount(), sizeof(int), cudaMemcpyHostToDevice)); 126 | _sb->pclHandle().updatePointCloudHandle(pcl_height*pcl_width, pcl_step, cloud); 127 | _sb->pclHandle().deviceUpload(s0); 128 | cudaDeviceSynchronize(); 129 | 130 | } 131 | 132 | void update(std::shared_ptr sensorData, cudaStream_t s0){ 133 | if(sensorData->sharedParameters()._type == 0){ 134 | _sb->hostSensor()->updateFrustum(sensorData->sharedParameters()); 135 | }else if(sensorData->sharedParameters()._type == 1){ 136 | _sb->hostSensor()->updateLaser(sensorData->sharedParameters()); 137 | } 138 | 139 | _sb->hostGridData()->update(sensorData->sharedParameters()._voxelBounds, 140 | sensorData->sharedParameters()._leafBounds, 141 | sensorData->sharedParameters()._leafBufferSize); 142 | cudaCheck(cudaMemcpy(_sb->devSensor(), _sb->hostSensor(), sizeof(nanomap::gpu::Sensor), cudaMemcpyHostToDevice)); 143 | cudaCheck(cudaMemcpy(_sb->devGridData(), _sb->hostGridData(), sizeof(nanomap::gpu::GridData), cudaMemcpyHostToDevice)); 144 | *(_sb->hostFrustumLeafCount()) = 0; 145 | cudaCheck(cudaMemcpy(_sb->devFrustumLeafCount(), _sb->hostFrustumLeafCount(), sizeof(int), cudaMemcpyHostToDevice)); 146 | *(_sb->hostRayCount()) = 0; 147 | cudaCheck(cudaMemcpy(_sb->devRayCount(), _sb->hostRayCount(), sizeof(int), cudaMemcpyHostToDevice)); 148 | } 149 | 150 | void downloadCloudToSensor(std::shared_ptr sensorData, cudaStream_t s0){ 151 | _sb->pclHandle().deviceDownload(s0); 152 | cudaDeviceSynchronize(); 153 | int cloudSize = sensorData->sharedParameters()._hRes*sensorData->sharedParameters()._vRes; 154 | nanomap::gpu::PointCloud* pcl = _sb->pclHandle().pointCloud(); 155 | for(int i = 0; i < cloudSize; i++){ 156 | if((*(pcl))(i).norm >= 0.0){ 157 | (sensorData->pointCloud()).col(i)(0) = ((*(pcl))(i).x); 158 | (sensorData->pointCloud()).col(i)(1) = ((*(pcl))(i).y); 159 | (sensorData->pointCloud()).col(i)(2) = ((*(pcl))(i).z); 160 | }else{ 161 | (sensorData->pointCloud()).col(i)(0) = ((*(pcl))(i).x); 162 | (sensorData->pointCloud()).col(i)(1) = ((*(pcl))(i).y); 163 | (sensorData->pointCloud()).col(i)(2) = nanf(""); 164 | } 165 | 166 | } 167 | } 168 | std::shared_ptr sensorBucket(){return _sb;} 169 | private: 170 | /******************************************************************************/ 171 | int _frustumVoxelAllocation = 0; 172 | int _laserVoxelAllocation = 0; 173 | std::shared_ptr _cf; 174 | std::shared_ptr _sb; 175 | // }; 176 | 177 | 178 | }; 179 | } 180 | } 181 | #endif 182 | -------------------------------------------------------------------------------- /include/nanomap/config/Config.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Config.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #ifndef NANOMAP_CONFIG_CONFIG_H_INCLUDED 10 | #define NANOMAP_CONFIG_CONFIG_H_INCLUDED 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include "nanomap/nanomap.h" 21 | #include "nanomap/sensor/SensorData.h" 22 | #include "nanomap/sensor/FrustumData.h" 23 | #include "nanomap/sensor/LaserData.h" 24 | #include "nanomap/agent/Agent.h" 25 | 26 | 27 | 28 | #define M_PI 3.14159265358979323846 29 | #define MAX_INT 32766 30 | #define VOXEL_SIZE 1 31 | 32 | namespace nanomap{ 33 | namespace config{ 34 | //Config class. This contains all relevant user defined information for operation. 35 | //Config information is loaded using config text files. 36 | class Config 37 | { 38 | public: 39 | //Empty Constructor 40 | Config(){} 41 | //Constructor that takes main config file as input 42 | Config(std::string configFile); 43 | void init(); 44 | 45 | 46 | 47 | 48 | void loadConfig(); 49 | void loadAgents(openvdb::FloatGrid::Ptr& simGrid); 50 | void loadAgentData(openvdb::FloatGrid::Ptr& simGrid); 51 | void loadSensors(); 52 | std::shared_ptr loadSensorData(float gridRes, int nodeEdge, std::string config); 53 | 54 | 55 | /******************************************************************************/ 56 | //Fetching Sensor and Agent Data; 57 | //return full sensor data vector 58 | std::vector>& sensorData(); 59 | std::vector>& agentData(); 60 | //return sensor data object by vector index or sensorId. 61 | std::shared_ptr sensorData(int index, bool searchById=false); 62 | std::shared_ptr agentData(int index, bool searchById=false); 63 | 64 | std::shared_ptr sensorData(std::string name); 65 | std::shared_ptr agentData(std::string name); 66 | /******************************************************************************/ 67 | //Fetch Mode variables 68 | int& filterType(){return _filterType;} 69 | int& updateType(){return _updateType;} 70 | int& exploreType(){return _exploreType;} 71 | int& processType(){return _processType;} 72 | int& precisionType(){return _precisionType;} 73 | int& simType(){return _simType;} 74 | int& publishSensor(){return _publishSensor;} 75 | float& gridRes(){return _gridRes;} 76 | float& mappingRes(){return _mappingRes;} 77 | int& leafEdge(){return _leafEdge;} 78 | int& leafVolume(){return _leafVolume;} 79 | float& frustumAllocationFactor(){return _frustumAllocationFactor;} 80 | float& laserAllocationFactor(){return _laserAllocationFactor;} 81 | int& maxPCLSize(){return _maxPCLSize;} 82 | int& laserPCLSize(){return _laserPCLSize;} 83 | int& laserVoxelBufferSize(){return _laserVoxelBufferSize;} 84 | int& frustumPCLSize(){return _frustumPCLSize;} 85 | int& frustumLeafBufferSize(){return _frustumLeafBufferSize;} 86 | bool& serialUpdate(){return _serialUpdate;} 87 | float& probHitThres(){return _probHitThres;} 88 | float& probMissThres(){return _probMissThres;} 89 | /****************************************************************************/ 90 | //Fetch _config 91 | std::string& config(){return _config;} 92 | 93 | /******************************************************************************/ 94 | 95 | 96 | /****************************************************************************/ 97 | 98 | private: 99 | bool _configCheck=false; 100 | bool _serialUpdate=false; 101 | std::string _config; 102 | //Variables that define mode of operation. 103 | float _gridRes; 104 | float _mappingRes; 105 | int _leafEdge; 106 | int _leafVolume; 107 | 108 | int _filterType; 109 | int _exploreType; 110 | int _updateType; 111 | int _processType; 112 | int _simType; 113 | int _precisionType; 114 | int _publishSensor; 115 | 116 | float _probHitThres; 117 | float _probMissThres; 118 | 119 | 120 | // 121 | int _maxPCLSize; 122 | int _frustumPCLSize; 123 | float _frustumAllocationFactor; 124 | int _frustumLeafBufferSize; 125 | int _laserPCLSize; 126 | float _laserAllocationFactor; 127 | int _laserVoxelBufferSize; 128 | 129 | 130 | 131 | //Contains the sensors definitions used by agents 132 | std::vector> _sensorData; 133 | //Contains the agent information 134 | std::vector> _agentData; 135 | }; 136 | } 137 | } 138 | 139 | 140 | 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /include/nanomap/gpu/GridData.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file GridData.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | /// @brief a griddata class for managing griddata information for CUDA kernels. 9 | 10 | 11 | 12 | #ifndef NANOMAP_GPU_GRIDDATA_H_HAS_BEEN_INCLUDED 13 | #define NANOMAP_GPU_GRIDDATA_H_HAS_BEEN_INCLUDED 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "nanomap/nanomap.h" 25 | namespace nanomap{ 26 | namespace gpu{ 27 | 28 | 29 | 30 | class GridData 31 | { 32 | 33 | using ValueT = float; 34 | public: 35 | __hostdev__ GridData() 36 | {} 37 | 38 | __hostdev__ void init(ValueT gridRes, int leafEdge, int maxNodeBufferSize){ 39 | m_gridRes = gridRes; 40 | m_leafEdge = leafEdge; 41 | m_leafFace = leafEdge*leafEdge; 42 | m_leafVolume = leafEdge*leafEdge*leafEdge; 43 | m_maxNodeBufferSize = maxNodeBufferSize; 44 | } 45 | 46 | __hostdev__ void update(nanovdb::CoordBBox voxelBounds, 47 | nanovdb::CoordBBox nodeBounds, 48 | int nodeBufferSize){ 49 | m_voxelBounds = voxelBounds; 50 | 51 | m_voxelDim = voxelBounds.dim(); 52 | 53 | m_voxelMin = voxelBounds.min(); 54 | 55 | m_nodeBounds = nodeBounds; 56 | 57 | m_nodeDim = nodeBounds.dim(); 58 | 59 | m_nodeMin = nodeBounds.min(); 60 | 61 | m_nodeBufferSize = nodeBufferSize; 62 | } 63 | 64 | __hostdev__ const int maxNodeBufferSize() const {return m_maxNodeBufferSize;} 65 | __hostdev__ const int nodeBufferSize() const {return m_nodeBufferSize;} 66 | __hostdev__ const nanovdb::CoordBBox nodeBounds() const {return m_nodeBounds;} 67 | __hostdev__ const nanovdb::Coord nodeDim() const {return m_nodeDim;} 68 | __hostdev__ const nanovdb::Coord nodeMin() const {return m_nodeMin;} 69 | __hostdev__ const nanovdb::Coord voxelDim() const {return m_voxelDim;} 70 | __hostdev__ const nanovdb::Coord voxelMin() const {return m_voxelMin;} 71 | __hostdev__ const int leafEdge() const {return m_leafEdge;} 72 | __hostdev__ const int leafFace() const {return m_leafFace;} 73 | __hostdev__ const int leafVolume() const {return m_leafVolume;} 74 | 75 | protected: 76 | 77 | ValueT m_gridRes; 78 | int m_maxNodeBufferSize, m_nodeBufferSize; 79 | nanovdb::CoordBBox m_nodeBounds, m_voxelBounds; 80 | nanovdb::Coord m_nodeDim, m_nodeMin, m_voxelDim, m_voxelMin; 81 | int m_leafEdge, m_leafFace, m_leafVolume; 82 | 83 | }; // griddata 84 | } //namespace gpu 85 | } // namespace nanomap 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /include/nanomap/gpu/NodeWorker.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file NodeWorker.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | /// @brief a class for managing node information during CUDA kernel calculations. 9 | 10 | 11 | #ifndef NANOMAP_GPU_NODEWORKER_H_HAS_BEEN_INCLUDED 12 | #define NANOMAP_GPU_NODEWORKER_H_HAS_BEEN_INCLUDED 13 | 14 | #include // for uint8_t 15 | #include // for std::string 16 | #include // for std::ofstream 17 | #include 18 | #include 19 | 20 | namespace nanomap{ 21 | namespace gpu{ 22 | 23 | struct NodeWorkerData 24 | { 25 | 26 | int m_maxSize; 27 | int m_nodeCount; 28 | NodeWorkerData(int maxSize) 29 | : m_maxSize(maxSize) 30 | , m_nodeCount(0) 31 | { 32 | } 33 | }; 34 | 35 | class NodeWorker : private NodeWorkerData 36 | { 37 | using DataT = NodeWorkerData; 38 | public: 39 | 40 | struct Node 41 | { 42 | int active; 43 | int index; 44 | __hostdev__ Node(int _active, int _index) 45 | : active(_active) 46 | , index(_index) 47 | { 48 | } 49 | }; 50 | 51 | __hostdev__ void clear(); 52 | __hostdev__ int maxSize() const { return DataT::m_maxSize; } 53 | __hostdev__ int& nodeCount() {return DataT::m_nodeCount; } 54 | __hostdev__ inline Node& operator()(int index); 55 | __hostdev__ inline Node& operator()(int x, int y, int z, int xDim, int yDim, int zDim); 56 | }; 57 | 58 | template 59 | class NodeWorkerHandle 60 | { 61 | BufferT m_Buffer; 62 | 63 | public: 64 | NodeWorkerHandle(int maxSize); 65 | 66 | const NodeWorker* nodeWorker() const { return reinterpret_cast(m_Buffer.data()); } 67 | 68 | NodeWorker* nodeWorker() { return reinterpret_cast(m_Buffer.data()); } 69 | 70 | template 71 | typename std::enable_if::hasDeviceDual, const NodeWorker*>::type 72 | deviceNodeWorker() const { return reinterpret_cast(m_Buffer.deviceData()); } 73 | 74 | template 75 | typename std::enable_if::hasDeviceDual, NodeWorker*>::type 76 | deviceNodeWorker() { return reinterpret_cast(m_Buffer.deviceData()); } 77 | 78 | template 79 | typename std::enable_if::hasDeviceDual, NodeWorker*>::type 80 | deviceSize() { return reinterpret_cast(m_Buffer.size()); } 81 | 82 | template 83 | typename std::enable_if::hasDeviceDual, void>::type 84 | deviceUpload(void* stream = nullptr, bool sync = true) { m_Buffer.deviceUpload(stream, sync); } 85 | 86 | template 87 | typename std::enable_if::hasDeviceDual, void>::type 88 | deviceDownload(void* stream = nullptr, bool sync = true) { m_Buffer.deviceDownload(stream, sync); } 89 | }; 90 | 91 | template 92 | NodeWorkerHandle::NodeWorkerHandle(int maxSize) 93 | : m_Buffer(sizeof(NodeWorkerData) + (maxSize) * sizeof(NodeWorker::Node)) 94 | { 95 | NodeWorkerData data(maxSize); 96 | *reinterpret_cast(m_Buffer.data()) = data; 97 | this->nodeWorker()->clear(); // clear pixels or set background 98 | } 99 | 100 | inline void NodeWorker::clear() 101 | { 102 | NodeWorker::Node* ptr = &(*this)(0); 103 | int clearSize = NodeWorkerData::m_maxSize; 104 | for (auto* end = ptr + clearSize; ptr != end;){ 105 | *ptr++ = NodeWorker::Node(0,0); 106 | } 107 | } 108 | 109 | inline NodeWorker::Node& NodeWorker::operator()(int index) 110 | { 111 | assert(index >= 0 && index < NodeWorkerData::m_maxSize); 112 | return *(reinterpret_cast((uint8_t*)this + sizeof(NodeWorkerData)) + index); 113 | } 114 | 115 | inline NodeWorker::Node& NodeWorker::operator()(int x, int y, int z, int xDim, int yDim, int zDim) 116 | { 117 | assert(x >= 0 && x < xDim); 118 | assert(y >= 0 && y < yDim); 119 | assert(z >= 0 && z < zDim); 120 | return *(reinterpret_cast((uint8_t*)this + sizeof(NodeWorkerData)) + z + y * zDim + x * yDim * zDim); 121 | } 122 | 123 | } // namespace gpu 124 | } // namespace nanovdb 125 | 126 | #endif 127 | -------------------------------------------------------------------------------- /include/nanomap/gpu/PointCloud.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file PointCloud.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | /// @brief a class for managing pointcloud information for CUDA kernels. 9 | 10 | #ifndef NANOMAP_GPU_POINTCLOUD_H_HAS_BEEN_INCLUDED 11 | #define NANOMAP_GPU_POINTCLOUD_H_HAS_BEEN_INCLUDED 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace nanomap{ 21 | namespace gpu{ 22 | 23 | struct PointCloudData 24 | { 25 | int m_Size, m_maxSize; 26 | PointCloudData(int maxSize) 27 | :m_Size(maxSize) 28 | ,m_maxSize(maxSize) 29 | {} 30 | }; 31 | 32 | class PointCloud : private PointCloudData 33 | { 34 | using DataT = PointCloudData; 35 | 36 | public: 37 | struct Point 38 | { 39 | float x,y,z, norm, count; 40 | __hostdev__ Point(float _x, float _y, float _z, float _norm, float _count) 41 | : x(_x) 42 | , y(_y) 43 | , z(_z) 44 | , norm(_norm) 45 | , count(_count) 46 | { 47 | } 48 | }; 49 | 50 | __hostdev__ void clear(); 51 | __hostdev__ void updateData(int size, int point_step, unsigned char* data_pointer); 52 | __hostdev__ void updateSize(int size); 53 | __hostdev__ int maxSize() const {return DataT::m_maxSize;} 54 | __hostdev__ int size() const { return DataT::m_Size; } 55 | __hostdev__ inline Point& operator()(int index); 56 | }; // PointCloud 57 | 58 | template 59 | class PointCloudHandle 60 | { 61 | BufferT m_Buffer; 62 | 63 | public: 64 | PointCloudHandle(int maxSize); 65 | 66 | void updatePointCloudHandle(int size, int point_step, unsigned char* data_pointer); 67 | void updatePointCloudSize(int size); 68 | 69 | const PointCloud* pointCloud() const { return reinterpret_cast(m_Buffer.data()); } 70 | 71 | PointCloud* pointCloud() { return reinterpret_cast(m_Buffer.data()); } 72 | 73 | template 74 | typename std::enable_if::hasDeviceDual, const PointCloud*>::type 75 | devicePointCloud() const { return reinterpret_cast(m_Buffer.deviceData()); } 76 | 77 | template 78 | typename std::enable_if::hasDeviceDual, PointCloud*>::type 79 | devicePointCloud() { return reinterpret_cast(m_Buffer.deviceData()); } 80 | 81 | template 82 | typename std::enable_if::hasDeviceDual, void>::type 83 | deviceUpload(void* stream = nullptr, bool sync = true) { m_Buffer.deviceUpload(stream, sync); } 84 | 85 | template 86 | typename std::enable_if::hasDeviceDual, void>::type 87 | deviceDownload(void* stream = nullptr, bool sync = true) { m_Buffer.deviceDownload(stream, sync); } 88 | }; 89 | 90 | template 91 | PointCloudHandle::PointCloudHandle(int maxSize) 92 | : m_Buffer(sizeof(PointCloudData) + maxSize*sizeof(PointCloud::Point)) 93 | { 94 | PointCloudData data(maxSize); 95 | *reinterpret_cast(m_Buffer.data()) = data; 96 | this->pointCloud()->clear(); 97 | } 98 | 99 | template 100 | void PointCloudHandle::updatePointCloudHandle(int size, int point_step, unsigned char* data_pointer) 101 | { 102 | this->pointCloud()->updateData(size, point_step, data_pointer); 103 | 104 | } 105 | 106 | template 107 | void PointCloudHandle::updatePointCloudSize(int size) 108 | { 109 | this->pointCloud()->updateSize(size); 110 | } 111 | 112 | __hostdev__ inline void PointCloud::clear() 113 | { 114 | Point* ptr = &(*this)(0); 115 | for (auto* end = ptr + PointCloudData::m_Size; ptr != end;){ 116 | *ptr++ = Point(0.0, 0.0, 0.0, 0.0, 0.0); 117 | 118 | } 119 | } 120 | 121 | __hostdev__ inline void PointCloud::updateData(int size, int point_step, unsigned char* data_pointer) 122 | { 123 | PointCloudData::m_Size = size; 124 | int index = 0; 125 | Point* ptr = &(*this)(0); 126 | for(auto* end = ptr + PointCloudData::m_Size; ptr != end;){ 127 | unsigned char* byte_ptr = data_pointer + index*point_step; 128 | *ptr++ = Point(*(reinterpret_cast(byte_ptr+0)), 129 | *(reinterpret_cast(byte_ptr+4)), 130 | *(reinterpret_cast(byte_ptr+8)), 0.0, 0.0); 131 | index += 1; 132 | //printf("xyz: %f %f %f \n", *(reinterpret_cast(byte_ptr+0)), *(reinterpret_cast(byte_ptr+4)), *(reinterpret_cast(byte_ptr+8))); 133 | } 134 | } 135 | 136 | __hostdev__ inline void PointCloud::updateSize(int size) 137 | { 138 | PointCloudData::m_Size = size; 139 | } 140 | 141 | inline PointCloud::Point& PointCloud::operator()(int index) 142 | { 143 | assert(index < PointCloudData::m_Size); 144 | return *(reinterpret_cast((uint8_t*)this + sizeof(PointCloudData)) + index); 145 | } 146 | 147 | } // namespace gpu 148 | } // namespace nanovdb 149 | 150 | #endif 151 | -------------------------------------------------------------------------------- /include/nanomap/gpu/Sensor.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Sensor.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | /// @brief a sensor class for managing sensor information for CUDA kernels. 9 | 10 | #ifndef NANOMAP_GPU_SENSOR_H_HAS_BEEN_INCLUDED 11 | #define NANOMAP_GPU_SENSOR_H_HAS_BEEN_INCLUDED 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace nanomap{ 23 | namespace gpu{ 24 | 25 | //Contains all variables and functions to perform Frustum or Laser updates on GPU 26 | //What is performed depends on sensorData type acquired during update(). 27 | //combined class instead of inheritance is used to avoid virtual issues 28 | //when moving between CPU and GPU. 29 | template,typename RayT = nanovdb::Ray,typename EigenVec = Eigen::Matrix, typename EigenMat = Eigen::Matrix> 30 | class Sensor 31 | { 32 | 33 | public: 34 | 35 | __hostdev__ Sensor() 36 | : m_gridRes(0.0) 37 | , m_leafOriginOffset(0.0,0.0,0.0) 38 | , m_voxelOriginOffset(0.0,0.0,0.0) 39 | , m_leafOffset(0,0,0) 40 | , m_worldMin(0.0,0.0,0.0) 41 | , m_worldMax(0.0,0.0,0.0) 42 | , m_maxRange(0.0) 43 | , m_minRange(0.0) 44 | , m_minVoxelRange(0) 45 | , m_maxVoxelRange(0) 46 | , m_logOddsHit(0.0) 47 | , m_logOddsMiss(0.0) 48 | , m_vRes(0) 49 | , m_hRes(0) 50 | { 51 | } 52 | 53 | __host__ void updateLaser(nanomap::sensor::SharedParameters shared){ 54 | updateShared(shared); 55 | m_hFOV = shared._hfov; 56 | m_vFOV = shared._vfov; 57 | m_scale[0] = m_hFOV/m_hRes; 58 | m_scale[1] = m_vFOV/m_vRes; 59 | m_transformedViewVecs = m_rotation; 60 | } 61 | 62 | __host__ void updateFrustum(nanomap::sensor::SharedParameters shared){ 63 | updateShared(shared); 64 | float aspect = shared._aspect; 65 | float vfov = shared._vfov; 66 | float halfHeight = ValueT(tan(vfov * 3.14159265358979323846 / 360)); 67 | float halfWidth = aspect * halfHeight; 68 | m_scale[0] = (float)(1.0/m_hRes); 69 | m_scale[1] = (float)(1.0/m_vRes); 70 | m_transformedViewVecs = m_rotation; 71 | m_transformedViewVecs.col(2) = halfWidth * m_transformedViewVecs.col(0) + halfHeight * m_transformedViewVecs.col(1) - m_transformedViewVecs.col(2); 72 | m_transformedViewVecs.col(0) *= 2 * halfWidth; 73 | m_transformedViewVecs.col(1) *= 2 * halfHeight; 74 | m_identityVecs.col(2) = halfWidth * Eigen::Vector3f(1.0,0.0,0.0) + halfHeight * Eigen::Vector3f(0.0,1.0,0.0)-Eigen::Vector3f(0.0,0.0,1.0);; 75 | m_identityVecs.col(0) = Eigen::Vector3f(1.0,0.0,0.0) * 2 * halfWidth; 76 | m_identityVecs.col(1) = Eigen::Vector3f(0.0,1.0,0.0) * 2 * halfHeight; 77 | } 78 | __host__ void updateShared(nanomap::sensor::SharedParameters& shared) 79 | { 80 | m_defaultViewVecs = shared._frameTransform; 81 | m_gridRes = shared._gridRes; 82 | m_position = shared._pose.position; 83 | m_voxelPosition = shared._voxelPosition; 84 | m_leafOffset = shared._leafOffset; 85 | m_leafOriginOffset = shared._leafOriginOffset; 86 | m_voxelOriginOffset = shared._voxelOriginOffset; 87 | Eigen::Quaternionf q = shared._pose.orientation.normalized(); 88 | m_rotation = q.toRotationMatrix()*m_defaultViewVecs; 89 | m_logOddsHit = log(shared._probHit)-log(1-shared._probHit); 90 | m_logOddsMiss = log(shared._probMiss)-log(1-shared._probMiss); 91 | m_worldMin(0) = shared._worldMin(0); 92 | m_worldMin(1) = shared._worldMin(1); 93 | m_worldMin(2) = shared._worldMin(2); 94 | #pragma STDC FENV_ACCESS ON 95 | std:: fesetround(FE_DOWNWARD); 96 | m_voxelWorldMin(0) = std::nearbyint(shared._worldMin(0)/m_gridRes); 97 | m_voxelWorldMin(1) = std::nearbyint(shared._worldMin(1)/m_gridRes); 98 | m_voxelWorldMin(2) = std::nearbyint(shared._worldMin(2)/m_gridRes); 99 | m_worldMax(0) = shared._worldMax(0); 100 | m_worldMax(1) = shared._worldMax(1); 101 | m_worldMax(2) = shared._worldMax(2); 102 | m_voxelWorldMax(0) = std::nearbyint(shared._worldMax(0)/m_gridRes); 103 | m_voxelWorldMax(1) = std::nearbyint(shared._worldMax(1)/m_gridRes); 104 | m_voxelWorldMax(2) = std::nearbyint(shared._worldMax(2)/m_gridRes); 105 | m_voxelWorldDim(0) = m_voxelWorldMax(0)-m_voxelWorldMin(0); 106 | m_voxelWorldDim(1) = m_voxelWorldMax(1)-m_voxelWorldMin(1); 107 | m_voxelWorldDim(2) = m_voxelWorldMax(2)-m_voxelWorldMin(2); 108 | m_transformedNorms = shared._transformedNorms; 109 | m_vRes = shared._vRes; 110 | m_hRes = shared._hRes; 111 | m_minRange = shared._minRange; 112 | m_maxRange = shared._maxRange; 113 | m_minVoxelRange = shared._minVoxelRange; 114 | m_maxVoxelRange = shared._maxVoxelRange; 115 | m_type = shared._type; 116 | 117 | } 118 | 119 | __hostdev__ void getRay(int w, int h, Vec3T& rotatedRay, Vec3T& rayEye, Vec3T& rayDir, ValueT& minTime, ValueT& maxTime, const ValueT& gridRes) const 120 | { 121 | if(m_type == 0){ 122 | float u = (float)w*m_scale[0]; 123 | float v = (float)h*m_scale[1]; 124 | EigenVec matrix_dir = u * m_transformedViewVecs.col(0) + v * m_transformedViewVecs.col(1) - m_transformedViewVecs.col(2); 125 | EigenVec default_dir = u * m_identityVecs.col(0) + v * m_identityVecs.col(1) - m_identityVecs.col(2); 126 | ValueT norm = matrix_dir.norm(); 127 | default_dir.normalize(); 128 | matrix_dir.normalize(); 129 | minTime = (m_minRange/gridRes)*norm; 130 | maxTime = (m_maxRange/gridRes)*norm; 131 | rayEye = Vec3T(m_position(0)/gridRes,m_position(1)/gridRes,m_position(2)/gridRes); 132 | rayDir = Vec3T(matrix_dir(0),matrix_dir(1),matrix_dir(2)); 133 | rotatedRay = Vec3T(default_dir(0), default_dir(1), default_dir(2)); 134 | }else if(m_type==1){ 135 | float u = -m_hFOV/2+((float)w)*m_scale[0]; 136 | float v = -m_vFOV/2+((float)h)*m_scale[1]; 137 | u *= 3.14159265358979323846 / 180; 138 | v *= 3.14159265358979323846 / 180; 139 | //Calculate unit direction of ray in xyz frame 140 | EigenVec matrix_dir(std::sin(u)*std::cos(v), std::sin(v),std::cos(u)*std::cos(v)); 141 | //normalise ray 142 | matrix_dir.normalize(); 143 | EigenVec default_dir = matrix_dir; 144 | //rotate dir matrix to match sensor 145 | matrix_dir = EigenVec(m_rotation(0,0)*matrix_dir(0)+m_rotation(0,1)*matrix_dir(1)+m_rotation(0,2)*matrix_dir(2), 146 | m_rotation(1,0)*matrix_dir(0)+m_rotation(1,1)*matrix_dir(1)+m_rotation(1,2)*matrix_dir(2), 147 | m_rotation(2,0)*matrix_dir(0)+m_rotation(2,1)*matrix_dir(1)+m_rotation(2,2)*matrix_dir(2)); 148 | matrix_dir.normalize(); 149 | default_dir.normalize(); 150 | minTime = m_minRange/gridRes; 151 | maxTime = m_maxRange/gridRes; 152 | rayEye = Vec3T(m_position(0)/gridRes,m_position(1)/gridRes,m_position(2)/gridRes); 153 | rayDir = Vec3T(matrix_dir(0),matrix_dir(1),matrix_dir(2)); 154 | rotatedRay = Vec3T(default_dir(0), default_dir(1), default_dir(2)); 155 | } 156 | } 157 | 158 | __hostdev__ const int type() const{return m_type;} 159 | __hostdev__ const EigenMat rotation() const {return m_rotation;} 160 | __hostdev__ const EigenVec leafOriginOffset() const {return m_leafOriginOffset;} 161 | __hostdev__ const EigenVec voxelOriginOffset() const {return m_voxelOriginOffset;} 162 | __hostdev__ const EigenVec position() const {return m_position;} 163 | __hostdev__ const EigenVec voxelPosition() const {return m_voxelPosition;} 164 | __hostdev__ const Eigen::Matrix leafOffset() const {return m_leafOffset;} 165 | __hostdev__ const EigenVec worldMin() const {return m_worldMin;} 166 | __hostdev__ const EigenVec worldMax() const {return m_worldMax;} 167 | __hostdev__ const Eigen::Matrix voxelWorldMin() const {return m_voxelWorldMin;} 168 | __hostdev__ const Eigen::Matrix voxelWorldMax() const {return m_voxelWorldMax;} 169 | __hostdev__ const Eigen::Matrix voxelWorldDim() const {return m_voxelWorldDim;} 170 | 171 | __hostdev__ const ValueT gridRes() const {return m_gridRes;} 172 | 173 | __hostdev__ const ValueT maxRange() const {return m_maxRange;} 174 | __hostdev__ const ValueT minRange() const {return m_minRange;} 175 | __hostdev__ const ValueT maxVoxelRange() const {return m_maxVoxelRange;} 176 | __hostdev__ const ValueT minVoxelRange() const {return m_minVoxelRange;} 177 | 178 | __hostdev__ const int vRes() const { return m_vRes;} 179 | __hostdev__ const int hRes() const { return m_hRes;} 180 | 181 | __hostdev__ const ValueT clogOddsMiss() const {return m_logOddsMiss;} 182 | __hostdev__ const ValueT clogOddsHit() const {return m_logOddsHit;} 183 | __hostdev__ ValueT logOddsMiss() {return m_logOddsMiss;} 184 | __hostdev__ ValueT logOddsHit() {return m_logOddsHit;} 185 | protected: 186 | 187 | ValueT m_gridRes; 188 | int m_type; 189 | EigenMat m_rotation; 190 | EigenVec m_worldMin; 191 | Eigen::Matrix m_voxelWorldMin; 192 | EigenVec m_worldMax; 193 | Eigen::Matrix m_voxelWorldMax; 194 | Eigen::Matrix m_voxelWorldDim; 195 | EigenVec m_position; 196 | EigenVec m_voxelPosition; 197 | Eigen::Matrix m_leafOffset; 198 | EigenVec m_voxelOriginOffset; 199 | EigenVec m_leafOriginOffset; 200 | ValueT m_maxRange; 201 | ValueT m_minRange; 202 | ValueT m_maxVoxelRange; 203 | ValueT m_minVoxelRange; 204 | ValueT m_logOddsHit; 205 | ValueT m_logOddsMiss; 206 | int m_vRes; 207 | int m_hRes; 208 | float m_vFOV; 209 | float m_hFOV; 210 | float m_scale[2]; 211 | EigenMat m_defaultViewVecs; 212 | EigenMat m_transformedViewVecs; 213 | EigenMat m_identityVecs; 214 | Eigen::Matrix m_transformedNorms; 215 | }; // sensor 216 | } //namespace gpu 217 | } // namespace nanomap 218 | 219 | #endif 220 | -------------------------------------------------------------------------------- /include/nanomap/gpu/SensorBucket.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SenorBucket.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | 10 | #ifndef NANOMAP_HANDLER_SENSORBUCKET_H_INCLUDED 11 | #define NANOMAP_HANDLER_SENSORBUCKET_H_INCLUDED 12 | #include 13 | #include 14 | #include "nanomap/gpu/NodeWorker.h" 15 | #include "nanomap/gpu/PointCloud.h" 16 | #include "nanomap/gpu/Sensor.h" 17 | #include "nanomap/gpu/GridData.h" 18 | #include "nanomap/nanomap.h" 19 | #include "nanomap/handler/handlerAssert.h" 20 | #include "cuda_fp16.h" 21 | 22 | namespace nanomap{ 23 | namespace gpu{ 24 | using BufferT = nanovdb::CudaDeviceBuffer; 25 | using ValueT = float; 26 | using EigenVec = Eigen::Matrix; 27 | class SensorBucket 28 | { 29 | 30 | public: 31 | ~SensorBucket(){ 32 | freeMemory(); 33 | } 34 | 35 | SensorBucket(int maxPCLSize, int frustumLeafBufferSize) 36 | :_pclHandle(maxPCLSize), 37 | _leafHandle(frustumLeafBufferSize) 38 | { 39 | cudaCheck(cudaMalloc((void**)&_devSensor, sizeof(nanomap::gpu::Sensor))); 40 | cudaCheck(cudaMallocHost((void**)&_hostSensor, sizeof(nanomap::gpu::Sensor))); 41 | cudaCheck(cudaMalloc((void**)&_devGridData, sizeof(nanomap::gpu::GridData))); 42 | cudaCheck(cudaMallocHost((void**)&_hostGridData, sizeof(nanomap::gpu::GridData))); 43 | cudaCheck(cudaMalloc((void**)&_devFrustumLeafCount, sizeof(int))); 44 | cudaCheck(cudaMallocHost((void**)&_hostFrustumLeafCount, sizeof(int))); 45 | cudaCheck(cudaMalloc((void**)&_devRayCount, sizeof(int))); 46 | cudaCheck(cudaMallocHost((void**)&_hostRayCount, sizeof(int))); 47 | _leafHandle.deviceUpload(); 48 | _pclHandle.deviceUpload(); 49 | } 50 | 51 | void freeMemory(){ 52 | cudaFreeHost(_hostSensor); 53 | cudaFree(_devSensor); 54 | cudaFreeHost(_hostGridData); 55 | cudaFree(_devGridData); 56 | cudaFreeHost(_hostRayCount); 57 | cudaFree(_devRayCount); 58 | if(_processType == 0 || _processType == 2){ 59 | cudaFree(_activeLeafNodes); 60 | cudaFree(_activeFlags); 61 | cudaFree(_activeIndices); 62 | cudaFree(_devFrustumLeafCount); 63 | cudaFreeHost(_hostFrustumLeafCount); 64 | cudaFree(_devFrustumVoxelWorker); 65 | cudaFree(_devFrustumVoxelBuffer); 66 | cudaFreeHost(_hostFrustumVoxelBuffer); 67 | cudaFree(_devFrustumLeafBuffer); 68 | cudaFreeHost(_hostFrustumLeafBuffer); 69 | } 70 | 71 | if(_filterType == 1){ 72 | if(_precisionType == 0){ 73 | cudaFree(_voxelFilterBuffer); 74 | } 75 | if(_precisionType == 1){ 76 | cudaFree(_voxelFilterBufferHalf2); 77 | } 78 | if(_precisionType == 2){ 79 | cudaFree(_voxelFilterBufferSimple); 80 | } 81 | if(_precisionType == 3){ 82 | cudaFree(_voxelFilterBufferSimpleQuad); 83 | } 84 | } 85 | } 86 | int getProcessType(){return _processType;} 87 | int getFilterType(){return _filterType;} 88 | int getPrecisionType(){return _precisionType;} 89 | int getFrustumVoxelAllocation(){return _frustumVoxelAllocation;} 90 | float getGridRes(){return _gridRes;} 91 | float getMappingRes(){return _mappingRes;} 92 | void setProcessType(int processType){_processType = processType;} 93 | void setFilterType(int filterType){_filterType = filterType;} 94 | void setPrecisionType(int precisionType){_precisionType = precisionType;} 95 | void setFrustumVoxelAllocation(int frustumVoxelAllocation){_frustumVoxelAllocation = frustumVoxelAllocation;} 96 | void setGridRes(float gridRes){_gridRes = gridRes;} 97 | void setMappingRes(float mappingRes){_mappingRes = mappingRes;} 98 | 99 | nanomap::gpu::PointCloudHandle& pclHandle(){return _pclHandle;} 100 | nanomap::gpu::NodeWorkerHandle& leafHandle(){return _leafHandle;} 101 | nanomap::gpu::Sensor* devSensor(){return _devSensor;} 102 | nanomap::gpu::Sensor* hostSensor(){return _hostSensor;} 103 | nanomap::gpu::GridData* devGridData(){return _devGridData;} 104 | nanomap::gpu::GridData* hostGridData(){return _hostGridData;} 105 | 106 | int* devFrustumLeafCount(){return _devFrustumLeafCount;} 107 | int* hostFrustumLeafCount(){return _hostFrustumLeafCount;} 108 | int* devFrustumLeafBuffer(){return _devFrustumLeafBuffer;} 109 | int* hostFrustumLeafBuffer(){return _hostFrustumLeafBuffer;} 110 | float* devFrustumVoxelWorker(){return _devFrustumVoxelWorker;} 111 | int8_t* devFrustumVoxelBuffer(){return _devFrustumVoxelBuffer;} 112 | int8_t* hostFrustumVoxelBuffer(){return _hostFrustumVoxelBuffer;} 113 | 114 | int* activeLeafNodes(){return _activeLeafNodes;} 115 | int* activeIndices(){return _activeIndices;} 116 | int* activeFlags(){return _activeFlags;} 117 | 118 | //Ray Count Variable for Filtering 119 | int* devRayCount(){return _devRayCount;} 120 | int* hostRayCount(){return _hostRayCount;} 121 | //Voxelized Filter Specific Buffers 122 | float* voxelFilterBuffer(){return _voxelFilterBuffer;} 123 | __hostdev__ __half2* voxelFilterBufferHalf2(){return _voxelFilterBufferHalf2;} 124 | int* voxelFilterBufferSimple(){return _voxelFilterBufferSimple;} 125 | int* voxelFilterBufferSimpleQuad(){return _voxelFilterBufferSimpleQuad;} 126 | //private: 127 | /******************************************************************************/ 128 | //DEFAULT ALLOCATIONS 129 | int _filterType; 130 | int _processType; 131 | int _precisionType; 132 | float _mappingRes; 133 | float _gridRes; 134 | //Tracks current frustum and laser allocations. 135 | int _frustumVoxelAllocation; 136 | int _laserVoxelAllocation; 137 | 138 | //This is the container for the point cloud. This is populated either via a an external point cloud or a simulated one. 139 | //Can be used to represent a point in world space, or a unit direction and magnitude. This allows the same container to be reused when converting 140 | //From the world space point cloud to grid space rays. 141 | nanomap::gpu::PointCloudHandle _pclHandle; 142 | //The node handle is used when performing the node raycasting step of the GPU calculations. It tracks a dense representation of the observable node space. 143 | nanomap::gpu::NodeWorkerHandle _leafHandle; 144 | //Pointers for device and host sensor object. One object is used per manager, 145 | //and is updated as necessary prior to use with different senserData objects 146 | nanomap::gpu::Sensor* _devSensor; 147 | nanomap::gpu::Sensor* _hostSensor; 148 | //Pointer to GridData Object. This tracks important data necessary for GPU calculations. 149 | nanomap::gpu::GridData* _devGridData; 150 | nanomap::gpu::GridData* _hostGridData; 151 | //This variable tracks the active leafNode count. 152 | int* _devFrustumLeafCount; 153 | int* _hostFrustumLeafCount; 154 | //contains the index coordinates of all active leaf nodess with Z being the fastest moving axis. 155 | int* _devFrustumLeafBuffer; 156 | int* _hostFrustumLeafBuffer; 157 | float* _devFrustumVoxelWorker; 158 | 159 | //Used to transfer probability info from gpu to cpu in compact fashion 160 | int8_t* _devFrustumVoxelBuffer; 161 | int8_t* _hostFrustumVoxelBuffer; 162 | 163 | //Used for sorting active node information to improve CPU side grid access. 164 | int* _activeLeafNodes; 165 | int* _activeIndices; 166 | int* _activeFlags; 167 | 168 | /******************************************************************************/ 169 | //FILTER SPECIFIC ALLOCATIONS 170 | //A dense representation of the observable voxel level environment 171 | //Used for bucketing points for voxel filtering. 172 | float* _voxelFilterBuffer; 173 | __half2* _voxelFilterBufferHalf2; 174 | int* _voxelFilterBufferSimple; 175 | int* _voxelFilterBufferSimpleQuad; 176 | //This variable tracks the number of pcl rays after filtering. 177 | int* _devRayCount; 178 | int* _hostRayCount; 179 | }; 180 | 181 | 182 | } 183 | } 184 | #endif 185 | -------------------------------------------------------------------------------- /include/nanomap/handler/BlockWorker.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file BlockWorker.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #ifndef NANOMAP_HANDLER_BLOCKWORKER_H_INCLUDED 10 | #define NANOMAP_HANDLER_BLOCKWORKER_H_INCLUDED 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include // for nanovdb::Ray 30 | #include 31 | 32 | #include "nanomap/gpu/NodeWorker.h" 33 | #include "nanomap/gpu/PointCloud.h" 34 | #include "nanomap/gpu/Sensor.h" 35 | #include "nanomap/gpu/GridData.h" 36 | #include "nanomap/sensor/SensorData.h" 37 | #include "nanomap/map/OccupancyMap.h" 38 | #include "nanomap/nanomap.h" 39 | 40 | namespace nanomap{ 41 | namespace handler{ 42 | using Map = nanomap::map::Map; 43 | using FloatGrid = openvdb::FloatGrid; 44 | 45 | using TreeT = openvdb::FloatGrid::TreeType; 46 | using LeafT = TreeT::LeafNodeType; 47 | using AccessorT = openvdb::tree::ValueAccessor; 48 | using VecTreeT = openvdb::Vec3DGrid::TreeType; 49 | using VecAccessorT = openvdb::tree::ValueAccessor; 50 | using BufferT = nanovdb::CudaDeviceBuffer; 51 | using ValueT = float; 52 | using Vec3T = nanovdb::Vec3; 53 | using RayT = nanovdb::Ray; 54 | using HDDA = nanovdb::HDDA; 55 | using SensorData = nanomap::sensor::SensorData; 56 | using IterType = TreeT::ValueOnIter; 57 | 58 | using EigenVec = Eigen::Matrix; 59 | using EigenMat = Eigen::Matrix; 60 | using Quaternion = Eigen::Quaternion; 61 | 62 | class BlockWorker 63 | { 64 | 65 | public: 66 | struct Block { 67 | openvdb::CoordBBox bbox; 68 | LeafT* leaf; 69 | std::pair node; 70 | Block(const openvdb::CoordBBox& b) : bbox(b), leaf(nullptr) {} 71 | }; 72 | std::vector* _blocks; 73 | std::vector* _leaves; 74 | 75 | BlockWorker(int nodeEdge, float occClampThres, float emptyClampThres, float logOddsHitThres, 76 | float logOddsMissThres, std::shared_ptr accessor, int8_t* hostVoxelBuffer, 77 | int* hostNodeBuffer, int hostCount); 78 | void destroyBlocks(); 79 | void processBlocks(bool serial); 80 | void operator()(const tbb::blocked_range &r) const; 81 | 82 | 83 | private: 84 | 85 | int _voxelVolume; 86 | int _nodeEdge; 87 | float _occupiedClampingThreshold; 88 | float _emptyClampingThreshold; 89 | float _logOddsHitThreshold; 90 | float _logOddsMissThreshold; 91 | 92 | int8_t* _hostVoxelBuffer; 93 | 94 | void fillOccupiedLeafFromBuffer(LeafT* leaf, int index) const; 95 | 96 | void combineOccupiedLeafFromBuffer(LeafT* leaf, 97 | LeafT* target, 98 | int index) const; 99 | 100 | 101 | }; 102 | 103 | } 104 | } 105 | #endif 106 | -------------------------------------------------------------------------------- /include/nanomap/handler/Handler.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Handler.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #ifndef NANOMAP_HANDLER_HANDLER_H_INCLUDED 10 | #define NANOMAP_HANDLER_HANDLER_H_INCLUDED 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include // for nanovdb::Ray 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include "nanomap/gpu/NodeWorker.h" 32 | #include "nanomap/gpu/PointCloud.h" 33 | #include "nanomap/gpu/Sensor.h" 34 | #include "nanomap/gpu/GridData.h" 35 | #include "nanomap/sensor/SensorData.h" 36 | #include "nanomap/map/OccupancyMap.h" 37 | #include "nanomap/nanomap.h" 38 | #include "nanomap/gpu/SensorBucket.h" 39 | #include "nanomap/allocator/SensorAllocator.h" 40 | #include "nanomap/handler/BlockWorker.h" 41 | #include "nanomap/config/Config.h" 42 | 43 | // The following functions are called by the host and launch the gpu kernels. 44 | 45 | //This kernel filters the cloud. Filter functionality depends on user defined settings provided at runtime 46 | extern "C" void filterCloud(nanomap::gpu::SensorBucket& _sensorBucket, cudaStream_t s0); 47 | 48 | //This kernel raycasts the cloud over the area that is observable by the sensor in question 49 | extern "C" void frustumCastCloud(nanomap::gpu::SensorBucket& _sensorBucket, cudaStream_t s0, cudaStream_t s1); 50 | 51 | 52 | 53 | namespace nanomap{ 54 | namespace handler{ 55 | using Map = nanomap::map::Map; 56 | using FloatGrid = openvdb::FloatGrid; 57 | 58 | using TreeT = openvdb::FloatGrid::TreeType; 59 | using LeafT = TreeT::LeafNodeType; 60 | using AccessorT = openvdb::tree::ValueAccessor; 61 | using VecTreeT = openvdb::Vec3DGrid::TreeType; 62 | using VecAccessorT = openvdb::tree::ValueAccessor; 63 | using BufferT = nanovdb::CudaDeviceBuffer; 64 | using ValueT = float; 65 | using Vec3T = nanovdb::Vec3; 66 | using RayT = nanovdb::Ray; 67 | using HDDA = nanovdb::HDDA; 68 | using SensorData = nanomap::sensor::SensorData; 69 | using IterType = TreeT::ValueOnIter; 70 | 71 | using EigenVec = Eigen::Matrix; 72 | using EigenMat = Eigen::Matrix; 73 | using Quaternion = Eigen::Quaternion; 74 | 75 | class Handler{ 76 | public: 77 | Handler(std::shared_ptr config); 78 | 79 | void populateTempGrid(openvdb::FloatGrid::Accessor& tempAcc, int sensorIndex, int pcl_width, int pcl_height, int pcl_step, 80 | unsigned char* cloudPtr, std::shared_ptr map); 81 | 82 | void integrateTempGrid(openvdb::FloatGrid::Ptr tempGrid, std::shared_ptr map); 83 | void processPointCloudCPU(int index, int pcl_width, int pcl_height, int pcl_step, unsigned char* cloud, std::shared_ptr map); 84 | void voxelUpdateFromFrustumBuffer(int nodeEdge); 85 | void blockUpdateFromFrustumBuffer(); 86 | void integrateTempGrid(openvdb::FloatGrid::Ptr tempGrid, 87 | openvdb::FloatGrid::Ptr Grid, 88 | openvdb::FloatGrid::Accessor& acc, 89 | float emptyClampThres, 90 | float occClampThres, 91 | float logodds_thres_min, 92 | float logodds_thres_max); 93 | void processPointCloud(int sensorIndex, int pcl_width, int pcl_height, 94 | int pcl_step, unsigned char* cloud, 95 | std::shared_ptr agentMap); 96 | void closeHandler(); 97 | void printUpdateTime(int count); 98 | 99 | private: 100 | cudaStream_t _s0; 101 | cudaStream_t _s1; 102 | float _mapUpdateTime; 103 | float _gpuTime; 104 | std::shared_ptr _config; 105 | std::shared_ptr _agentMap; 106 | nanomap::allocator::SensorAllocator _sensorAllocator; 107 | nanovdb::GridHandle _laserHandle; 108 | }; 109 | 110 | 111 | } 112 | } 113 | #endif 114 | -------------------------------------------------------------------------------- /include/nanomap/handler/SimHandler.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SimHandler.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_HANDLER_SIMHANDLER_H_INCLUDED 9 | #define NANOMAP_HANDLER_SIMHANDLER_H_INCLUDED 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "nanomap/gpu/NodeWorker.h" 34 | #include "nanomap/gpu/PointCloud.h" 35 | #include "nanomap/gpu/Sensor.h" 36 | #include "nanomap/gpu/GridData.h" 37 | #include "nanomap/sensor/SensorData.h" 38 | #include "nanomap/map/OccupancyMap.h" 39 | #include "nanomap/nanomap.h" 40 | #include "nanomap/gpu/SensorBucket.h" 41 | #include "nanomap/allocator/SensorAllocator.h" 42 | #include "nanomap/handler/BlockWorker.h" 43 | #include "nanomap/config/Config.h" 44 | 45 | // The following functions are called by the host and launch the gpu kernels. 46 | 47 | //This kernel generates a point cloud using a simulated sensor view and an existing map 48 | extern "C" void generateCloud(nanomap::gpu::SensorBucket& _sensorBucket, nanovdb::GridHandle& _simGridHandle, cudaStream_t s0); 49 | 50 | //This kernel filters the cloud. Filter functionality depends on user defined settings provided at runtime 51 | extern "C" void filterCloud(nanomap::gpu::SensorBucket& _sensorBucket, cudaStream_t s0); 52 | 53 | //This kernel raycasts the cloud over the area that is observable by the sensor in question 54 | extern "C" void frustumCastCloud(nanomap::gpu::SensorBucket& _sensorBucket, cudaStream_t s0, cudaStream_t s1); 55 | 56 | 57 | namespace nanomap{ 58 | namespace handler{ 59 | using Map = nanomap::map::Map; 60 | using FloatGrid = openvdb::FloatGrid; 61 | 62 | using TreeT = openvdb::FloatGrid::TreeType; 63 | using LeafT = TreeT::LeafNodeType; 64 | using AccessorT = openvdb::tree::ValueAccessor; 65 | using VecTreeT = openvdb::Vec3DGrid::TreeType; 66 | using VecAccessorT = openvdb::tree::ValueAccessor; 67 | using BufferT = nanovdb::CudaDeviceBuffer; 68 | using ValueT = float; 69 | using Vec3T = nanovdb::Vec3; 70 | using RayT = nanovdb::Ray; 71 | using HDDA = nanovdb::HDDA; 72 | using SensorData = nanomap::sensor::SensorData; 73 | using IterType = TreeT::ValueOnIter; 74 | 75 | using EigenVec = Eigen::Matrix; 76 | using EigenMat = Eigen::Matrix; 77 | using Quaternion = Eigen::Quaternion; 78 | 79 | class SimHandler{ 80 | public: 81 | SimHandler(std::shared_ptr config, openvdb::FloatGrid::Ptr simGrid); 82 | void populateTempGrid(openvdb::FloatGrid::Accessor& tempAcc, int sensorIndex, std::shared_ptr map); 83 | 84 | void integrateTempGrid(openvdb::FloatGrid::Ptr tempGrid, std::shared_ptr map); 85 | void processPointCloudCPU(int index, std::shared_ptr map); 86 | void voxelUpdateFromFrustumBuffer(int nodeEdge); 87 | void blockUpdateFromFrustumBuffer(); 88 | void integrateTempGrid(openvdb::FloatGrid::Ptr tempGrid, 89 | openvdb::FloatGrid::Ptr Grid, 90 | openvdb::FloatGrid::Accessor& acc, 91 | float emptyClampThres, 92 | float occClampThres, 93 | float logodds_thres_min, 94 | float logodds_thres_max); 95 | void processPointCloud(int sensorIndex, 96 | std::shared_ptr agentMap); 97 | void closeHandler(); 98 | void printUpdateTime(int count); 99 | 100 | private: 101 | cudaStream_t _s0; 102 | cudaStream_t _s1; 103 | float _mapUpdateTime; 104 | float _gpuTime; 105 | std::shared_ptr _agentMap; 106 | std::shared_ptr _config; 107 | openvdb::FloatGrid::Ptr _simGrid; 108 | nanomap::allocator::SensorAllocator _sensorAllocator; 109 | nanovdb::GridHandle _laserHandle; 110 | nanovdb::GridHandle _simGridHandle; 111 | 112 | 113 | }; 114 | 115 | 116 | } 117 | } 118 | #endif 119 | -------------------------------------------------------------------------------- /include/nanomap/handler/handlerAssert.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file handlerAssert.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_HANDLER_HANDLERASSERT_H_INCLUDED 9 | #define NANOMAP_HANDLER_HANDLERASSERT_H_INCLUDED 10 | 11 | #include 12 | 13 | 14 | #define cudaCheck(ans) \ 15 | { \ 16 | gpuAssert((ans), __FILE__, __LINE__); \ 17 | } 18 | 19 | static inline bool gpuAssert(cudaError_t code, const char* file, int line, bool abort = true) 20 | { 21 | #if defined(DEBUG) || defined(_DEBUG) 22 | if (code != cudaSuccess) { 23 | fprintf(stderr, "CUDA Runtime Error: %s %s %d\n", cudaGetErrorString(code), file, line); 24 | if (abort) 25 | exit(code); 26 | return false; 27 | } 28 | #endif 29 | return true; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /include/nanomap/manager/Manager.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Manager.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_MANAGER_MANAGER_H_INCLUDED 9 | #define NANOMAP_MANAGER_MANAGER_H_INCLUDED 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "nanomap/sensor/SensorData.h" 16 | #include "nanomap/map/OccupancyMap.h" 17 | #include "nanomap/handler/Handler.h" 18 | #include "nanomap/nanomap.h" 19 | #include "nanomap/config/Config.h" 20 | 21 | /****************************************************************************** 22 | 23 | *******************************************************************************/ 24 | 25 | namespace nanomap{ 26 | namespace manager{ 27 | 28 | 29 | 30 | 31 | using Map = nanomap::map::Map; 32 | 33 | using Handler = nanomap::handler::Handler; 34 | using ValueT = float; 35 | using Vec3T = nanovdb::Vec3; 36 | 37 | using EigenVec = Eigen::Matrix; 38 | using EigenMat = Eigen::Matrix; 39 | using Quaternion = Eigen::Quaternion; 40 | using SensorData = nanomap::sensor::SensorData; 41 | 42 | 43 | class Manager{ 44 | 45 | public: 46 | Manager(std::shared_ptr config) 47 | :_config(config) 48 | { 49 | _handler = std::make_unique(_config); 50 | } 51 | 52 | void insertPointCloud(std::string sensorName, int pcl_width, int pcl_height, int pcl_step, unsigned char* cloud, 53 | const nanomap::Pose& pose, std::shared_ptr map){ 54 | 55 | for(auto itr = _config->sensorData().begin(); itr != _config->sensorData().end(); itr++){ 56 | if((*itr)->sensorName().compare(sensorName)==0){ 57 | int index = std::distance(_config->sensorData().begin(), itr); 58 | (*itr)->updatePose(pose); 59 | (*itr)->rotateView(); 60 | _handler->processPointCloud(index, pcl_width, pcl_height, pcl_step, cloud, map); 61 | } 62 | } 63 | } 64 | 65 | void printUpdateTime(int count){ 66 | _handler->printUpdateTime(count); 67 | } 68 | 69 | void closeHandler(){ 70 | _handler->closeHandler(); 71 | _handler.reset(); 72 | } 73 | std::shared_ptr config(){return _config;} 74 | 75 | 76 | 77 | 78 | 79 | protected: 80 | 81 | std::unique_ptr _handler; 82 | 83 | std::shared_ptr _config; 84 | 85 | }; 86 | } 87 | } 88 | #endif 89 | -------------------------------------------------------------------------------- /include/nanomap/manager/SimManager.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SimManager.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_MANAGER_SIMMANAGER_H_INCLUDED 9 | #define NANOMAP_MANAGER_SIMMANAGER_H_INCLUDED 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include // converter from OpenVDB to NanoVDB (includes NanoVDB.h and GridManager.h) 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "nanomap/handler/SimHandler.h" 28 | #include "nanomap/agent/Agent.h" 29 | #include "nanomap/sensor/FrustumData.h" 30 | #include "nanomap/sensor/SensorData.h" 31 | #include "nanomap/map/OccupancyMap.h" 32 | #include "nanomap/nanomap.h" 33 | #include "nanomap/config/Config.h" 34 | 35 | namespace nanomap{ 36 | namespace manager{ 37 | 38 | using Map = nanomap::map::Map; 39 | using Agent = nanomap::agent::Agent; 40 | using Map = nanomap::map::Map; 41 | using FloatGrid = openvdb::FloatGrid; 42 | 43 | using GridType = openvdb::FloatGrid; 44 | using TreeT = GridType::TreeType; 45 | using RootType = TreeT::RootNodeType; // level 3 RootNode 46 | using Int1Type = RootType::ChildNodeType; // level 2 InternalNode 47 | using Int2Type = Int1Type::ChildNodeType; // level 1 InternalNode 48 | using LeafType = TreeT::LeafNodeType; // level 0 LeafNode 49 | using IterType = TreeT::ValueOnIter; 50 | using LeafT = TreeT::LeafNodeType; 51 | using AccessorT = openvdb::tree::ValueAccessor; 52 | using VecTreeT = openvdb::Vec3DGrid::TreeType; 53 | using VecAccessorT = openvdb::tree::ValueAccessor; 54 | using BufferT = nanovdb::CudaDeviceBuffer; 55 | using ValueT = float; 56 | using Vec3T = nanovdb::Vec3; 57 | using RayT = nanovdb::Ray; 58 | using HDDA = nanovdb::HDDA; 59 | using IterType = TreeT::ValueOnIter; 60 | using EigenVec = Eigen::Matrix; 61 | using EigenMat = Eigen::Matrix; 62 | using Quaternion = Eigen::Quaternion; 63 | using SensorData = nanomap::sensor::SensorData; 64 | 65 | class SimManager{ 66 | 67 | public: 68 | SimManager(std::shared_ptr config) 69 | :_config(config){ 70 | 71 | loadSimGrid(); 72 | _config->loadAgents(_simGrid); 73 | for(int i = 0; i<_config->agentData().size(); i++){ 74 | if(_config->agentData(i)->spawnRandom()){ 75 | _config->agentData(i)->updatePose(getRandomSpawn()); 76 | } 77 | } 78 | _handler = std::make_unique(_config, _simGrid); 79 | std::cout << "_simHandler instantiated" << std::endl; 80 | } 81 | 82 | //Given a global pose, set agent pose to new pose. 83 | void updateAgentPoses(std::vector poses){ 84 | int index = 0; 85 | for(auto itr = poses.begin(); itr != poses.end(); itr++){ 86 | index = std::distance(poses.begin(), itr); 87 | _config->agentData(index)->updatePose(*itr); 88 | } 89 | } 90 | 91 | std::vector agentPoses(){ 92 | std::vector poses; 93 | for(auto itr = _config->agentData().begin(); itr != _config->agentData().end(); itr++){ 94 | poses.push_back((*itr)->pose()); 95 | } 96 | return poses; 97 | } 98 | //For each agent, for each sensor, upload relevant info to GPU. 99 | //Then generate and process simulated point clouds into map. 100 | void updateAgentViews(){ 101 | //For each agent; 102 | for(int i = 0; i<_config->agentData().size(); i++){ 103 | for(int j = 0; j<(_config->agentData(i)->sensorNames().size()); j++){ 104 | for(int k = 0; k < _config->sensorData().size() ; k++){ 105 | if(_config->sensorData(k)->sensorName().compare(_config->agentData(i)->sensorNames()[j])==0){ 106 | _config->sensorData(k)->updatePose(_config->agentData(i)->sensorPose(j)); 107 | _config->sensorData(k)->rotateView(); 108 | _handler->processPointCloud(k, _config->agentData(i)->map()); 109 | } 110 | } 111 | } 112 | } 113 | } 114 | void updateAgentViews(std::shared_ptr map_ptr){ 115 | //For each agent; 116 | for(int i = 0; i<_config->agentData().size(); i++){ 117 | for(int j = 0; j<(_config->agentData(i)->sensorNames().size()); j++){ 118 | for(int k = 0; k < _config->sensorData().size() ; k++){ 119 | if(_config->sensorData(k)->sensorName().compare(_config->agentData(i)->sensorNames()[j])==0){ 120 | _config->sensorData(k)->updatePose(_config->agentData(i)->sensorPose(j)); 121 | _config->sensorData(k)->rotateView(); 122 | _handler->processPointCloud(k, map_ptr); 123 | } 124 | } 125 | } 126 | } 127 | } 128 | 129 | void closeHandler(){ 130 | _handler->closeHandler(); 131 | } 132 | 133 | std::shared_ptr getMap(int index){ 134 | return _config->agentData(index)->map(); 135 | } 136 | 137 | void step(){}; 138 | 139 | void updateObservations(){}; 140 | 141 | bool getAgentSimCollision(int index, Eigen::Vector3f positionDelta){ 142 | Eigen::Vector3f newEye = _config->agentData(index)->pose().position + positionDelta; 143 | openvdb::Vec3d vecEye(newEye(0)/_config->gridRes(), newEye(1)/_config->gridRes(), newEye(2)/_config->gridRes()); 144 | openvdb::Vec3d vecDir; 145 | 146 | double t0=0; 147 | double t1=0; 148 | openvdb::tools::VolumeRayIntersector intersector(*_simGrid); 149 | for(int i=0; i<_config->agentData(index)->observationRays().cols(); i++){ 150 | vecDir = openvdb::Vec3f(_config->agentData(index)->observationRays().col(i)(0), 151 | _config->agentData(index)->observationRays().col(i)(1), 152 | _config->agentData(index)->observationRays().col(i)(2)); 153 | openvdb::math::Ray simRay(vecEye, vecDir); 154 | t0 = 0; 155 | t1 = 0; 156 | if(intersector.setIndexRay(simRay)){ 157 | if(intersector.march(t0,t1)){ 158 | if(t1<_config->agentData(index)->observationNorms().col(i)(0)){ 159 | std::cout << "time: " << t1<< std::endl; 160 | std::cout << _config->agentData(index)->observationNorms().col(i)(0) << std::endl; 161 | return false; 162 | } 163 | } 164 | 165 | } 166 | } 167 | return true; 168 | } 169 | 170 | 171 | void loadSimGrid(){ 172 | std::cout <<"reading in map config file: " << _config->config() << std::endl; 173 | std::ifstream *input = new std::ifstream(_config->config().c_str(), std::ios::in | std::ios::binary); 174 | bool end = false; 175 | std::string file, line; 176 | *input >> line; 177 | float res; 178 | if(line.compare("#config") != 0){ 179 | std::cout << "Error: first line reads [" << line << "] instead of [#config]" << std::endl; 180 | delete input; 181 | return; 182 | } 183 | while(input->good()) { 184 | *input >> line; 185 | if (line.compare("SimGrid:") == 0){ 186 | *input >> file; 187 | _simGrid = loadGridFromFile(file); 188 | }else if (line.compare("#endconfig")==0){ 189 | break; 190 | } 191 | } 192 | input->close(); 193 | } 194 | 195 | 196 | openvdb::FloatGrid::Ptr loadGridFromFile(std::string fileIn){ 197 | openvdb::io::File file(fileIn); 198 | // Open the file. This reads the file header, but not any grids. 199 | file.open(); 200 | openvdb::GridBase::Ptr baseGrid; 201 | for (openvdb::io::File::NameIterator nameIter = file.beginName(); 202 | nameIter != file.endName(); ++nameIter) 203 | { 204 | // Read in only the grid we are interested in. 205 | if (nameIter.gridName() == "grid") { 206 | baseGrid = file.readGrid(nameIter.gridName()); 207 | } else { 208 | std::cout << "skipping grid " << nameIter.gridName() << std::endl; 209 | } 210 | } 211 | file.close(); 212 | return openvdb::gridPtrCast(baseGrid); 213 | } 214 | Pose getRandomSpawn(){ 215 | Pose pose; 216 | Eigen::Matrix rotation; 217 | //Right 218 | rotation.col(0)=Eigen::Matrix(0.0,-1.0,0.0); 219 | //Forward 220 | rotation.col(1)=Eigen::Matrix(1.0,0.0,0.0); 221 | //Up 222 | rotation.col(2)=Eigen::Matrix(0.0,0.0,1.0); 223 | pose.orientation = Eigen::Quaternionf(rotation); 224 | 225 | auto bbox = _simGrid->evalActiveVoxelBoundingBox(); 226 | openvdb::Coord min = bbox.min(); 227 | openvdb::Coord max = bbox.max(); 228 | openvdb::Vec3f center = bbox.getCenter(); 229 | int x_range = max.x()-min.x(); 230 | int y_range = max.y()-min.y(); 231 | int z_range = max.z()-min.z(); 232 | srand(time(0)); 233 | auto acc = _simGrid->getAccessor(); 234 | int x = rand()%x_range+min.x(); 235 | int y = rand()%y_range+min.y(); 236 | int z = rand()%z_range+min.z(); 237 | openvdb::Coord coord_spawn(x,y,z); 238 | openvdb::Coord coord_value; 239 | bool close_to_wall; 240 | float wall_distance; 241 | float length = 10; 242 | bool finished = false; 243 | while(!finished){ 244 | x = (int)(std::floor(rand()%x_range+min.x())); 245 | x = (int)(std::floor(rand()%y_range+min.y())); 246 | x = (int)(std::floor(rand()%z_range+min.z())); 247 | coord_spawn = openvdb::Coord(x,y,z); 248 | 249 | TreeT::NodeIter iter = _simGrid->tree().beginNode(); 250 | iter.setMaxDepth(TreeT::NodeIter::LEAF_DEPTH-1); 251 | for (iter; iter; ++iter){ 252 | switch (iter.getDepth()){ 253 | case 1: { 254 | Int1Type* node = nullptr; 255 | iter.getNode(node); 256 | if (node){ 257 | } 258 | break; 259 | } 260 | case 2: { 261 | Int2Type* node = nullptr; 262 | iter.getNode(node); 263 | if (node && !finished){ 264 | if(acc.getValue(iter.getCoord())<=-0.3){ 265 | coord_value = iter.getCoord()+openvdb::Coord(4,4,4); 266 | length = (coord_value-coord_spawn).asVec3s().length(); 267 | if(length<=(5/_config->gridRes())){ 268 | finished = true; 269 | break; 270 | } 271 | } 272 | } 273 | break; 274 | } 275 | case 3: { 276 | LeafType* node = nullptr; 277 | iter.getNode(node); 278 | if (node){ 279 | 280 | } 281 | break; 282 | } 283 | } 284 | } 285 | } 286 | pose.position = Eigen::Matrix(coord_spawn.asVec3d().x()*_config->gridRes(), 287 | coord_spawn.asVec3d().y()*_config->gridRes(), 288 | coord_spawn.asVec3d().z()*_config->gridRes()); 289 | return pose; 290 | } 291 | 292 | 293 | protected: 294 | 295 | std::unique_ptr _handler; 296 | 297 | std::shared_ptr _config; 298 | 299 | openvdb::FloatGrid::Ptr _simGrid; 300 | }; 301 | } 302 | } 303 | #endif 304 | -------------------------------------------------------------------------------- /include/nanomap/map/OccupancyMap.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file OccupancyMap.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_MAP_MAP_H_INCLUDED 9 | #define NANOMAP_MAP_MAP_H_INCLUDED 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | namespace nanomap{ 18 | namespace map{ 19 | 20 | 21 | //Base Map Class, contains a single grid. 22 | class Map{ 23 | using FloatGrid = openvdb::FloatGrid; 24 | using FloatTreeT = openvdb::FloatGrid::TreeType; 25 | using FloatLeafT = FloatTreeT::LeafNodeType; 26 | using FloatAccessorT = openvdb::tree::ValueAccessor; 27 | 28 | using IntGrid = openvdb::Int32Grid; 29 | using IntTreeT = IntGrid::TreeType; 30 | using IntLeafT = IntTreeT::LeafNodeType; 31 | using IntAccessorT = openvdb::tree::ValueAccessor; 32 | public: 33 | Map(){} 34 | 35 | Map(float gridRes, float probThresHit, float probThresMiss,float mappingRes=0, float plannerRes=0, int nodeEdge=0){ 36 | //Define GridRes 37 | _gridRes = gridRes; 38 | //Define Mapping Variables 39 | _logOddsMissThreshold = log(probThresMiss)-log(1-probThresMiss); 40 | _logOddsHitThreshold = log(probThresHit)-log(1-probThresHit); 41 | _occupiedClampingThreshold = log(0.97)-log(1-0.97); 42 | _emptyClampingThreshold = (log(0.12)-log(1-0.12)); 43 | //Create Grids 44 | _occupiedGrid = openvdb::FloatGrid::create(0.0); 45 | // Create Linear Transforms for World to GridRes transform. 46 | _occupiedGrid->setTransform(openvdb::math::Transform::createLinearTransform(_gridRes)); 47 | // Identify the grids as level set 48 | _occupiedGrid->setGridClass(openvdb::GRID_LEVEL_SET); 49 | _occupiedGrid->setName("OccupiedGrid"); 50 | //create accessor ptr 51 | _occAccessor = std::make_shared(_occupiedGrid->getAccessor()); 52 | } 53 | virtual void populateGridIndex(){} 54 | virtual openvdb::FloatGrid::Ptr occupiedGrid(){return _occupiedGrid;} 55 | 56 | virtual float occupiedClampingThreshold() {return _occupiedClampingThreshold;} 57 | virtual float emptyClampingThreshold() {return _emptyClampingThreshold;} 58 | virtual float logOddsHitThreshold() {return _logOddsHitThreshold;} 59 | virtual float logOddsMissThreshold() {return _logOddsMissThreshold;} 60 | virtual std::shared_ptr occAccessor(){return _occAccessor;} 61 | float gridRes(){return _gridRes;} 62 | 63 | private: 64 | float _gridRes; 65 | float _logOddsMissThreshold; 66 | float _logOddsHitThreshold; 67 | float _occupiedClampingThreshold; 68 | float _emptyClampingThreshold; 69 | FloatGrid::Ptr _occupiedGrid; 70 | std::shared_ptr _occAccessor; 71 | 72 | 73 | }; 74 | }//namespace map 75 | }//namespace nanomap 76 | #endif 77 | -------------------------------------------------------------------------------- /include/nanomap/mapgen/procedural/CaveGen.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file CaveGen.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_MAPGEN_CAVEGEN_H_INCLUDED 9 | #define NANOMAP_MAPGEN_CAVEGEN_H_INCLUDED 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #define LEAF_SIZE 8 32 | namespace nanomap{ 33 | namespace mapgen{ 34 | class CaveGen{ 35 | public: 36 | CaveGen(std::string config){ 37 | loadConfig(config); 38 | srand(_seed); 39 | } 40 | 41 | void gen(); 42 | void populateMap(); 43 | void populateMap2D(); 44 | int getActiveNeighbourCount(int x, int y, int z); 45 | int get2DNeighbourCount(int x, int y); 46 | void smoothMap(); 47 | void smoothMapXY(); 48 | void setFloorAndCeil(); 49 | void getRegions(); 50 | void keepLargestRegion(); 51 | void addNoise(); 52 | void invertAndSave(); 53 | void Save(); 54 | void loadConfig(std::string file_in); 55 | 56 | private: 57 | bool _use_rand_seed; 58 | std::string _file_out; 59 | int _seed, _active_max, _active_min; 60 | std::array _map_size; 61 | double _grid_res; 62 | std::array _map_dimensions; 63 | int _smoothing_count, _fill_percentage; 64 | openvdb::FloatGrid::Ptr _cave_grid = openvdb::FloatGrid::create(0.0); 65 | std::vector> _regions; 66 | int _region_index; 67 | }; 68 | }//namespace mapgen 69 | }//namespace gymvdb 70 | #endif 71 | -------------------------------------------------------------------------------- /include/nanomap/nanomap.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file nanomap.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_H_INCLUDED 9 | #define NANOMAP_H_INCLUDED 10 | #include 11 | #include 12 | #include 13 | 14 | #define M_PI 3.14159265358979323846 15 | #define MAX_INT 32766 16 | #define VOXEL_SIZE 1 17 | 18 | namespace nanomap{ 19 | //Basic pose structure for managing agent Poses 20 | struct Pose 21 | { 22 | Eigen::Vector3f position; 23 | Eigen::Quaternionf orientation; 24 | Pose(){ 25 | position = Eigen::Vector3f(0.0f,0.0f,0.0f); 26 | orientation = Eigen::Quaternionf(0.0f,0.0f,0.0f,1.0f); 27 | } 28 | }; 29 | 30 | } 31 | 32 | 33 | 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/nanomap/sensor/FrustumData.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file FrustumData.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_SENSOR_FRUSTUMDATA_H_HAS_BEEN_INCLUDED 9 | #define NANOMAP_SENSOR_FRUSTUMDATA_H_HAS_BEEN_INCLUDED 10 | #include "SensorData.h" 11 | 12 | 13 | using ValueT = float; 14 | using EigenVec = Eigen::Matrix; 15 | namespace nanomap{ 16 | namespace sensor{ 17 | class FrustumData : public SensorData{ 18 | 19 | public: 20 | FrustumData(std::string sensorName, int sensorId, Eigen::Matrix frameTransform, ValueT gridRes, int leafEdge, int hRes, int vRes, 21 | ValueT vFOV, int rate, ValueT maxRange, ValueT minRange, ValueT probHit, ValueT probMiss) 22 | { 23 | _shared._vfov = vFOV; 24 | _shared._aspect = ((float)hRes)/((float)vRes); 25 | _shared._type = 0; 26 | _shared._hRes = hRes; 27 | _shared._vRes = vRes; 28 | _shared._rate = rate; 29 | _shared._frameTime = 1/rate; 30 | _shared._frameTransform = frameTransform; 31 | _shared._pclSize = hRes*vRes; 32 | _shared._probHit = probHit; 33 | _shared._probMiss = probMiss; 34 | _shared._maxRange = maxRange; 35 | _shared._minRange = minRange; 36 | _shared._minVoxelRange = minRange/gridRes; 37 | _shared._maxVoxelRange = maxRange/gridRes; 38 | _sensorId = sensorId; 39 | _sensorName = sensorName; 40 | _shared._gridRes = gridRes; 41 | _shared._leafEdge = leafEdge; 42 | _pointCloud.resize(3,_shared._pclSize); 43 | calculateSensorBounds(); 44 | calculateSensorNorms(); 45 | calculateBufferSize(); 46 | 47 | } 48 | 49 | 50 | void calculateSensorBounds(){ 51 | ValueT halfHeight = _shared._maxVoxelRange*(tan(_shared._vfov*M_PI/360)); 52 | ValueT halfWidth = _shared._aspect*halfHeight; 53 | ValueT closeHeight = _shared._minVoxelRange*(tan(_shared._vfov*M_PI/360)); 54 | ValueT closeWidth = _shared._aspect*closeHeight; 55 | _shared._sensorBounds.col(0) = EigenVec( -closeWidth, -closeHeight,_shared._minVoxelRange); 56 | _shared._sensorBounds.col(1) = EigenVec( -closeWidth, closeHeight,_shared._minVoxelRange); 57 | _shared._sensorBounds.col(2) = EigenVec( closeWidth, closeHeight,_shared._minVoxelRange); 58 | _shared._sensorBounds.col(3) = EigenVec( closeWidth, -closeHeight,_shared._minVoxelRange); 59 | _shared._sensorBounds.col(4) = EigenVec( -halfWidth, -halfHeight,_shared._maxVoxelRange); 60 | _shared._sensorBounds.col(5) = EigenVec(-halfWidth, halfHeight,_shared._maxVoxelRange); 61 | _shared._sensorBounds.col(6) = EigenVec(halfWidth, halfHeight,_shared._maxVoxelRange); 62 | _shared._sensorBounds.col(7) = EigenVec( halfWidth, -halfHeight,_shared._maxVoxelRange); 63 | _shared._sensorBounds.col(8) = EigenVec(0.0f, 0.0f, 0.0f); 64 | halfHeight = _shared._maxRange*(tan(_shared._vfov*M_PI/360)); 65 | halfWidth = _shared._aspect*halfHeight; 66 | closeHeight = _shared._minRange*(tan(_shared._vfov*M_PI/360)); 67 | closeWidth = _shared._aspect*closeHeight; 68 | _shared._worldBounds.col(0) = EigenVec( -closeWidth, -closeHeight,_shared._minRange); 69 | _shared._worldBounds.col(1) = EigenVec( -closeWidth, closeHeight,_shared._minRange); 70 | _shared._worldBounds.col(2) = EigenVec( closeWidth, closeHeight,_shared._minRange); 71 | _shared._worldBounds.col(3) = EigenVec( closeWidth, -closeHeight,_shared._minRange); 72 | _shared._worldBounds.col(4) = EigenVec( -halfWidth, -halfHeight,_shared._maxRange); 73 | _shared._worldBounds.col(5) = EigenVec(-halfWidth, halfHeight,_shared._maxRange); 74 | _shared._worldBounds.col(6) = EigenVec(halfWidth, halfHeight,_shared._maxRange); 75 | _shared._worldBounds.col(7) = EigenVec( halfWidth, -halfHeight,_shared._maxRange); 76 | _shared._worldBounds.col(8) = EigenVec(0.0f, 0.0f, 0.0f); 77 | _shared._worldMax = Eigen::Vector3f(_shared._worldBounds.row(0).maxCoeff(), 78 | _shared._worldBounds.row(1).maxCoeff(), 79 | _shared._worldBounds.row(2).maxCoeff()); 80 | _shared._worldMin = Eigen::Vector3f(_shared._worldBounds.row(0).minCoeff(), 81 | _shared._worldBounds.row(1).minCoeff(), 82 | _shared._worldBounds.row(2).minCoeff()); 83 | } 84 | 85 | void calculateBufferSize(){ 86 | #pragma STDC FENV_ACCESS ON 87 | std::fesetround(FE_DOWNWARD); 88 | int leafBufferSize; 89 | int roll, pitch, yaw; 90 | float leafEdgeFactor = ((float)(_shared._leafEdge)-0.5); 91 | 92 | _shared._maxLeafBufferSize = 0; 93 | 94 | 95 | Eigen::Matrix rotatedBounds; 96 | Eigen::Matrix offsetBounds; 97 | Eigen::Matrix offset; 98 | Eigen::Quaternionf q = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) 99 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 100 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 101 | Eigen::Matrix r = q.toRotationMatrix(); 102 | for(roll = 0; roll < 90; roll ++){ 103 | for(pitch = 0; pitch < 180; pitch ++){ 104 | for(yaw = 0; yaw < 360; yaw ++ ){ 105 | q = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) 106 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 107 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 108 | r = q.toRotationMatrix(); 109 | rotatedBounds = r*_shared._sensorBounds; 110 | Eigen::Vector3f max(rotatedBounds.row(0).maxCoeff(), 111 | rotatedBounds.row(1).maxCoeff(), 112 | rotatedBounds.row(2).maxCoeff()); 113 | Eigen::Vector3f min(rotatedBounds.row(0).minCoeff(), 114 | rotatedBounds.row(1).minCoeff(), 115 | rotatedBounds.row(2).minCoeff()); 116 | nanovdb::CoordBBox leafBound(nanovdb::Coord( 117 | std::floor(std::nearbyint(min(0))/_shared._leafEdge), 118 | std::floor(std::nearbyint(min(1))/_shared._leafEdge), 119 | std::floor(std::nearbyint(min(2))/_shared._leafEdge)), 120 | nanovdb::Coord( 121 | std::floor(std::nearbyint(max(0))/_shared._leafEdge), 122 | std::floor(std::nearbyint(max(1))/_shared._leafEdge), 123 | std::floor(std::nearbyint(max(2))/_shared._leafEdge))); 124 | leafBufferSize = leafBound.dim().x()*leafBound.dim().y()*leafBound.dim().z(); 125 | if(_shared._maxLeafBufferSize < leafBufferSize){ 126 | _shared._maxLeafBufferSize = leafBufferSize; 127 | } 128 | } 129 | } 130 | } 131 | } 132 | 133 | protected: 134 | 135 | 136 | }; 137 | } 138 | } 139 | #endif 140 | -------------------------------------------------------------------------------- /include/nanomap/sensor/LaserData.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file LaserData.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_SENSOR_LASERDATA_H_HAS_BEEN_INCLUDED 9 | #define NANOMAP_SENSOR_LASERDATA_H_HAS_BEEN_INCLUDED 10 | #include "SensorData.h" 11 | #include 12 | #include "nanomap/handler/handlerAssert.h" 13 | using ValueT = float; 14 | using EigenVec = Eigen::Matrix; 15 | namespace nanomap{ 16 | namespace sensor{ 17 | class LaserData : public SensorData{ 18 | 19 | public: 20 | LaserData(std::string sensorName, int sensorId, Eigen::Matrix frameTransform, ValueT gridRes, int leafEdge, float aHRes, float aVRes, 21 | ValueT hFOV, ValueT vFOV, int rate, ValueT maxRange, ValueT minRange, ValueT probHit, ValueT probMiss) 22 | { 23 | _shared._vfov = vFOV; 24 | _shared._hfov = hFOV; 25 | _shared._hRes = (hFOV/aHRes+1); 26 | _shared._vRes = (vFOV/aVRes+1); 27 | _scale[0] = hFOV/_shared._hRes; 28 | _scale[1] = vFOV/_shared._vRes; 29 | _shared._rate = rate; 30 | _shared._frameTime = 1/rate; 31 | _shared._type = 1; 32 | _shared._frameTransform = frameTransform; 33 | _shared._pclSize = _shared._hRes*_shared._vRes; 34 | _shared._probHit = probHit; 35 | _shared._probMiss = probMiss; 36 | _shared._maxRange = maxRange; 37 | _shared._minRange = minRange; 38 | _shared._minVoxelRange = minRange/gridRes; 39 | _shared._maxVoxelRange = maxRange/gridRes; 40 | _sensorId = sensorId; 41 | _sensorName = sensorName; 42 | _shared._gridRes = gridRes; 43 | _shared._leafEdge = leafEdge; 44 | _pointCloud.resize(3,_shared._pclSize); 45 | calculateSensorBounds(); 46 | calculateSensorNorms(); 47 | //calculateBufferSize(); 48 | 49 | } 50 | 51 | // void getLaserRay(int w, int h, openvdb::Vec3d& rayDir){ 52 | // 53 | // float u = -_shared._hfov/2+((float)w)*_scale[0]; // yaw angle in radians 54 | // float v = -_shared._vfov/2+((float)h)*_scale[1]; // pitch angle in radians 55 | // u *= 3.14159265358979323846 / 180; 56 | // v *= 3.14159265358979323846 / 180; 57 | // //Calculate unit direction of ray in xyz frame 58 | // EigenVec matrix_dir(std::sin(u)*std::cos(v), std::sin(v),std::cos(u)*std::cos(v)); 59 | // //normalise ray 60 | // matrix_dir.normalize(); 61 | // rayDir = openvdb::Vec3d(matrix_dir(0),matrix_dir(1),matrix_dir(2)); 62 | // 63 | // } 64 | // void calculateBufferSize(){ 65 | // openvdb::Int32Grid::Ptr tempGrid = openvdb::Int32Grid::create(0); 66 | // openvdb::Vec3d rayEyes[] = { 67 | // openvdb::Vec3d{0.5,0.5,0.5}, 68 | // openvdb::Vec3d{0.0,0.0,0.0}, 69 | // openvdb::Vec3d{0.0,0.0,1.0}, 70 | // openvdb::Vec3d{0.0,1.0,0.0}, 71 | // openvdb::Vec3d{0.0,1.0,1.0}, 72 | // openvdb::Vec3d{1.0,0.0,0.0}, 73 | // openvdb::Vec3d{1.0,0.0,1.0}, 74 | // openvdb::Vec3d{1.0,1.0,0.0}, 75 | // openvdb::Vec3d{1.0,1.0,1.0} 76 | // }; 77 | // auto acc = tempGrid->getAccessor(); 78 | // openvdb::math::Ray ray; 79 | // openvdb::math::DDA, 0> dda; 80 | // for(int pos = 0; pos < 9; pos++){ 81 | // for(int w = 0; w < _shared._hRes; w++){ 82 | // for(int h = 0; h < _shared._vRes; h++){ 83 | // openvdb::Vec3d rayEye = rayEyes[pos]; 84 | // openvdb::Vec3d rayDir; 85 | // getLaserRay(w, h, rayDir); 86 | // ray.setEye(rayEye); 87 | // ray.setDir(rayDir); 88 | // dda.init(ray, _shared._minVoxelRange, _shared._maxVoxelRange); 89 | // openvdb::Coord voxel = dda.voxel(); 90 | // while(dda.step()){ 91 | // acc.setValue(voxel, 1.0); 92 | // voxel = dda.voxel(); 93 | // } 94 | // acc.setValue(voxel, 1.0); 95 | // } 96 | // } 97 | // } 98 | // _laserVoxelCount = tempGrid->activeVoxelCount(); 99 | // std::cout << "laserVoxelCount = " << _laserVoxelCount << std::endl; 100 | // cudaCheck(cudaMallocHost((void**)&_laserIndex, (_laserVoxelCount)*sizeof(nanovdb::Coord))); 101 | // 102 | // int count = 0; 103 | // _laserGrid = openvdb::Int32Grid::create(-1); 104 | // auto laserAcc = _laserGrid->getAccessor(); 105 | // for (openvdb::Int32Grid::ValueOnCIter iter = tempGrid->cbeginValueOn(); iter; ++iter) { 106 | // openvdb::Coord coord = iter.getCoord(); 107 | // _laserIndex[count] = nanovdb::Coord(coord.x(), coord.y(), coord.z()); 108 | // laserAcc.setValue(coord, count); 109 | // count++; 110 | // //std::cout << "Grid" << iter.getCoord() << " = " << *iter << std::endl; 111 | // } 112 | // } 113 | // 114 | // 115 | void calculateSensorBounds(){ 116 | ValueT ZMin, XMax, XMin, WorldZMin, WorldXMax, WorldXMin; 117 | ValueT YMax = _shared._maxVoxelRange*(sin(_shared._vfov*M_PI/360)); 118 | ValueT WorldYMax = _shared._maxRange*(sin(_shared._vfov*M_PI/360)); 119 | ValueT YMin = -YMax; 120 | ValueT WorldYMin = -WorldYMax; 121 | ValueT ZMax = _shared._maxVoxelRange; 122 | ValueT WorldZMax = _shared._maxRange; 123 | if(_shared._hfov < 180){ 124 | ZMin = 0.0; 125 | WorldZMin = 0.0; 126 | XMax = _shared._maxVoxelRange*(sin(_shared._hfov*M_PI/360)); 127 | XMin = - XMax; 128 | WorldXMax = _shared._maxRange*(sin(_shared._hfov*M_PI/360)); 129 | WorldXMin = -WorldXMax; 130 | }else if(_shared._hfov>= 180 && _shared._hfov < 360){ 131 | ZMin = _shared._maxVoxelRange*(cos(_shared._hfov*M_PI/360)); 132 | WorldZMin = _shared._maxRange*(cos(_shared._hfov*M_PI/360)); 133 | XMax = _shared._maxVoxelRange; 134 | XMin = -XMax; 135 | WorldXMax = _shared._maxRange; 136 | WorldXMin = -WorldXMax; 137 | }else if(_shared._hfov>=360){ 138 | ZMin = -ZMax; 139 | WorldZMin = -WorldZMax; 140 | XMax = _shared._maxVoxelRange; 141 | XMin = -XMax; 142 | WorldXMax = _shared._maxRange; 143 | WorldXMin = -WorldXMax; 144 | } 145 | _shared._sensorBounds.col(0) = EigenVec( XMin, YMin, ZMin); 146 | _shared._sensorBounds.col(1) = EigenVec( XMin, YMax, ZMin); 147 | _shared._sensorBounds.col(2) = EigenVec( XMax, YMax, ZMin); 148 | _shared._sensorBounds.col(3) = EigenVec( XMax, YMin, ZMin); 149 | _shared._sensorBounds.col(4) = EigenVec( XMin, YMin, ZMax); 150 | _shared._sensorBounds.col(5) = EigenVec( XMin, YMax, ZMax); 151 | _shared._sensorBounds.col(6) = EigenVec( XMax, YMax, ZMax); 152 | _shared._sensorBounds.col(7) = EigenVec( XMax, YMin, ZMax); 153 | _shared._sensorBounds.col(8) = EigenVec(0.0f, 0.0f, 0.0f); 154 | _shared._worldBounds.col(0) = EigenVec( WorldXMin, WorldYMin, WorldZMin); 155 | _shared._worldBounds.col(1) = EigenVec( WorldXMin, WorldYMax, WorldZMin); 156 | _shared._worldBounds.col(2) = EigenVec( WorldXMax, WorldYMax, WorldZMin); 157 | _shared._worldBounds.col(3) = EigenVec( WorldXMax, WorldYMin, WorldZMin); 158 | _shared._worldBounds.col(4) = EigenVec( WorldXMin, WorldYMin, WorldZMax); 159 | _shared._worldBounds.col(5) = EigenVec( WorldXMin, WorldYMax, WorldZMax); 160 | _shared._worldBounds.col(6) = EigenVec( WorldXMax, WorldYMax, WorldZMax); 161 | _shared._worldBounds.col(7) = EigenVec( WorldXMax, WorldYMin, WorldZMax); 162 | _shared._worldBounds.col(8) = EigenVec(0.0f, 0.0f, 0.0f); 163 | _shared._worldMax = Eigen::Vector3f(_shared._worldBounds.row(0).maxCoeff(), 164 | _shared._worldBounds.row(1).maxCoeff(), 165 | _shared._worldBounds.row(2).maxCoeff()); 166 | _shared._worldMin = Eigen::Vector3f(_shared._worldBounds.row(0).minCoeff(), 167 | _shared._worldBounds.row(1).minCoeff(), 168 | _shared._worldBounds.row(2).minCoeff()); 169 | } 170 | 171 | protected: 172 | float _scale[2]; 173 | 174 | }; 175 | } 176 | } 177 | #endif 178 | -------------------------------------------------------------------------------- /include/nanomap/sensor/SensorData.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SensorData.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_SENSOR_SENSORDATA_H_HAS_BEEN_INCLUDED 9 | #define NANOMAP_SENSOR_SENSORDATA_H_HAS_BEEN_INCLUDED 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "nanomap/nanomap.h" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "nanomap/sensor/SharedParameters.h" 27 | #include 28 | #include 29 | using ValueT = float; 30 | 31 | namespace nanomap{ 32 | namespace sensor{ 33 | class SensorData{ 34 | 35 | public: 36 | SensorData(){} 37 | 38 | virtual void calculateSensorBounds(){} 39 | virtual void calculateBufferSize(){} 40 | void updatePose(nanomap::Pose pose){ 41 | #pragma STDC FENV_ACCESS ON 42 | std::fesetround(FE_DOWNWARD); 43 | _shared._pose = pose; 44 | if(_shared._pose.position(0) == 0.0){ 45 | _shared._voxelPosition(0) = 0; 46 | _shared._leafOffset(0) = 0; 47 | _shared._leafOriginOffset(0) = 0; 48 | _shared._voxelOffset(0) = 0; 49 | _shared._voxelOriginOffset(0) = 0; 50 | }else{ 51 | _shared._voxelPosition(0) = _shared._pose.position(0)/_shared._gridRes; 52 | _shared._leafOffset(0) = std::floor(std::nearbyint(_shared._voxelPosition(0))/_shared._leafEdge)*_shared._leafEdge; 53 | _shared._leafOriginOffset(0) = _shared._voxelPosition(0) - _shared._leafOffset(0); 54 | _shared._voxelOffset(0) = std::nearbyint(_shared._voxelPosition(0)); 55 | _shared._voxelOriginOffset(0) = _shared._voxelPosition(0)-_shared._voxelOffset(0); 56 | } 57 | if(_shared._pose.position(1)==0.0){ 58 | _shared._voxelPosition(1) = 0; 59 | _shared._leafOffset(1) = 0; 60 | _shared._leafOriginOffset(1) = 0; 61 | _shared._voxelOffset(1) = 0; 62 | _shared._voxelOriginOffset(1) = 0; 63 | }else{ 64 | _shared._voxelPosition(1) = _shared._pose.position(1)/_shared._gridRes; 65 | _shared._leafOffset(1) = std::floor(std::nearbyint(_shared._voxelPosition(1))/_shared._leafEdge)*_shared._leafEdge; 66 | _shared._leafOriginOffset(1) = _shared._voxelPosition(1) - _shared._leafOffset(1); 67 | _shared._voxelOffset(1) = std::nearbyint(_shared._voxelPosition(1)); 68 | _shared._voxelOriginOffset(1) = _shared._voxelPosition(1)-_shared._voxelOffset(1); 69 | } 70 | if(_shared._pose.position(2) == 0.0){ 71 | _shared._voxelPosition(2) = 0; 72 | _shared._leafOffset(2) = 0; 73 | _shared._leafOriginOffset(2) = 0; 74 | _shared._voxelOffset(2) = 0; 75 | _shared._voxelOriginOffset(2) = 0; 76 | }else{ 77 | _shared._voxelPosition(2) = _shared._pose.position(2)/_shared._gridRes; 78 | _shared._leafOffset(2) = std::floor(std::nearbyint(_shared._voxelPosition(2))/_shared._leafEdge)*_shared._leafEdge; 79 | _shared._leafOriginOffset(2) = _shared._voxelPosition(2) - _shared._leafOffset(2); 80 | _shared._voxelOffset(2) = std::nearbyint(_shared._voxelPosition(2)); 81 | _shared._voxelOriginOffset(2) = _shared._voxelPosition(2)-_shared._voxelOffset(2); 82 | } 83 | } 84 | 85 | void rotateView(){ 86 | Eigen::Matrix rotation = _shared._pose.orientation.toRotationMatrix()*_shared._frameTransform; 87 | _shared._transformedBounds = rotation*_shared._sensorBounds; 88 | _shared._transformedNorms = rotation*_shared._sensorNorms; 89 | _shared._voxelTransformedBounds = _shared._transformedBounds.colwise()+_shared._voxelOriginOffset; 90 | _shared._leafTransformedBounds = _shared._transformedBounds.colwise() + _shared._leafOriginOffset; 91 | getBoundingBoxes(); 92 | } 93 | 94 | void calculateSensorNorms(){ 95 | _shared._sensorNorms.col(0) = 96 | ((_shared._sensorBounds.col(1)-_shared._sensorBounds.col(2)).cross( 97 | _shared._sensorBounds.col(0)-_shared._sensorBounds.col(1))).normalized(); 98 | _shared._sensorNorms.col(1) = 99 | (_shared._sensorBounds.col(5).cross( 100 | _shared._sensorBounds.col(4)-_shared._sensorBounds.col(5))).normalized(); 101 | _shared._sensorNorms.col(2) = 102 | (_shared._sensorBounds.col(6).cross( 103 | _shared._sensorBounds.col(5)-_shared._sensorBounds.col(6))).normalized(); 104 | _shared._sensorNorms.col(3) = 105 | (_shared._sensorBounds.col(7).cross( 106 | _shared._sensorBounds.col(6)-_shared._sensorBounds.col(7))).normalized(); 107 | _shared._sensorNorms.col(4) = 108 | (_shared._sensorBounds.col(4).cross( 109 | _shared._sensorBounds.col(7)-_shared._sensorBounds.col(4))).normalized(); 110 | } 111 | void getBoundingBoxes(){ 112 | #pragma STDC FENV_ACCESS ON 113 | std::fesetround(FE_DOWNWARD); 114 | Eigen::Vector3f leafMax(_shared._leafTransformedBounds.row(0).maxCoeff(), 115 | _shared._leafTransformedBounds.row(1).maxCoeff(), 116 | _shared._leafTransformedBounds.row(2).maxCoeff()); 117 | Eigen::Vector3f leafMin(_shared._leafTransformedBounds.row(0).minCoeff(), 118 | _shared._leafTransformedBounds.row(1).minCoeff(), 119 | _shared._leafTransformedBounds.row(2).minCoeff()); 120 | nanovdb::CoordBBox leafBound(nanovdb::Coord( 121 | std::floor((std::nearbyint(leafMin(0)))/_shared._leafEdge), 122 | std::floor((std::nearbyint(leafMin(1)))/_shared._leafEdge), 123 | std::floor((std::nearbyint(leafMin(2)))/_shared._leafEdge)), 124 | nanovdb::Coord( 125 | std::floor((std::nearbyint(leafMax(0)))/_shared._leafEdge), 126 | std::floor((std::nearbyint(leafMax(1)))/_shared._leafEdge), 127 | std::floor((std::nearbyint(leafMax(2)))/_shared._leafEdge))); 128 | 129 | Eigen::Vector3f voxelMax(_shared._voxelTransformedBounds.row(0).maxCoeff(), 130 | _shared._voxelTransformedBounds.row(1).maxCoeff(), 131 | _shared._voxelTransformedBounds.row(2).maxCoeff()); 132 | Eigen::Vector3f voxelMin(_shared._voxelTransformedBounds.row(0).minCoeff(), 133 | _shared._voxelTransformedBounds.row(1).minCoeff(), 134 | _shared._voxelTransformedBounds.row(2).minCoeff()); 135 | nanovdb::CoordBBox voxelBound(nanovdb::Coord( 136 | std::nearbyint(voxelMin(0)), 137 | std::nearbyint(voxelMin(1)), 138 | std::nearbyint(voxelMin(2))), 139 | nanovdb::Coord( 140 | std::nearbyint(voxelMax(0)), 141 | std::nearbyint(voxelMax(1)), 142 | std::nearbyint(voxelMax(2)))); 143 | _shared._leafBounds = leafBound; 144 | _shared._voxelBounds = voxelBound; 145 | _shared._leafBufferSize = leafBound.dim().x()*leafBound.dim().y()*leafBound.dim().z(); 146 | } 147 | 148 | std::string sensorName(){return _sensorName;} 149 | int getId(){return _sensorId;} 150 | std::string getName(){return _sensorName;} 151 | nanomap::sensor::SharedParameters sharedParameters(){return _shared;} 152 | openvdb::Int32Grid::Ptr laserGrid(){return _laserGrid;} 153 | nanovdb::Coord* laserIndex(){return _laserIndex;} 154 | int laserVoxelCount(){return _laserVoxelCount;} 155 | Eigen::Matrix& pointCloud(){return _pointCloud;} 156 | protected: 157 | std::string _sensorName; 158 | int _sensorId; 159 | openvdb::Int32Grid::Ptr _laserGrid; 160 | nanovdb::Coord* _laserIndex; 161 | int _laserVoxelCount; 162 | 163 | nanomap::sensor::SharedParameters _shared; 164 | 165 | Eigen::Matrix _pointCloud; 166 | }; 167 | } 168 | } 169 | #endif 170 | -------------------------------------------------------------------------------- /include/nanomap/sensor/SharedParameters.h: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SharedParameters.h 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | #ifndef NANOMAP_SENSOR_SHAREDPARAMETERS_H_HAS_BEEN_INCLUDED 9 | #define NANOMAP_SENSOR_SHAREDPARAMETERS_H_HAS_BEEN_INCLUDED 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "nanomap/nanomap.h" 19 | #include 20 | #include 21 | using ValueT = float; 22 | 23 | namespace nanomap{ 24 | namespace sensor{ 25 | class SharedParameters{ 26 | 27 | public: 28 | int _type; 29 | int _rate; 30 | float _frameTime; 31 | ValueT _gridRes; 32 | int _hRes; 33 | int _vRes; 34 | int _pclSize; 35 | ValueT _aspect; 36 | ValueT _vfov; 37 | ValueT _hfov; 38 | nanomap::Pose _pose; 39 | Eigen::Matrix _voxelPosition; 40 | Eigen::Matrix _leafOffset; 41 | Eigen::Matrix _voxelOffset; 42 | Eigen::Matrix _leafOriginOffset; 43 | Eigen::Matrix _voxelOriginOffset; 44 | Eigen::Matrix _sensorBounds; 45 | Eigen::Matrix _worldBounds; 46 | Eigen::Matrix _transformedBounds; 47 | Eigen::Matrix _voxelTransformedBounds; 48 | Eigen::Matrix _leafTransformedBounds; 49 | Eigen::Matrix _sensorNorms; 50 | Eigen::Matrix _transformedNorms; 51 | Eigen::Matrix _frameTransform; 52 | nanovdb::CoordBBox _leafBounds; 53 | nanovdb::CoordBBox _voxelBounds; 54 | Eigen::Vector3f _worldMax; 55 | Eigen::Vector3f _worldMin; 56 | ValueT _probHit; 57 | ValueT _probMiss; 58 | 59 | ValueT _maxRange; 60 | ValueT _minRange; 61 | ValueT _minVoxelRange; 62 | ValueT _maxVoxelRange; 63 | 64 | int _leafEdge; 65 | int _maxLeafBufferSize; 66 | int _maxVoxelBufferSize; 67 | int _leafBufferSize; 68 | 69 | }; 70 | } 71 | } 72 | #endif 73 | -------------------------------------------------------------------------------- /src/nanomap/config/Config.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Config.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/config/Config.h" 10 | 11 | #define M_PI 3.14159265358979323846 12 | #define MAX_INT 32766 13 | #define VOXEL_SIZE 1 14 | 15 | namespace nanomap{ 16 | namespace config{ 17 | //Large Config Class Definitions 18 | Config::Config(std::string configFile) 19 | { 20 | _config = configFile; 21 | _configCheck = true; 22 | init(); 23 | } 24 | void Config::init() 25 | { 26 | if(_configCheck){ 27 | loadConfig(); 28 | loadSensors(); 29 | _leafVolume = _leafEdge * _leafEdge * _leafEdge; 30 | _frustumLeafBufferSize = 0; 31 | _frustumPCLSize = 0; 32 | _laserPCLSize = 0; 33 | //_laserVoxelBufferSize = 0; 34 | _maxPCLSize = 0; 35 | for(int i = 0; i < _sensorData.size(); i++){ 36 | if(_sensorData[i]->sharedParameters()._pclSize>_maxPCLSize){ 37 | _maxPCLSize = _sensorData[i]->sharedParameters()._pclSize; 38 | } 39 | if(_sensorData[i]->sharedParameters()._type==0){ 40 | if(_sensorData[i]->sharedParameters()._pclSize > _frustumPCLSize){ 41 | _frustumPCLSize = _sensorData[i]->sharedParameters()._pclSize; 42 | } 43 | if(_sensorData[i]->sharedParameters()._maxLeafBufferSize > _frustumLeafBufferSize){ 44 | _frustumLeafBufferSize = _sensorData[i]->sharedParameters()._maxLeafBufferSize; 45 | } 46 | }else if(_sensorData[i]->sharedParameters()._type==1){ 47 | if(_sensorData[i]->sharedParameters()._pclSize > _laserPCLSize){ 48 | _laserPCLSize = _sensorData[i]->sharedParameters()._pclSize; 49 | 50 | } 51 | } 52 | 53 | 54 | } 55 | if(_frustumLeafBufferSize > 0){ 56 | _processType = 1; 57 | } 58 | } 59 | } 60 | void Config::loadConfig(){ 61 | std::cout <<"reading in config file: " << _config << std::endl; 62 | std::ifstream *input = new std::ifstream(_config.c_str(), std::ios::in | std::ios::binary); 63 | //bool end = false; 64 | std::string file, line; 65 | float gridRes, mappingRes, frustumAllocationFactor, laserAllocationFactor, probHitThres, probMissThres; 66 | int leafEdge, filterType, processType, updateType, exploreType, precisionType, simType, publishSensor; 67 | int serialUpdate; 68 | *input >> line; 69 | //std::cout << line << std::endl; 70 | float res; 71 | if(line.compare("#config") != 0){ 72 | std::cout << "Error: first line reads [" << line << "] instead of [#config]" << std::endl; 73 | delete input; 74 | return; 75 | } 76 | while(input->good()) { 77 | *input >> line; 78 | if (line.compare("MappingRes:") == 0){ 79 | *input >> mappingRes; 80 | _mappingRes = mappingRes; 81 | }else if (line.compare("ProbHitThres:") == 0){ 82 | *input >> probHitThres; 83 | _probHitThres = probHitThres; 84 | }else if (line.compare("ProbMissThres:") == 0){ 85 | *input >> probMissThres; 86 | _probMissThres = probMissThres; 87 | }else if (line.compare("GridRes:") == 0){ 88 | *input >> gridRes; 89 | _gridRes = gridRes; 90 | }else if (line.compare("NodeEdge:") == 0){ 91 | *input >> leafEdge; 92 | _leafEdge = leafEdge; 93 | }else if (line.compare("FrustumAllocationFactor:") == 0){ 94 | *input >> frustumAllocationFactor; 95 | _frustumAllocationFactor = frustumAllocationFactor; 96 | }else if (line.compare("FilterType:") == 0){ 97 | *input >> filterType; 98 | _filterType = filterType; 99 | }else if(line.compare("UpdateType:")==0){ 100 | *input >> updateType; 101 | _updateType = updateType; 102 | }else if (line.compare("PrecisionType:") == 0){ 103 | *input >> precisionType; 104 | _precisionType = precisionType; 105 | }else if (line.compare("PublishType:") == 0){ 106 | *input >> publishSensor; 107 | _publishSensor = publishSensor; 108 | }else if (line.compare("#endconfig")==0){ 109 | break; 110 | } 111 | } 112 | input->close(); 113 | } 114 | 115 | void Config::loadAgents(openvdb::FloatGrid::Ptr& simGrid){ 116 | loadAgentData(simGrid); 117 | } 118 | /******************************************************************************/ 119 | void Config::loadAgentData(openvdb::FloatGrid::Ptr& simGrid){ 120 | std::string line; 121 | std::string agentFile; 122 | std::vector agentFiles; 123 | std::ifstream *input = new std::ifstream(_config.c_str(), std::ios::in | std::ios::binary); 124 | *input >> line; 125 | //check if file is frustumconfig 126 | if(line.compare("#config") == 0){ 127 | while(input->good()) { 128 | *input >> line; 129 | if (line.compare("Agent:") == 0){ 130 | *input >> agentFile; 131 | agentFiles.push_back(agentFile); 132 | }else if(line.compare("#endconfig")==0){ 133 | break; 134 | } 135 | } 136 | input->close(); 137 | for(int i=0; i(nanomap::agent::Agent(simGrid, agentFiles[i], _mappingRes, _gridRes, _probHitThres, _probMissThres))); 139 | } 140 | } 141 | } 142 | 143 | 144 | 145 | void Config::loadSensors(){ 146 | std::vector sensorConfigs; 147 | std::string configFile, line; 148 | std::ifstream *input = new std::ifstream(_config.c_str(), std::ios::in | std::ios::binary); 149 | *input >> line; 150 | if(line.compare("#config") != 0){ 151 | std::cout << "Error: first line reads [" << line << "] instead of [#config]" << std::endl; 152 | delete input; 153 | return; 154 | } 155 | while(input->good()){ 156 | *input>>line; 157 | if(line.compare("Sensor:")==0){ 158 | *input>>configFile; 159 | sensorConfigs.push_back(configFile); 160 | }else if(line.compare("#endconfig")==0){ 161 | break; 162 | } 163 | } 164 | input->close(); 165 | for(int i=0; i Config::loadSensorData(float gridRes, 173 | int leafEdge, 174 | std::string config) 175 | { 176 | std::cout <<"reading in sensor config file: " << config << std::endl; 177 | std::string line; 178 | std::string name; 179 | int id, rate, hRes, vRes; 180 | float aHRes, aVRes, vfov, hfov, maxRange, minRange, probHit, probMiss; 181 | Eigen::Matrix frameTransform; 182 | std::ifstream *input = new std::ifstream(config.c_str(), std::ios::in | std::ios::binary); 183 | *input >> line; 184 | //check if file is frustumconfig 185 | if(line.compare("#laserconfig") == 0){ 186 | while(input->good()) { 187 | *input >> line; 188 | if (line.compare("Name:") == 0){ 189 | *input >> name; 190 | }else if(line.compare("Id:") == 0){ 191 | *input >> id; 192 | }else if(line.compare("AngularHRes:") == 0){ 193 | *input >> aHRes; 194 | }else if(line.compare("AngularVRes:") == 0){ 195 | *input >> aVRes; 196 | }else if(line.compare("HFOV:") == 0){ 197 | *input >> hfov; 198 | }else if(line.compare("VFOV:") == 0){ 199 | *input >> vfov; 200 | }else if(line.compare("Rate:") == 0){ 201 | *input >> rate; 202 | }else if(line.compare("MaxRange:") == 0){ 203 | *input >> maxRange; 204 | }else if(line.compare("MinRange:") == 0){ 205 | *input >> minRange; 206 | }else if(line.compare("ProbHit:") == 0){ 207 | *input >> probHit; 208 | }else if(line.compare("ProbMiss:") == 0){ 209 | *input >> probMiss; 210 | }else if(line.compare("FrameTransform:") == 0){ 211 | float x; 212 | for(int i = 0; i <3 ; i++){ 213 | for(int j = 0; j<3; j++){ 214 | *input >> x; 215 | frameTransform.col(i)(j) = x ; 216 | } 217 | } 218 | }else if (line.compare("#endconfig")==0){ 219 | break; 220 | } 221 | } 222 | input->close(); 223 | return std::make_shared(nanomap::sensor::LaserData(name, id, frameTransform, gridRes, leafEdge, aHRes, aVRes, hfov, 224 | vfov, rate, maxRange, minRange, 225 | probHit, probMiss)); 226 | }else if(line.compare("#frustumconfig")==0){ 227 | while(input->good()) { 228 | *input >> line; 229 | if (line.compare("Name:") == 0){ 230 | *input >> name; 231 | }else if(line.compare("Id:") == 0){ 232 | *input >> id; 233 | }else if(line.compare("HRes:") == 0){ 234 | *input >> hRes; 235 | }else if(line.compare("VRes:") == 0){ 236 | *input >> vRes; 237 | }else if(line.compare("VFOV:") == 0){ 238 | *input >> vfov; 239 | }else if(line.compare("Rate:") == 0){ 240 | *input >> rate; 241 | }else if(line.compare("MaxRange:") == 0){ 242 | *input >> maxRange; 243 | }else if(line.compare("MinRange:") == 0){ 244 | *input >> minRange; 245 | }else if(line.compare("ProbHit:") == 0){ 246 | *input >> probHit; 247 | }else if(line.compare("ProbMiss:") == 0){ 248 | *input >> probMiss; 249 | }else if(line.compare("FrameTransform:") == 0){ 250 | float x; 251 | for(int i = 0; i <3 ; i++){ 252 | for(int j = 0; j<3; j++){ 253 | *input >> x; 254 | frameTransform.col(i)(j) = x ; 255 | } 256 | } 257 | }else if (line.compare("#endconfig")==0){ 258 | break; 259 | } 260 | } 261 | input->close(); 262 | return std::make_shared(nanomap::sensor::FrustumData(name, id, frameTransform, gridRes, leafEdge, hRes, vRes, 263 | vfov, rate, maxRange, minRange, 264 | probHit, probMiss)); 265 | } 266 | return nullptr; 267 | } 268 | 269 | /******************************************************************************/ 270 | //Fetching Sensor and Agent Data; 271 | //return full sensor data vector 272 | std::vector>& Config::sensorData(){return _sensorData;} 273 | //return full agent data vector 274 | std::vector>& Config::agentData(){return _agentData;} 275 | //return sensor data object by vector index or sensorId. 276 | std::shared_ptr Config::sensorData(int index, bool searchById){ 277 | if(searchById){ 278 | //Searching objects by id and not by vector index; 279 | for (auto it = _sensorData.begin(); it != _sensorData.end(); ++it) { 280 | if((*it)->getId() == index){ 281 | return *it; 282 | } 283 | } 284 | //Object wasn't found. 285 | //RETURN EXCEPTON HERE. 286 | 287 | }else{ 288 | if(index >=0 && index < _sensorData.size()){ 289 | //index refers to valid object 290 | return _sensorData[index]; 291 | }//else{ 292 | //RETURN AN EXCEPTION HERE 293 | //} 294 | } 295 | } 296 | 297 | //return agent data object by vector index or sensorId. 298 | std::shared_ptr Config::agentData(int index, bool searchById){ 299 | if(searchById){ 300 | //Searching objects by id and not by vector index; 301 | for (auto it = _agentData.begin(); it != _agentData.end(); ++it) { 302 | if((*it)->getId() == index){ 303 | return *it; 304 | } 305 | } 306 | //Object wasn't found. 307 | //RETURN EXCEPTON HERE. 308 | 309 | }else{ 310 | if(index >=0 && index < _agentData.size()){ 311 | //index refers to valid object 312 | return _agentData[index]; 313 | }//else{ 314 | //RETURN AN EXCEPTION HERE 315 | //} 316 | } 317 | } 318 | //return sensor data from string 319 | std::shared_ptr Config::sensorData(std::string name){ 320 | //Searching objects by name; 321 | for (auto it = _sensorData.begin(); it != _sensorData.end(); ++it) { 322 | if((*it)->getName() == name){ 323 | return *it; 324 | } 325 | } 326 | //Object wasn't found. 327 | //RETURN EXCEPTON HERE. 328 | } 329 | //return agent data from string 330 | std::shared_ptr Config::agentData(std::string name){ 331 | //Searching objects by name; 332 | for (auto it = _agentData.begin(); it != _agentData.end(); ++it) { 333 | if((*it)->getName() == name){ 334 | return *it; 335 | } 336 | } 337 | //Object wasn't found. 338 | //RETURN EXCEPTON HERE. 339 | } 340 | } 341 | } 342 | -------------------------------------------------------------------------------- /src/nanomap/handler/BlockWorker.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file BlockWorker.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/handler/BlockWorker.h" 10 | 11 | namespace nanomap{ 12 | namespace handler{ 13 | BlockWorker::BlockWorker(int nodeEdge, 14 | float occClampThres, 15 | float emptyClampThres, 16 | float logOddsHitThres, 17 | float logOddsMissThres, 18 | std::shared_ptr accessor, 19 | int8_t* hostVoxelBuffer, 20 | int* hostNodeBuffer, 21 | int hostCount) 22 | : _nodeEdge(nodeEdge), 23 | _occupiedClampingThreshold(occClampThres), 24 | _emptyClampingThreshold(emptyClampThres), 25 | _logOddsHitThreshold(logOddsHitThres), 26 | _hostVoxelBuffer(hostVoxelBuffer) 27 | { 28 | _voxelVolume = nodeEdge*nodeEdge*nodeEdge; 29 | _blocks = new std::vector(); 30 | _leaves = new std::vector(); 31 | for(int i = 0; i < hostCount ; i++){ 32 | _blocks->push_back(Block(openvdb::CoordBBox(openvdb::Coord( 33 | *(hostNodeBuffer+i*3), 34 | *(hostNodeBuffer+i*3+1), 35 | *(hostNodeBuffer+i*3+2)), 36 | openvdb::Coord( 37 | *(hostNodeBuffer+i*3)+nodeEdge-1, 38 | *(hostNodeBuffer+i*3+1)+nodeEdge-1, 39 | *(hostNodeBuffer+i*3+2)+nodeEdge-1) 40 | ))); 41 | if(LeafT* target = accessor->probeLeaf((*_blocks)[i].bbox.min())){ 42 | _leaves->push_back(target); 43 | }else{ 44 | _leaves->push_back(nullptr); 45 | } 46 | } 47 | } 48 | 49 | void BlockWorker::destroyBlocks() 50 | { 51 | delete _blocks; 52 | _blocks = nullptr; 53 | delete _leaves; 54 | _leaves = nullptr; 55 | } 56 | 57 | void BlockWorker::processBlocks(bool serial) 58 | { 59 | if(serial){ 60 | (*this)(tbb::blocked_range(0,_blocks->size())); 61 | }else{ 62 | tbb::parallel_for(tbb::blocked_range(0, _blocks->size()), *this); 63 | } 64 | } 65 | 66 | 67 | void BlockWorker::operator()(const tbb::blocked_range &r) 68 | const{ 69 | assert(_blocks); 70 | assert(_leaves); 71 | LeafT* leaf = new LeafT(); 72 | for(size_t m=r.begin(), end=r.end(); m!=end; ++m){ 73 | Block& block = (*_blocks)[m]; 74 | const openvdb::CoordBBox &bbox = block.bbox; 75 | if((*_leaves)[m] != nullptr){ 76 | combineOccupiedLeafFromBuffer(leaf, (*_leaves)[m], m*_voxelVolume); 77 | }else{ 78 | fillOccupiedLeafFromBuffer(leaf, m*_voxelVolume); 79 | } 80 | leaf->setOrigin(bbox.min()); 81 | block.leaf = leaf; 82 | leaf = new LeafT(); 83 | } 84 | delete leaf; 85 | } 86 | 87 | void BlockWorker::fillOccupiedLeafFromBuffer(LeafT* leaf, int index) 88 | const{ 89 | float value; 90 | auto update = [&log_hit_thres = _logOddsHitThreshold, 91 | &log_miss_thres = _logOddsMissThreshold, 92 | &occ_clamp = _occupiedClampingThreshold, 93 | &empty_clamp = _emptyClampingThreshold, 94 | &temp_value = value] 95 | (float& voxel_value, bool& active) 96 | { 97 | voxel_value = temp_value; 98 | if (voxel_value > occ_clamp){ 99 | voxel_value = occ_clamp; 100 | }else if(voxel_value < empty_clamp){ 101 | voxel_value = empty_clamp; 102 | } 103 | 104 | if(voxel_value > log_hit_thres){ 105 | active = true; 106 | }else if(voxel_value < log_miss_thres){ 107 | active = false; 108 | } 109 | }; 110 | for(int x = 0; x < _nodeEdge ; x++){ 111 | for(int y = 0; y < _nodeEdge ; y++){ 112 | for(int z = 0; z < _nodeEdge ; z++){ 113 | value = (float)((*(_hostVoxelBuffer + index + z + y*_nodeEdge + x*_nodeEdge*_nodeEdge)))/10; 114 | if(value != 0.0){ 115 | leaf->modifyValueAndActiveState(openvdb::Coord(x,y,z), update); 116 | }else{ 117 | leaf->setValueOff(openvdb::Coord(x,y,z), 0.0); 118 | } 119 | } 120 | } 121 | } 122 | } 123 | 124 | void BlockWorker::combineOccupiedLeafFromBuffer(LeafT* leaf, 125 | LeafT* target, 126 | int index) 127 | const{ 128 | float probeValue; 129 | float value; 130 | int linearOffset; 131 | bool targetActive = false; 132 | auto update = [&log_hit_thres = _logOddsHitThreshold, 133 | &log_miss_thres = _logOddsMissThreshold, 134 | &occ_clamp = _occupiedClampingThreshold, 135 | &empty_clamp = _emptyClampingThreshold, 136 | &probe_value = probeValue, 137 | &temp_value = value, 138 | &target_activity = targetActive] 139 | (float& voxel_value, bool& active) 140 | { 141 | voxel_value = probe_value + temp_value; 142 | if (voxel_value > occ_clamp){ 143 | voxel_value = occ_clamp; 144 | }else if(voxel_value < empty_clamp){ 145 | voxel_value = empty_clamp; 146 | } 147 | 148 | if(voxel_value > log_hit_thres){ 149 | active = true; 150 | }else if(voxel_value < log_miss_thres){ 151 | active = false; 152 | } 153 | }; 154 | 155 | auto probe = [&probe_value = probeValue, 156 | &target_activity = targetActive] 157 | (float& voxel_value, bool& active){ 158 | voxel_value = probe_value; 159 | active = target_activity; 160 | }; 161 | 162 | for(int x = 0; x < _nodeEdge ; x++){ 163 | for(int y = 0; y < _nodeEdge ; y++){ 164 | for(int z = 0; z < _nodeEdge ; z++){ 165 | linearOffset = z + y*_nodeEdge + x*_nodeEdge*_nodeEdge; 166 | value = (float)(*(_hostVoxelBuffer + index + linearOffset))/10; 167 | targetActive = target->probeValue(openvdb::Coord(x,y,z), probeValue); 168 | if(value != 0.0){ 169 | leaf->modifyValueAndActiveState(openvdb::Coord(x,y,z), update); 170 | }else{ 171 | leaf->modifyValueAndActiveState(openvdb::Coord(x,y,z), probe); 172 | } 173 | } 174 | } 175 | } 176 | } 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /src/nanomap/handler/Handler.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file Handler.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/handler/Handler.h" 10 | 11 | // The following functions are called by the host and launch the gpu kernels. 12 | 13 | //This kernel generates a point cloud using a simulated sensor view and an existing map 14 | //extern "C" void generateCloud(nanomap::handler::SensorBucket& _sensorBucket, cudaStream_t s0); 15 | namespace nanomap{ 16 | namespace handler{ 17 | Handler::Handler(std::shared_ptr config) 18 | :_config(config) 19 | ,_sensorAllocator(config) 20 | { 21 | _gpuTime = 0.0; 22 | _mapUpdateTime = 0.0; 23 | cudaCheck(cudaStreamCreate(&_s0)); 24 | cudaCheck(cudaStreamCreate(&_s1)); 25 | } 26 | 27 | 28 | void Handler::populateTempGrid(openvdb::FloatGrid::Accessor& tempAcc, int sensorIndex, int pcl_width, int pcl_height, int pcl_step, 29 | unsigned char* cloudPtr, std::shared_ptr map){ 30 | openvdb::math::Ray ray; 31 | openvdb::math::DDA ,0> dda; 32 | nanomap::Pose pose = _config->sensorData(sensorIndex)->sharedParameters()._pose; 33 | openvdb::Vec3d ray_origin_world(pose.position(0), pose.position(1), pose.position(2)); 34 | openvdb::Vec3d ray_origin_index(map->occupiedGrid()->worldToIndex(ray_origin_world)); 35 | openvdb::Vec3d ray_direction; 36 | bool max_range_ray; 37 | openvdb::Vec3d x; 38 | double ray_length; 39 | float max_time = _config->sensorData(sensorIndex)->sharedParameters()._maxRange/map->gridRes(); 40 | float pointx, pointy, pointz; 41 | Eigen::Matrix rotation = pose.orientation.normalized().toRotationMatrix()*_config->sensorData(sensorIndex)->sharedParameters()._frameTransform; 42 | float prob_miss = _config->sensorData(sensorIndex)->sharedParameters()._probMiss; 43 | float prob_hit = _config->sensorData(sensorIndex)->sharedParameters()._probMiss; 44 | float logodds_miss = log(prob_miss)-log(1-prob_miss); 45 | float logodds_hit = log(prob_hit)-log(1-prob_hit); 46 | // Probability update lambda for empty grid elements 47 | auto miss = [&prob_miss = logodds_miss](float& voxel_value, bool& active) { 48 | voxel_value += prob_miss; 49 | active = true; 50 | }; 51 | 52 | // Probability update lambda for occupied grid elements 53 | auto hit = [&prob_hit = logodds_hit](float& voxel_value, bool& active) { 54 | voxel_value += prob_hit; 55 | active = true; 56 | }; 57 | // Raycasting of every point in the input cloud 58 | for (int i = 0; i < pcl_width*pcl_height; i++) 59 | { 60 | unsigned char* byte_ptr = cloudPtr + i*pcl_step; 61 | pointx = *(reinterpret_cast(byte_ptr+0)); 62 | pointy = *(reinterpret_cast(byte_ptr+4)); 63 | pointz = *(reinterpret_cast(byte_ptr+8)); 64 | max_range_ray = false; 65 | ray_direction = openvdb::Vec3d( 66 | rotation(0,0)*pointx+rotation(0,1)*pointy+rotation(0,2)*pointz, 67 | rotation(1,0)*pointx+rotation(1,1)*pointy+rotation(1,2)*pointz, 68 | rotation(2,0)*pointx+rotation(2,1)*pointy+rotation(2,2)*pointz); 69 | 70 | ray_length = ray_direction.length()/map->gridRes(); 71 | if(ray_length < max_time){ 72 | ray_direction.normalize(); 73 | ray.setEye(ray_origin_index); 74 | ray.setDir(ray_direction); 75 | dda.init(ray,0,ray_length); 76 | openvdb::Coord voxel = dda.voxel(); 77 | while(dda.step()){ 78 | tempAcc.modifyValueAndActiveState(voxel, miss); 79 | voxel = dda.voxel(); 80 | } 81 | if(dda.time() map){ 89 | float tempValue; 90 | // Probability update lambda for occupied grid elements 91 | float occClampThres = map->occupiedClampingThreshold(); 92 | float emptyClampThres = map->emptyClampingThreshold(); 93 | float logodds_thres_max = map->logOddsHitThreshold(); 94 | float logodds_thres_min = map->logOddsMissThreshold(); 95 | auto update = [&prob_thres_max = logodds_thres_max, &prob_thres_min = logodds_thres_min, 96 | &occ_clamp = occClampThres, &empty_clamp = emptyClampThres, &temp_value = tempValue] 97 | (float& voxel_value, bool& active) { 98 | voxel_value += temp_value; 99 | if (voxel_value > occ_clamp) 100 | { 101 | voxel_value = occ_clamp; 102 | }else if(voxel_value < empty_clamp){ 103 | voxel_value = empty_clamp; 104 | } 105 | 106 | if(voxel_value > prob_thres_max){ 107 | active = true; 108 | }else if(voxel_value < prob_thres_min){ 109 | active = false; 110 | } 111 | }; 112 | // Integrating the data of the temporary grid into the map using the probability update functions 113 | for (openvdb::FloatGrid::ValueOnCIter iter = tempGrid->cbeginValueOn(); iter; ++iter) 114 | { 115 | tempValue = iter.getValue(); 116 | if (tempValue!=0.0) 117 | { 118 | map->occAccessor()->modifyValueAndActiveState(iter.getCoord(), update); 119 | } 120 | } 121 | return; 122 | } 123 | 124 | void Handler::processPointCloudCPU(int index, int pcl_width, int pcl_height, int pcl_step, unsigned char* cloud, std::shared_ptr map){ 125 | openvdb::FloatGrid::Ptr tempGrid = openvdb::FloatGrid::create(0.0); 126 | //tempGrid->setTransform(openvdb::math::Transform::createLinearTransform(_config->mappingRes())); 127 | auto tempAcc = tempGrid->getAccessor(); 128 | populateTempGrid(tempAcc, index, pcl_width, pcl_height, pcl_step, 129 | cloud, map); 130 | 131 | integrateTempGrid(tempGrid, map); 132 | } 133 | 134 | void Handler::voxelUpdateFromFrustumBuffer(int leafEdge){ 135 | int leafVolume = leafEdge*leafEdge*leafEdge; 136 | float logOddsHit = _agentMap->logOddsHitThreshold(); 137 | float logOddsMiss = _agentMap->logOddsMissThreshold(); 138 | float occClamp = _agentMap->occupiedClampingThreshold(); 139 | float emptyClamp = _agentMap->emptyClampingThreshold(); 140 | float probeValue; 141 | float value; 142 | float targetActive = false; 143 | auto update = [&log_hit_thres = logOddsHit, 144 | &log_miss_thres = logOddsMiss, 145 | &occ_clamp = occClamp, 146 | &empty_clamp = emptyClamp, 147 | &probe_value = probeValue, 148 | &temp_value = value] 149 | (float& voxel_value, bool& active) { 150 | voxel_value += temp_value; 151 | if (voxel_value > occ_clamp){ 152 | voxel_value = occ_clamp; 153 | }else if(voxel_value < empty_clamp){ 154 | voxel_value = empty_clamp; 155 | } 156 | if(voxel_value > log_hit_thres){ 157 | active = true; 158 | }else if(voxel_value < log_miss_thres){ 159 | active = false; 160 | } 161 | }; 162 | int nodeIndex, voxelIndex, voxelBufferIndex; 163 | for(int i = 0; i < *(_sensorAllocator.sensorBucket()->hostFrustumLeafCount()); i++){ 164 | int nodeX = *(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer()+ i*3); 165 | int nodeY = *(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer()+ i*3+1); 166 | int nodeZ = *(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer()+ i*3+2); 167 | for(int x = 0; x < leafEdge; x++){ 168 | for(int y = 0; y < leafEdge ; y++){ 169 | for(int z = 0; z < leafEdge; z++){ 170 | nodeIndex = i*leafVolume; 171 | voxelIndex = z+y*leafEdge + x*leafEdge*leafEdge; 172 | voxelBufferIndex = nodeIndex+voxelIndex; 173 | value = (float)(*(_sensorAllocator.sensorBucket()->hostFrustumVoxelBuffer()+voxelBufferIndex))/100; 174 | if(value != 0.0){ 175 | //std::cout << "new info" << std::endl; 176 | _agentMap->occAccessor()->modifyValueAndActiveState(openvdb::Coord(nodeX+x,nodeY+y,nodeZ+z), update); 177 | } 178 | } 179 | } 180 | } 181 | } 182 | } 183 | 184 | 185 | 186 | void Handler::blockUpdateFromFrustumBuffer(){ 187 | BlockWorker occBlockWorker(_sensorAllocator.sensorBucket()->hostGridData()->leafEdge(), 188 | _agentMap->occupiedClampingThreshold(), 189 | _agentMap->emptyClampingThreshold(), 190 | _agentMap->logOddsHitThreshold(), 191 | _agentMap->logOddsMissThreshold(), 192 | _agentMap->occAccessor(), 193 | _sensorAllocator.sensorBucket()->hostFrustumVoxelBuffer(), 194 | _sensorAllocator.sensorBucket()->hostFrustumLeafBuffer(), 195 | *(_sensorAllocator.sensorBucket()->hostFrustumLeafCount())); 196 | 197 | occBlockWorker.processBlocks(_config->serialUpdate()); 198 | for(int i = 0; i < *(_sensorAllocator.sensorBucket()->hostFrustumLeafCount()); i++){ 199 | BlockWorker::Block& occBlock = (*(occBlockWorker._blocks))[i]; 200 | if (occBlock.leaf) { 201 | _agentMap->occAccessor()->addLeaf(occBlock.leaf); 202 | } 203 | } 204 | occBlockWorker.destroyBlocks(); 205 | } 206 | void Handler::integrateTempGrid(openvdb::FloatGrid::Ptr tempGrid, openvdb::FloatGrid::Ptr Grid, openvdb::FloatGrid::Accessor& acc, float emptyClampThres, float occClampThres, float logodds_thres_min, float logodds_thres_max){ 207 | float tempValue; 208 | // Probability update lambda for occupied grid elements 209 | auto update = [&prob_thres_max = logodds_thres_max, &prob_thres_min = logodds_thres_min, 210 | &occ_clamp = occClampThres, &empty_clamp = emptyClampThres, &temp_value = tempValue](float& voxel_value, 211 | bool& active) { 212 | voxel_value += temp_value; 213 | if (voxel_value > occ_clamp) 214 | { 215 | voxel_value = occ_clamp; 216 | }else if(voxel_value < empty_clamp){ 217 | voxel_value = empty_clamp; 218 | } 219 | 220 | if(voxel_value > prob_thres_max){ 221 | active = true; 222 | }else if(voxel_value < prob_thres_min){ 223 | active = false; 224 | } 225 | }; 226 | // Integrating the data of the temporary grid into the map using the probability update functions 227 | for (openvdb::FloatGrid::ValueOnCIter iter = tempGrid->cbeginValueOn(); iter; ++iter) 228 | { 229 | tempValue = iter.getValue(); 230 | if (tempValue!=0.0) 231 | { 232 | acc.modifyValueAndActiveState(iter.getCoord(), update); 233 | } 234 | } 235 | return; 236 | } 237 | void Handler::processPointCloud(int sensorIndex, int pcl_width, int pcl_height, 238 | int pcl_step, unsigned char* cloud, 239 | std::shared_ptr agentMap){ 240 | 241 | std::chrono::duration delay; 242 | 243 | auto gpu_start = std::chrono::high_resolution_clock::now(); 244 | 245 | _agentMap = agentMap; 246 | auto start = std::chrono::high_resolution_clock::now(); 247 | if(_config->sensorData(sensorIndex)->sharedParameters()._type==1){ 248 | start = std::chrono::high_resolution_clock::now(); 249 | processPointCloudCPU(sensorIndex, pcl_width, pcl_height, pcl_step, cloud, agentMap); 250 | }else if(_config->sensorData(sensorIndex)->sharedParameters()._type==0){ 251 | _sensorAllocator.update(pcl_width, pcl_height, pcl_step, cloud, _config->sensorData(sensorIndex), _s0); 252 | 253 | cudaDeviceSynchronize(); 254 | 255 | filterCloud(*(_sensorAllocator.sensorBucket()), _s0); 256 | 257 | cudaDeviceSynchronize(); 258 | //std::cout << (*_sensorAllocator.sensorBucket()->hostFrustumLeafCount()) << std::endl; 259 | //std::cout << (*_sensorAllocator.sensorBucket()->hostRayCount()) << std::endl; 260 | if(_sensorAllocator.sensorBucket()->hostSensor()->type()==0){ 261 | frustumCastCloud(*(_sensorAllocator.sensorBucket()), _s0, _s1); 262 | cudaCheck(cudaMemcpyAsync(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer(), _sensorAllocator.sensorBucket()->devFrustumLeafBuffer(), (*_sensorAllocator.sensorBucket()->hostFrustumLeafCount())*3*sizeof(int), cudaMemcpyDeviceToHost, _s0)); 263 | cudaCheck(cudaMemcpyAsync(_sensorAllocator.sensorBucket()->hostFrustumVoxelBuffer(), _sensorAllocator.sensorBucket()->devFrustumVoxelBuffer(), (*_sensorAllocator.sensorBucket()->hostFrustumLeafCount())*512*sizeof(int8_t), cudaMemcpyDeviceToHost,_s1)); 264 | } 265 | cudaStreamSynchronize(_s0); 266 | cudaStreamSynchronize(_s1); 267 | auto gpu_end = std::chrono::high_resolution_clock::now(); 268 | delay = gpu_end-gpu_start; 269 | _gpuTime+=delay.count(); 270 | start = std::chrono::high_resolution_clock::now(); 271 | 272 | if(_sensorAllocator.sensorBucket()->hostSensor()->type()==0){ 273 | if(_config->updateType() == 0){ 274 | voxelUpdateFromFrustumBuffer(_config->sensorData(sensorIndex)->sharedParameters()._leafEdge); 275 | }else if(_config->updateType() == 1){ 276 | blockUpdateFromFrustumBuffer(); 277 | } 278 | } 279 | } 280 | auto end = std::chrono::high_resolution_clock::now(); 281 | delay = end-start; 282 | _mapUpdateTime+=delay.count(); 283 | 284 | } 285 | void Handler::closeHandler(){ 286 | cudaCheck(cudaStreamDestroy(_s0)); 287 | cudaCheck(cudaStreamDestroy(_s1)); 288 | } 289 | 290 | void Handler::printUpdateTime(int count){ 291 | std::cout << "gpu update time per loop:" << _gpuTime / count << std::endl; 292 | std::cout << "map update time per loop:" << _mapUpdateTime / count << std::endl; 293 | } 294 | } 295 | } 296 | -------------------------------------------------------------------------------- /src/nanomap/handler/SimHandler.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file SimHandler.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/handler/SimHandler.h" 10 | // The following functions are called by the host and launch the gpu kernels. 11 | namespace nanomap{ 12 | namespace handler{ 13 | SimHandler::SimHandler(std::shared_ptr config, openvdb::FloatGrid::Ptr simGrid) 14 | :_config(config) 15 | ,_simGrid(simGrid) 16 | ,_sensorAllocator(config) 17 | { 18 | _gpuTime = 0.0; 19 | _mapUpdateTime = 0.0; 20 | cudaCheck(cudaStreamCreate(&_s0)); 21 | cudaCheck(cudaStreamCreate(&_s1)); 22 | _simGridHandle = nanovdb::openToNanoVDB(*(_simGrid)); 23 | _simGridHandle.deviceUpload(); 24 | } 25 | 26 | void SimHandler::populateTempGrid(openvdb::FloatGrid::Accessor& tempAcc, 27 | int sensorIndex, 28 | std::shared_ptr map){ 29 | openvdb::math::Ray ray; 30 | openvdb::math::DDA,0> dda; 31 | nanomap::Pose pose = _config->sensorData(sensorIndex)->sharedParameters()._pose; 32 | openvdb::Vec3d ray_origin_world(pose.position(0), pose.position(1), pose.position(2)); 33 | openvdb::Vec3d ray_origin_index(map->occupiedGrid()->worldToIndex(ray_origin_world)); 34 | openvdb::Vec3d ray_direction; 35 | bool max_range_ray; 36 | openvdb::Vec3d x; 37 | double ray_length; 38 | float max_time = _config->sensorData(sensorIndex)->sharedParameters()._maxRange/map->gridRes(); 39 | float pointx, pointy, pointz; 40 | Eigen::Matrix rotation = pose.orientation.normalized().toRotationMatrix()*_config->sensorData(sensorIndex)->sharedParameters()._frameTransform; 41 | float prob_miss = _config->sensorData(sensorIndex)->sharedParameters()._probMiss; 42 | float prob_hit = _config->sensorData(sensorIndex)->sharedParameters()._probHit; 43 | float logodds_miss = log(prob_miss)-log(1-prob_miss); 44 | float logodds_hit = log(prob_hit)-log(1-prob_hit); 45 | // Probability update lambda for empty grid elements 46 | auto miss = [&prob_miss = logodds_miss](float& voxel_value, bool& active) { 47 | voxel_value += prob_miss; 48 | active = true; 49 | }; 50 | 51 | // Probability update lambda for occupied grid elements 52 | auto hit = [&prob_hit = logodds_hit](float& voxel_value, bool& active) { 53 | voxel_value += prob_hit; 54 | active = true; 55 | }; 56 | // Raycasting of every point in the input cloud 57 | for (int i = 0; i < _config->sensorData(sensorIndex)->sharedParameters()._pclSize; i++) 58 | { 59 | pointx = (_config->sensorData(sensorIndex)->pointCloud()).col(i)(0); 60 | pointy = (_config->sensorData(sensorIndex)->pointCloud()).col(i)(1); 61 | pointz = (_config->sensorData(sensorIndex)->pointCloud()).col(i)(2); 62 | if(!(std::isnan(pointz))){ 63 | max_range_ray = false; 64 | ray_direction = openvdb::Vec3d( 65 | rotation(0,0)*pointx+rotation(0,1)*pointy+rotation(0,2)*pointz, 66 | rotation(1,0)*pointx+rotation(1,1)*pointy+rotation(1,2)*pointz, 67 | rotation(2,0)*pointx+rotation(2,1)*pointy+rotation(2,2)*pointz); 68 | 69 | ray_length = ray_direction.length()/map->gridRes(); 70 | if(ray_length < max_time){ 71 | ray_direction.normalize(); 72 | ray.setEye(ray_origin_index); 73 | ray.setDir(ray_direction); 74 | dda.init(ray,0,ray_length); 75 | openvdb::Coord voxel = dda.voxel(); 76 | while(dda.step()){ 77 | tempAcc.modifyValueAndActiveState(voxel, miss); 78 | voxel = dda.voxel(); 79 | } 80 | if(dda.time() map){ 89 | float tempValue; 90 | // Probability update lambda for occupied grid elements 91 | float occClampThres = map->occupiedClampingThreshold(); 92 | float emptyClampThres = map->emptyClampingThreshold(); 93 | float logodds_thres_max = map->logOddsHitThreshold(); 94 | float logodds_thres_min = map->logOddsMissThreshold(); 95 | auto update = [&prob_thres_max = logodds_thres_max, &prob_thres_min = logodds_thres_min, 96 | &occ_clamp = occClampThres, &empty_clamp = emptyClampThres, &temp_value = tempValue] 97 | (float& voxel_value, bool& active) { 98 | voxel_value += temp_value; 99 | 100 | if(voxel_value > prob_thres_max){ 101 | active = true; 102 | if(voxel_value > occ_clamp){ 103 | voxel_value = occ_clamp; 104 | } 105 | }else if(voxel_value < prob_thres_min){ 106 | active = false; 107 | if(voxel_value < empty_clamp){ 108 | voxel_value = empty_clamp; 109 | } 110 | } 111 | }; 112 | // Integrating the data of the temporary grid into the map using the probability update functions 113 | for (openvdb::FloatGrid::ValueOnCIter iter = tempGrid->cbeginValueOn(); iter; ++iter) 114 | { 115 | tempValue = iter.getValue(); 116 | if (tempValue!=0.0) 117 | { 118 | map->occAccessor()->modifyValueAndActiveState(iter.getCoord(), update); 119 | } 120 | } 121 | return; 122 | } 123 | 124 | void SimHandler::processPointCloudCPU(int sensorIndex, std::shared_ptr map){ 125 | openvdb::FloatGrid::Ptr tempGrid = openvdb::FloatGrid::create(0.0); 126 | auto tempAcc = tempGrid->getAccessor(); 127 | populateTempGrid(tempAcc, sensorIndex, map); 128 | integrateTempGrid(tempGrid, map); 129 | } 130 | 131 | void SimHandler::voxelUpdateFromFrustumBuffer(int leafEdge){ 132 | int leafVolume = leafEdge*leafEdge*leafEdge; 133 | float logOddsHit = _agentMap->logOddsHitThreshold(); 134 | float logOddsMiss = _agentMap->logOddsMissThreshold(); 135 | float occClamp = _agentMap->occupiedClampingThreshold(); 136 | float emptyClamp = _agentMap->emptyClampingThreshold(); 137 | float probeValue; 138 | float value; 139 | float targetActive = false; 140 | auto update = [&log_hit_thres = logOddsHit, 141 | &log_miss_thres = logOddsMiss, 142 | &occ_clamp = occClamp, 143 | &empty_clamp = emptyClamp, 144 | &probe_value = probeValue, 145 | &temp_value = value] 146 | (float& voxel_value, bool& active) { 147 | voxel_value += temp_value; 148 | if (voxel_value > occ_clamp){ 149 | voxel_value = occ_clamp; 150 | }else if(voxel_value < empty_clamp){ 151 | voxel_value = empty_clamp; 152 | } 153 | if(voxel_value > log_hit_thres){ 154 | active = true; 155 | }else if(voxel_value < log_miss_thres){ 156 | active = false; 157 | } 158 | }; 159 | int nodeIndex, voxelIndex, voxelBufferIndex; 160 | for(int i = 0; i < *(_sensorAllocator.sensorBucket()->hostFrustumLeafCount()); i++){ 161 | int nodeX = *(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer()+ i*3); 162 | int nodeY = *(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer()+ i*3+1); 163 | int nodeZ = *(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer()+ i*3+2); 164 | 165 | for(int x = 0; x < leafEdge; x++){ 166 | for(int y = 0; y < leafEdge ; y++){ 167 | for(int z = 0; z < leafEdge; z++){ 168 | nodeIndex = i*leafVolume; 169 | voxelIndex = z+y*leafEdge + x*leafEdge*leafEdge; 170 | voxelBufferIndex = nodeIndex+voxelIndex; 171 | value = (float)(*(_sensorAllocator.sensorBucket()->hostFrustumVoxelBuffer()+voxelBufferIndex))/100; 172 | if(value != 0.0){ 173 | _agentMap->occAccessor()->modifyValueAndActiveState(openvdb::Coord(nodeX+x,nodeY+y,nodeZ+z), update); 174 | } 175 | } 176 | } 177 | } 178 | } 179 | } 180 | void SimHandler::blockUpdateFromFrustumBuffer(){ 181 | BlockWorker occBlockWorker(_sensorAllocator.sensorBucket()->hostGridData()->leafEdge(), 182 | _agentMap->occupiedClampingThreshold(), 183 | _agentMap->emptyClampingThreshold(), 184 | _agentMap->logOddsHitThreshold(), 185 | _agentMap->logOddsMissThreshold(), 186 | _agentMap->occAccessor(), 187 | _sensorAllocator.sensorBucket()->hostFrustumVoxelBuffer(), 188 | _sensorAllocator.sensorBucket()->hostFrustumLeafBuffer(), 189 | *(_sensorAllocator.sensorBucket()->hostFrustumLeafCount())); 190 | 191 | occBlockWorker.processBlocks(_config->serialUpdate()); 192 | for(int i = 0; i < *(_sensorAllocator.sensorBucket()->hostFrustumLeafCount()); i++){ 193 | BlockWorker::Block& occBlock = (*(occBlockWorker._blocks))[i]; 194 | if (occBlock.leaf) { 195 | _agentMap->occAccessor()->addLeaf(occBlock.leaf); 196 | } 197 | } 198 | occBlockWorker.destroyBlocks(); 199 | } 200 | 201 | void SimHandler::integrateTempGrid(openvdb::FloatGrid::Ptr tempGrid, openvdb::FloatGrid::Ptr Grid, openvdb::FloatGrid::Accessor& acc, float emptyClampThres, float occClampThres, float logodds_thres_min, float logodds_thres_max){ 202 | float tempValue; 203 | // Probability update lambda for occupied grid elements 204 | auto update = [&prob_thres_max = logodds_thres_max, &prob_thres_min = logodds_thres_min, 205 | &occ_clamp = occClampThres, &empty_clamp = emptyClampThres, &temp_value = tempValue](float& voxel_value, 206 | bool& active) { 207 | voxel_value += temp_value; 208 | if (voxel_value > occ_clamp) 209 | { 210 | voxel_value = occ_clamp; 211 | }else if(voxel_value < empty_clamp){ 212 | voxel_value = empty_clamp; 213 | } 214 | 215 | if(voxel_value > prob_thres_max){ 216 | active = true; 217 | }else if(voxel_value < prob_thres_min){ 218 | active = false; 219 | } 220 | }; 221 | 222 | // Integrating the data of the temporary grid into the map using the probability update functions 223 | for (openvdb::FloatGrid::ValueOnCIter iter = tempGrid->cbeginValueOn(); iter; ++iter) 224 | { 225 | tempValue = iter.getValue(); 226 | if (tempValue!=0.0) 227 | { 228 | acc.modifyValueAndActiveState(iter.getCoord(), update); 229 | } 230 | } 231 | return; 232 | } 233 | 234 | void SimHandler::processPointCloud(int sensorIndex, std::shared_ptr agentMap){ 235 | std::chrono::duration delay; 236 | 237 | auto gpu_start = std::chrono::high_resolution_clock::now(); 238 | 239 | _agentMap = agentMap; 240 | _sensorAllocator.update(_config->sensorData(sensorIndex), _s0); 241 | generateCloud(*(_sensorAllocator.sensorBucket()), _simGridHandle, _s0); 242 | cudaDeviceSynchronize(); 243 | auto start = std::chrono::high_resolution_clock::now(); 244 | if(_config->sensorData(sensorIndex)->sharedParameters()._type == 1){ 245 | _sensorAllocator.downloadCloudToSensor(_config->sensorData(sensorIndex), _s0); 246 | start = std::chrono::high_resolution_clock::now(); 247 | processPointCloudCPU(sensorIndex,agentMap); 248 | }else if(_config->publishSensor() == 1){ 249 | _sensorAllocator.downloadCloudToSensor(_config->sensorData(sensorIndex), _s0); 250 | 251 | } 252 | if(_config->sensorData(sensorIndex)->sharedParameters()._type == 0){ 253 | filterCloud(*(_sensorAllocator.sensorBucket()), _s0); 254 | 255 | cudaDeviceSynchronize(); 256 | if(_sensorAllocator.sensorBucket()->hostSensor()->type()==0){ 257 | frustumCastCloud(*(_sensorAllocator.sensorBucket()), _s0, _s1); 258 | cudaCheck(cudaMemcpyAsync(_sensorAllocator.sensorBucket()->hostFrustumLeafBuffer(), _sensorAllocator.sensorBucket()->devFrustumLeafBuffer(), (*_sensorAllocator.sensorBucket()->hostFrustumLeafCount())*3*sizeof(int), cudaMemcpyDeviceToHost, _s0)); 259 | cudaCheck(cudaMemcpyAsync(_sensorAllocator.sensorBucket()->hostFrustumVoxelBuffer(), _sensorAllocator.sensorBucket()->devFrustumVoxelBuffer(), (*_sensorAllocator.sensorBucket()->hostFrustumLeafCount())*512*sizeof(int8_t), cudaMemcpyDeviceToHost,_s1)); 260 | } 261 | cudaStreamSynchronize(_s0); 262 | cudaStreamSynchronize(_s1); 263 | 264 | auto gpu_end = std::chrono::high_resolution_clock::now(); 265 | delay = gpu_end-gpu_start; 266 | _gpuTime+=delay.count(); 267 | start = std::chrono::high_resolution_clock::now(); 268 | 269 | if(_sensorAllocator.sensorBucket()->hostSensor()->type()==0){ 270 | if(_config->updateType() == 0){ 271 | voxelUpdateFromFrustumBuffer(_config->sensorData(sensorIndex)->sharedParameters()._leafEdge); 272 | }else if(_config->updateType() == 1){ 273 | blockUpdateFromFrustumBuffer(); 274 | } 275 | } 276 | } 277 | auto end = std::chrono::high_resolution_clock::now(); 278 | delay = end-start; 279 | _mapUpdateTime+=delay.count(); 280 | } 281 | void SimHandler::closeHandler(){ 282 | cudaCheck(cudaStreamDestroy(_s0)); 283 | cudaCheck(cudaStreamDestroy(_s1)); 284 | } 285 | 286 | 287 | void SimHandler::printUpdateTime(int count){ 288 | std::cout << "gpu update time per loop:" << _gpuTime / count << std::endl; 289 | std::cout << "map update time per loop:" << _mapUpdateTime / count << std::endl; 290 | 291 | } 292 | } 293 | } 294 | -------------------------------------------------------------------------------- /src/nanomap/kernels/frustumCastCloud.cu: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file frustumCastCloud.cu 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | /// @brief A CUDA kernel that performs frustum based ray casting. 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "nanomap/gpu/NodeWorker.h" 16 | #include "nanomap/gpu/GridData.h" 17 | #include "nanomap/gpu/PointCloud.h" 18 | #include "nanomap/gpu/SensorBucket.h" 19 | #include "nanomap/gpu/Sensor.h" 20 | 21 | 22 | /*****************************************************************************/ 23 | __global__ void clearNodeWorkerKernel(nanomap::gpu::NodeWorker& worker, int* activeIndices, int* activeFlags, int maxBufferSize) 24 | { 25 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 26 | if(x >= maxBufferSize){ 27 | return; 28 | } 29 | //printf("clear %d \n", x); 30 | worker(x) = nanomap::gpu::NodeWorker::Node(0,0); 31 | *(activeIndices+x)=0; 32 | *(activeFlags+x)=0; 33 | } 34 | /*****************************************************************************/ 35 | __global__ void clearVoxelWorkerKernel(float* worker, int size) 36 | { 37 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 38 | if(x >= size){ 39 | return; 40 | } 41 | *(worker+x) = 0.0f; 42 | } 43 | /*****************************************************************************/ 44 | __global__ void activeCount( 45 | nanomap::gpu::NodeWorker& Array, int* activeIndices, int* activeFlags, int maxBufferSize) 46 | { 47 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 48 | if(x >= maxBufferSize){ 49 | return; 50 | } 51 | if(Array(x).active == 1){ 52 | *(activeIndices+x) = x; 53 | *(activeFlags+x) = 1; 54 | } 55 | 56 | } 57 | /*****************************************************************************/ 58 | __host__ void activePartition(int* activeIndices, int* activeFlags, int* activeLeafNodes, int maxBufferSize, int* devCount, cudaStream_t cStream) 59 | { 60 | void *d_temp_storage = NULL; 61 | size_t temp_storage_bytes = 0; 62 | cub::DevicePartition::Flagged(d_temp_storage, temp_storage_bytes, activeIndices, activeFlags, activeLeafNodes, devCount, maxBufferSize, cStream); 63 | // Allocate temporary storage 64 | cudaMallocAsync(&d_temp_storage, temp_storage_bytes, cStream); 65 | // Run selection 66 | cub::DevicePartition::Flagged(d_temp_storage, temp_storage_bytes, activeIndices, activeFlags, activeLeafNodes, devCount, maxBufferSize, cStream); 67 | cudaStreamSynchronize(cStream); 68 | cudaFree(d_temp_storage); 69 | } 70 | /*****************************************************************************/ 71 | __global__ void activeAssignment(nanomap::gpu::NodeWorker& array, int* activeLeafNodes, int* count) 72 | { 73 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 74 | if(x >=*count){ 75 | return; 76 | } 77 | array(*(activeLeafNodes+x)).index = x; 78 | } 79 | /*****************************************************************************/ 80 | __global__ void setCount(nanomap::gpu::NodeWorker& Array, int* count){ 81 | Array.nodeCount() = *count; 82 | } 83 | /*****************************************************************************/ 84 | __global__ void nodePassHDDA( 85 | nanomap::gpu::PointCloud& pclArray, 86 | int* devRayCount, 87 | nanomap::gpu::NodeWorker& nodeArray, 88 | const nanomap::gpu::GridData& gridData, 89 | const nanomap::gpu::Sensor& sensor) 90 | { 91 | using ValueT = float; 92 | using EigenVec = Eigen::Matrix; 93 | using Vec3T = nanovdb::Vec3; 94 | using RayT = nanovdb::Ray; 95 | using HDDA = nanovdb::HDDA; 96 | const int index = blockIdx.x * blockDim.x + threadIdx.x; 97 | //printf("rayDir: %f | %f | %f \n", pclArray(index).x, pclArray(index).y, pclArray(index).z); 98 | if (index >= *devRayCount){ 99 | return; 100 | }else if(pclArray(index).norm <= 0.0 || pclArray(index).norm >= sensor.maxVoxelRange()){ 101 | return; 102 | }else{ 103 | Vec3T rayEye(sensor.leafOriginOffset()(0), 104 | sensor.leafOriginOffset()(1), 105 | sensor.leafOriginOffset()(2)); 106 | 107 | Vec3T rayDir(pclArray(index).x, 108 | pclArray(index).y, 109 | pclArray(index).z); 110 | 111 | 112 | ValueT maxTime = pclArray(index).norm; 113 | 114 | ValueT minTime = 0.0f; 115 | RayT pointRay(rayEye, rayDir); 116 | HDDA hdda; 117 | hdda.init(pointRay, minTime, maxTime, gridData.leafEdge()); 118 | nanovdb::Coord voxel = hdda.voxel(); 119 | minTime = hdda.time(); 120 | int nodeX, nodeY, nodeZ; 121 | int count = 0; 122 | while(hdda.step()){ 123 | nodeX = __float2int_rd((float)(voxel[0])/(float)gridData.leafEdge())-gridData.nodeMin()[0]; 124 | nodeY = __float2int_rd((float)(voxel[1])/(float)gridData.leafEdge())-gridData.nodeMin()[1]; 125 | nodeZ = __float2int_rd((float)(voxel[2])/(float)gridData.leafEdge())-gridData.nodeMin()[2]; 126 | if(!(nodeX < 0 || nodeX >= gridData.nodeDim()[0] || 127 | nodeY < 0 || nodeY >= gridData.nodeDim()[1] || 128 | nodeZ < 0 || nodeZ >= gridData.nodeDim()[2])){ 129 | (nodeArray)(nodeX, nodeY, nodeZ, 130 | gridData.nodeDim()[0], 131 | gridData.nodeDim()[1], 132 | gridData.nodeDim()[2]).active = 1; 133 | } 134 | voxel = hdda.voxel(); 135 | } 136 | nodeX = __float2int_rd((float)(voxel[0])/(float)gridData.leafEdge())-gridData.nodeMin()[0]; 137 | nodeY = __float2int_rd((float)(voxel[1])/(float)gridData.leafEdge())-gridData.nodeMin()[1]; 138 | nodeZ = __float2int_rd((float)(voxel[2])/(float)gridData.leafEdge())-gridData.nodeMin()[2]; 139 | if(!(nodeX < 0 || nodeX >= gridData.nodeDim()[0] || 140 | nodeY < 0 || nodeY >= gridData.nodeDim()[1] || 141 | nodeZ < 0 || nodeZ >= gridData.nodeDim()[2])){ 142 | (nodeArray)(nodeX, nodeY, nodeZ, 143 | gridData.nodeDim()[0], 144 | gridData.nodeDim()[1], 145 | gridData.nodeDim()[2]).active = 1; 146 | //printf("%d | %d | %d \n", nodeX, nodeY, nodeZ); 147 | } 148 | } 149 | } 150 | /*****************************************************************************/ 151 | __global__ void voxelPassHDDA( 152 | nanomap::gpu::PointCloud& pclArray, 153 | int* devRayCount, 154 | nanomap::gpu::NodeWorker& nodeArray, 155 | float* voxelWorker, 156 | const nanomap::gpu::GridData& gridData, 157 | const nanomap::gpu::Sensor& sensor) 158 | { 159 | using ValueT = float; 160 | using EigenVec = Eigen::Matrix; 161 | using Vec3T = nanovdb::Vec3; 162 | using RayT = nanovdb::Ray; 163 | using HDDA = nanovdb::HDDA; 164 | const int i = blockIdx.x * blockDim.x + threadIdx.x; 165 | if (i >= *devRayCount){ 166 | return; 167 | }else if(pclArray(i).norm <= 0.0 ){ 168 | return; 169 | }else{ 170 | Vec3T rayEye(sensor.leafOriginOffset()(0), 171 | sensor.leafOriginOffset()(1), 172 | sensor.leafOriginOffset()(2)); 173 | 174 | Vec3T rayDir(pclArray(i).x, 175 | pclArray(i).y, 176 | pclArray(i).z); 177 | int voxelX = 0; 178 | int voxelY = 0; 179 | int voxelZ = 0; 180 | int nodeVolume = gridData.leafEdge()*gridData.leafEdge()*gridData.leafEdge(); 181 | 182 | ValueT time = 0; 183 | ValueT maxTime = pclArray(i).norm; 184 | RayT pointRay(rayEye, rayDir); 185 | HDDA hddaVoxel; 186 | 187 | 188 | hddaVoxel.init(pointRay,time,maxTime, 1); 189 | nanovdb::Coord voxel = hddaVoxel.voxel(); 190 | int index = 0; 191 | 192 | nanomap::gpu::NodeWorker::Node nodeNode(0,0); 193 | int nodeX, nodeZ, nodeY; 194 | voxel = hddaVoxel.voxel(); 195 | time = hddaVoxel.time(); 196 | while(hddaVoxel.step()){ 197 | nodeX = __float2int_rd((float)(voxel[0])/(float)gridData.leafEdge())-gridData.nodeMin()[0]; 198 | nodeY = __float2int_rd((float)(voxel[1])/(float)gridData.leafEdge())-gridData.nodeMin()[1]; 199 | nodeZ = __float2int_rd((float)(voxel[2])/(float)gridData.leafEdge())-gridData.nodeMin()[2]; 200 | if(!(nodeX < 0 || nodeX >= gridData.nodeDim()[0] || 201 | nodeY < 0 || nodeY >= gridData.nodeDim()[1] || 202 | nodeZ < 0 || nodeZ >= gridData.nodeDim()[2])){ 203 | nodeNode = (nodeArray)( 204 | nodeX, 205 | nodeY, 206 | nodeZ, 207 | gridData.nodeDim()[0], 208 | gridData.nodeDim()[1], 209 | gridData.nodeDim()[2]); 210 | 211 | voxelX = voxel[0]-__float2int_rd((float)(voxel[0])/(float)gridData.leafEdge())*gridData.leafEdge(); 212 | voxelY = voxel[1]-__float2int_rd((float)(voxel[1])/(float)gridData.leafEdge())*gridData.leafEdge(); 213 | voxelZ = voxel[2]-__float2int_rd((float)(voxel[2])/(float)gridData.leafEdge())*gridData.leafEdge(); 214 | if(nodeNode.index>=0 && nodeNode.index= gridData.nodeDim()[0] || 227 | nodeY < 0 || nodeY >= gridData.nodeDim()[1] || 228 | nodeZ < 0 || nodeZ >= gridData.nodeDim()[2])){ 229 | nodeNode = (nodeArray)( 230 | nodeX, 231 | nodeY, 232 | nodeZ, 233 | gridData.nodeDim()[0], 234 | gridData.nodeDim()[1], 235 | gridData.nodeDim()[2]); 236 | voxelX = voxel[0]-__float2int_rd((float)(voxel[0])/(float)gridData.leafEdge())*gridData.leafEdge(); 237 | voxelY = voxel[1]-__float2int_rd((float)(voxel[1])/(float)gridData.leafEdge())*gridData.leafEdge(); 238 | voxelZ = voxel[2]-__float2int_rd((float)(voxel[2])/(float)gridData.leafEdge())*gridData.leafEdge(); 239 | if(nodeNode.index>=0 && nodeNode.index& sensor 254 | ) 255 | { 256 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 257 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 258 | const int z = blockIdx.z * blockDim.z + threadIdx.z; 259 | if(x >= gridData.nodeDim()[0] || y >= gridData.nodeDim()[1] || z >= gridData.nodeDim()[2]) { 260 | return; 261 | } 262 | int index = z + y*gridData.nodeDim()[2] + x*gridData.nodeDim()[2]*gridData.nodeDim()[1]; 263 | if((nodeArray)(index).active == 1){ 264 | *(devNodeBuffer+3*((nodeArray)(index).index)) = (x+gridData.nodeMin()[0])*gridData.leafEdge()+sensor.leafOffset()[0]; 265 | *(devNodeBuffer+3*((nodeArray)(index).index)+1) = (y+gridData.nodeMin()[1])*gridData.leafEdge()+sensor.leafOffset()[1]; 266 | *(devNodeBuffer+3*((nodeArray)(index).index)+2) = (z+gridData.nodeMin()[2])*gridData.leafEdge()+sensor.leafOffset()[2]; 267 | } 268 | } 269 | /*****************************************************************************/ 270 | __global__ void populateVoxelBuffer( 271 | int* devNodeBuffer, 272 | int8_t* devVoxelBuffer, 273 | float* devVoxelWorker, 274 | const nanomap::gpu::GridData& gridData, 275 | int* devCount) 276 | { 277 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 278 | if(x >= *devCount){ 279 | return; 280 | } 281 | int index; 282 | int value = 0; 283 | int voxelVolume = gridData.leafEdge()* gridData.leafEdge()* gridData.leafEdge(); 284 | for(int i = 0; i 127){ 288 | value = 127; 289 | }else if(value < -128){ 290 | value = -128; 291 | } 292 | *(devVoxelBuffer+index) = value; 293 | } 294 | } 295 | /*****************************************************************************/ 296 | __global__ void clearNodeBuffer( 297 | int* devNodeBuffer, 298 | int* devCount 299 | ){ 300 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 301 | if(x >= *devCount){ 302 | return; 303 | } 304 | *(devNodeBuffer+x*3) = 0; 305 | *(devNodeBuffer+x*3+1) = 0; 306 | *(devNodeBuffer+x*3+2) = 0; 307 | } 308 | 309 | /*****************************************************************************/ 310 | // This is called by the host 311 | extern "C" void frustumCastCloud( nanomap::gpu::SensorBucket& sensorBucket, 312 | cudaStream_t s0, 313 | cudaStream_t s1) 314 | { 315 | using ValueT = float; 316 | using EigenVec = Eigen::Matrix; 317 | using Vec3T = nanovdb::Vec3; 318 | auto round = [](int a, int b) { return (a + b - 1) / b; }; 319 | 320 | //Clear All Node Worker Related Arrays using parallel clear 321 | const dim3 rayThreads(512), rayNumBlocks(round(*(sensorBucket.hostRayCount()), rayThreads.x)); 322 | int nodeSize = sensorBucket.hostGridData()->nodeBufferSize(); 323 | //printf("nodeSize = %d", nodeSize); 324 | const dim3 nodeThreads(512), nodeNumBlocks(round(nodeSize, nodeThreads.x)); 325 | clearNodeWorkerKernel<<>>(*(sensorBucket.leafHandle().deviceNodeWorker()), 326 | sensorBucket.activeIndices(), 327 | sensorBucket.activeFlags(), 328 | nodeSize); 329 | //Determine the active nodes using ray casting 330 | nodePassHDDA<<>>(*(sensorBucket.pclHandle().devicePointCloud()), 331 | sensorBucket.devRayCount(), 332 | *(sensorBucket.leafHandle().deviceNodeWorker()), 333 | *(sensorBucket.devGridData()), 334 | *(sensorBucket.devSensor())); 335 | //Count the active number of nodes and assign each node worker an index 336 | activeCount<<>>(*(sensorBucket.leafHandle().deviceNodeWorker()), 337 | sensorBucket.activeIndices(), 338 | sensorBucket.activeFlags(), 339 | nodeSize); 340 | //Partition the worker arrays to remove non active nodes from the resultant node array 341 | activePartition(sensorBucket.activeIndices(), 342 | sensorBucket.activeFlags(), 343 | sensorBucket.activeLeafNodes(), 344 | nodeSize, 345 | sensorBucket.devFrustumLeafCount(), s0); 346 | //Copy the leaf node count to the host. 347 | cudaMemcpy(sensorBucket.hostFrustumLeafCount(), sensorBucket.devFrustumLeafCount(), sizeof(int), cudaMemcpyDeviceToHost); 348 | //Set the leaf node count in the node worker object. 349 | setCount<<<1,1,0, s0>>>(*(sensorBucket.leafHandle().deviceNodeWorker()), sensorBucket.hostFrustumLeafCount()); 350 | //If there are active leaf nodes, then we need to do voxel level raycasting. 351 | if(*(sensorBucket.hostFrustumLeafCount()) > 0){ 352 | const dim3 assignmentThreads(512), assignmentBlocks(round(*(sensorBucket.hostFrustumLeafCount()), assignmentThreads.x)); 353 | activeAssignment<<>>(*(sensorBucket.leafHandle().deviceNodeWorker()), 354 | sensorBucket.activeLeafNodes(), sensorBucket.devFrustumLeafCount()); 355 | 356 | cudaStreamSynchronize(s0); 357 | int voxelCount = (*(sensorBucket.hostFrustumLeafCount()))*sensorBucket.hostGridData()->leafVolume();; 358 | if(voxelCount > 0){ 359 | if(voxelCount > sensorBucket.getFrustumVoxelAllocation()){ 360 | //NEED TO INCREASE ALLOCATION SIZE OF VOXEL ARRAYS 361 | //TO DO 362 | } 363 | //Clear voxel workers 364 | const dim3 voxelThreads(512), voxelNumBlocks(round(voxelCount, voxelThreads.x)); 365 | clearVoxelWorkerKernel<<>>(sensorBucket.devFrustumVoxelWorker(), voxelCount); 366 | //Define node threads and blocks 367 | const dim3 node3DThreads(8,8,8), node3DBlocks(round(sensorBucket.hostGridData()->nodeDim()[0], node3DThreads.x), 368 | round(sensorBucket.hostGridData()->nodeDim()[1], node3DThreads.y), 369 | round(sensorBucket.hostGridData()->nodeDim()[2], node3DThreads.z)); 370 | 371 | populateNodeBuffer<<>>(*(sensorBucket.leafHandle().deviceNodeWorker()), 372 | sensorBucket.devFrustumLeafBuffer(), 373 | *(sensorBucket.devGridData()), 374 | *(sensorBucket.devSensor())); 375 | //Depending on the filter type, 376 | voxelPassHDDA<<>>(*(sensorBucket.pclHandle().devicePointCloud()), 377 | sensorBucket.devRayCount(), 378 | *(sensorBucket.leafHandle().deviceNodeWorker()), 379 | sensorBucket.devFrustumVoxelWorker(), 380 | *(sensorBucket.devGridData()), 381 | *(sensorBucket.devSensor())); 382 | 383 | //Populate the Voxel Level Buffer with the results of the raycast. 384 | const dim3 voxelBufferThreads(512), voxelBufferBlocks(round(*(sensorBucket.hostFrustumLeafCount()), voxelBufferThreads.x)); 385 | populateVoxelBuffer<<>>( 386 | sensorBucket.devFrustumLeafBuffer(), 387 | sensorBucket.devFrustumVoxelBuffer(), 388 | sensorBucket.devFrustumVoxelWorker(), 389 | *(sensorBucket.devGridData()), 390 | sensorBucket.devFrustumLeafCount()); 391 | } 392 | } 393 | } 394 | -------------------------------------------------------------------------------- /src/nanomap/kernels/generateCloud.cu: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file generateCloud.cu 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | /// @brief A CUDA kernel that generates a sensor output from sensor info and a nanoVDB Grid. 9 | 10 | #include 11 | #include 12 | #include // for CUDA memory management 13 | #include // for nanovdb::Ray 14 | #include 15 | #include "nanomap/gpu/PointCloud.h" 16 | #include "nanomap/gpu/Sensor.h" 17 | #include "nanomap/gpu/SensorBucket.h" 18 | 19 | 20 | template 21 | __hostdev__ bool getActiveCrossing(RayT& ray, AccT& acc, CoordT& ijk, typename AccT::ValueType& v, float& t) 22 | { 23 | if (!ray.clip(acc.root().bbox()) || ray.t1() > 1e20) 24 | return false; // clip ray to bbox 25 | ijk = nanovdb::RoundDown(ray.start()); 26 | nanovdb::HDDA hdda(ray, 1); 27 | 28 | while(hdda.step()){ 29 | if(hdda.dim() != acc.getDim(hdda.voxel(),ray)){ 30 | hdda.update(ray, acc.getDim(hdda.voxel(), ray)); 31 | } 32 | if(hdda.dim()>1){ 33 | continue; 34 | } 35 | hdda.update(ray, 1); 36 | if(acc.isActive(hdda.voxel())){ 37 | t = hdda.time(); 38 | return true; 39 | } 40 | } 41 | return false; 42 | } 43 | template 44 | __global__ void viewSimKernel( nanovdb::NanoGrid& grid, 45 | nanomap::gpu::PointCloud& pclArray, 46 | const nanomap::gpu::Sensor& devSensor, 47 | const float gridRes) 48 | { 49 | using ValueT = float; 50 | using Vec3T = nanovdb::Vec3; 51 | using CoordT = nanovdb::Coord; 52 | using RayT = nanovdb::Ray; 53 | using Point = nanomap::gpu::PointCloud::Point; 54 | 55 | const int w = blockIdx.x * blockDim.x + threadIdx.x; 56 | const int h = blockIdx.y * blockDim.y + threadIdx.y; 57 | if (w >= devSensor.hRes() || h >= devSensor.vRes()){ 58 | return; 59 | } 60 | float minTime; 61 | float maxTime; 62 | Vec3T rayEye; 63 | Vec3T rayDir; 64 | Vec3T defaultRay; 65 | devSensor.getRay(w, h, defaultRay, rayEye, rayDir, minTime, maxTime, gridRes); 66 | RayT ray = RayT(rayEye, rayDir); 67 | auto acc = grid.getAccessor(); 68 | CoordT ijk; 69 | float t; 70 | float v0; 71 | pclArray(w+h*devSensor.hRes()) = Point(0.0,0.0,0.0,-1.0,0.0); 72 | 73 | if (getActiveCrossing(ray, acc, ijk, v0, t)) { 74 | if(t>=minTime && t<=maxTime){ 75 | pclArray(w+h*devSensor.hRes()) = Point(defaultRay[0]*(t*gridRes), defaultRay[1]*(t*gridRes), defaultRay[2]*(t*gridRes), 0.0, 0.0); 76 | }else{ 77 | pclArray(w+h*devSensor.hRes()) = Point(defaultRay[0],defaultRay[1],0.0,-1.0,0.0); 78 | } 79 | }else{ 80 | pclArray(w+h*devSensor.hRes()) = Point(defaultRay[0],defaultRay[1],0.0,-1.0,0.0); 81 | } 82 | } 83 | // This is called by the host 84 | extern "C" void generateCloud(nanomap::gpu::SensorBucket& sensorBucket, nanovdb::GridHandle& gridHandle, cudaStream_t s0) 85 | { 86 | auto round = [](int a, int b) { return (a + b - 1) / b; }; 87 | const dim3 threadsPerBlock(8, 8), numBlocks(round(sensorBucket.hostSensor()->hRes(), threadsPerBlock.x), 88 | round(sensorBucket.hostSensor()->vRes(), threadsPerBlock.y)); 89 | auto* deviceGrid = gridHandle.deviceGrid(); // note this cannot be de-referenced since it rays to a memory address on the GPU! 90 | auto* devicePointCloud = sensorBucket.pclHandle().devicePointCloud(); // note this cannot be de-referenced since it rays to a memory address on the GPU! 91 | assert(deviceGrid && devicePointCloud); 92 | viewSimKernel<<>>(*deviceGrid, *devicePointCloud, *(sensorBucket.devSensor()), sensorBucket.getGridRes()); 93 | return; 94 | } 95 | -------------------------------------------------------------------------------- /src/nanomap/mapgen/procedural/CaveGen.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file CaveGen.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/mapgen/procedural/CaveGen.h" 10 | namespace nanomap{ 11 | namespace mapgen{ 12 | 13 | void CaveGen::populateMap(){ 14 | _cave_grid->setTransform(openvdb::math::Transform::createLinearTransform(_grid_res)); 15 | _cave_grid->setGridClass(openvdb::GRID_LEVEL_SET); 16 | auto _cave_acc = _cave_grid->getAccessor(); 17 | int x,y,z; 18 | _map_dimensions[0] = std::ceil(_map_size[0]/_grid_res); 19 | _map_dimensions[1] = std::ceil(_map_size[1]/_grid_res); 20 | _map_dimensions[2] = std::ceil(_map_size[2]/_grid_res); 21 | std::cout << "Map_dimensions: " << _map_dimensions[0] << "," 22 | << _map_dimensions[1] << "," 23 | << _map_dimensions[2] << std::endl; 24 | for(x = 0; x < 3; x++){ 25 | if((_map_dimensions[x]%2)!= 0){ 26 | _map_dimensions[x] = (_map_dimensions[x]+1)/2; 27 | }else{ 28 | _map_dimensions[x] = _map_dimensions[x]/2; 29 | } 30 | } 31 | int fill; 32 | std::cout << "Performing Fill, Fill Percentage: "<<_fill_percentage << std::endl; 33 | for(x=-_map_dimensions[0]+LEAF_SIZE;x<_map_dimensions[0]-LEAF_SIZE;x+=LEAF_SIZE){ 34 | //std::cout << "Percentage Complete: " << ((double(x)+double(_map_dimensions[0]))/(double(_map_dimensions[0]*2)))*100 << std::endl; 35 | for(y=-_map_dimensions[1]+LEAF_SIZE;y<_map_dimensions[1]-LEAF_SIZE;y+=LEAF_SIZE){ 36 | for(z=-_map_dimensions[2]+LEAF_SIZE;z<_map_dimensions[2]-LEAF_SIZE;z+=LEAF_SIZE){ 37 | fill = rand()%100; 38 | if(fill<_fill_percentage){ 39 | _cave_acc.setValueOn(openvdb::Coord(x,y,z), 1.0); 40 | } 41 | } 42 | } 43 | } 44 | //for(z=-_map_dimensions[2];z<_map_dimensions[2]-LEAF_SIZE;z+=LEAF_SIZE){ 45 | //openvdb::io::File("caveTestGrid.vdb").write({_cave_grid}); 46 | } 47 | 48 | void CaveGen::populateMap2D(){ 49 | _cave_grid->setTransform(openvdb::math::Transform::createLinearTransform(_grid_res)); 50 | _cave_grid->setGridClass(openvdb::GRID_LEVEL_SET); 51 | auto _cave_acc = _cave_grid->getAccessor(); 52 | int x,y,z; 53 | _map_dimensions[0] = std::ceil(_map_size[0]/_grid_res); 54 | _map_dimensions[1] = std::ceil(_map_size[1]/_grid_res); 55 | _map_dimensions[2] = std::ceil(_map_size[2]/_grid_res); 56 | std::cout << "Map_dimensions: " << _map_dimensions[0] << "," 57 | << _map_dimensions[1] << "," 58 | << _map_dimensions[2] << std::endl; 59 | for(x = 0; x < 3; x++){ 60 | if((_map_dimensions[x]%2)!= 0){ 61 | _map_dimensions[x] = (_map_dimensions[x]+1)/2; 62 | }else{ 63 | _map_dimensions[x] = _map_dimensions[x]/2; 64 | } 65 | } 66 | int fill; 67 | std::cout << "Performing Fill, Fill Percentage: "<<_fill_percentage << std::endl; 68 | for(x=-_map_dimensions[0]+LEAF_SIZE;x<_map_dimensions[0]-LEAF_SIZE;x+=LEAF_SIZE){ 69 | //std::cout << "Percentage Complete: " << ((double(x)+double(_map_dimensions[0]))/(double(_map_dimensions[0]*2)))*100 << std::endl; 70 | for(y=-_map_dimensions[1]+LEAF_SIZE;y<_map_dimensions[1]-LEAF_SIZE;y+=LEAF_SIZE){ 71 | // 72 | fill = rand()%100; 73 | 74 | if(fill<_fill_percentage){ 75 | for(z=-_map_dimensions[2];z<_map_dimensions[2];z+=LEAF_SIZE){ 76 | _cave_acc.setValueOn(openvdb::Coord(x,y,z), 1.0); 77 | } 78 | } 79 | //} 80 | } 81 | } 82 | //for(z=-_map_dimensions[2];z<_map_dimensions[2]-LEAF_SIZE;z+=LEAF_SIZE){ 83 | //openvdb::io::File("caveTestGrid.vdb").write({_cave_grid}); 84 | } 85 | 86 | void CaveGen::setFloorAndCeil(){ 87 | auto _cave_acc = _cave_grid->getAccessor(); 88 | int x,y,z; 89 | for(x=-_map_dimensions[0]+LEAF_SIZE;x<_map_dimensions[0]-LEAF_SIZE;x+=LEAF_SIZE){ 90 | //std::cout << "Percentage Complete: " << ((double(x)+double(_map_dimensions[0]))/(double(_map_dimensions[0]*2)))*100 << std::endl; 91 | for(y=-_map_dimensions[1]+LEAF_SIZE;y<_map_dimensions[1]-LEAF_SIZE;y+=LEAF_SIZE){ 92 | _cave_acc.setValueOn(openvdb::Coord(x,y,-_map_dimensions[2]), 1.0); 93 | _cave_acc.setValueOn(openvdb::Coord(x,y,_map_dimensions[2]), 1.0); 94 | } 95 | } 96 | } 97 | void CaveGen::smoothMap(){ 98 | openvdb::FloatGrid::Ptr _temp_grid = openvdb::FloatGrid::create(0.0); 99 | openvdb::FloatGrid::Accessor _temp_acc = _temp_grid->getAccessor(); 100 | auto _cave_acc = _cave_grid->getAccessor(); 101 | std::cout << "Starting Map Smooth" << std::endl; 102 | int i,x,y,z; 103 | int active_count; 104 | for(i=0;i<_smoothing_count;i++){ 105 | std::cout << "Smooth Iteration: " << i << std::endl; 106 | for(x=-_map_dimensions[0]+LEAF_SIZE;x<_map_dimensions[0]-LEAF_SIZE;x+=LEAF_SIZE){ 107 | //std::cout << "Percentage Complete: " << ((double(x)+double(_map_dimensions[0]))/(double(_map_dimensions[0]*2)))*100 << std::endl; 108 | for(y=-_map_dimensions[1]+LEAF_SIZE;y<_map_dimensions[1]-LEAF_SIZE;y+=LEAF_SIZE){ 109 | for(z=-_map_dimensions[2];z<_map_dimensions[2];z+=LEAF_SIZE){ 110 | //std::cout << active_count; 111 | active_count = getActiveNeighbourCount(x,y,z); 112 | //std::cout << active_count<_active_max){ 114 | _temp_acc.setValueOn(openvdb::Coord(x,y,z), 1.0); 115 | }else if(active_count<_active_min){ 116 | _temp_acc.setValueOff(openvdb::Coord(x,y,z),0.0); 117 | }else{ 118 | if(_cave_acc.isValueOn(openvdb::Coord(x, y,z))){ 119 | _temp_acc.setValueOn(openvdb::Coord(x,y,z), 1.0); 120 | }else{ 121 | _temp_acc.setValueOff(openvdb::Coord(x,y,z),0.0); 122 | } 123 | } 124 | } 125 | } 126 | } 127 | _cave_grid = _temp_grid->deepCopy(); 128 | } 129 | } 130 | void CaveGen::smoothMapXY(){ 131 | openvdb::FloatGrid::Ptr _temp_grid = openvdb::FloatGrid::create(0.0); 132 | openvdb::FloatGrid::Accessor _temp_acc = _temp_grid->getAccessor(); 133 | openvdb::FloatGrid::Accessor _cave_acc = _cave_grid->getAccessor();; 134 | std::cout << "Starting Map Smooth" << std::endl; 135 | int i,x,y,z; 136 | int active_count=0; 137 | for(i=0;i<_smoothing_count;i++){ 138 | _cave_acc = _cave_grid->getAccessor(); 139 | std::cout << "Smooth Iteration: " << i << std::endl; 140 | for(x=-_map_dimensions[0]+LEAF_SIZE;x<_map_dimensions[0]-LEAF_SIZE;x+=LEAF_SIZE){ 141 | //std::cout << "Percentage Complete: " << ((double(x)+double(_map_dimensions[0]))/(double(_map_dimensions[0]*2)))*100 << std::endl; 142 | for(y=-_map_dimensions[1]+LEAF_SIZE;y<_map_dimensions[1]-LEAF_SIZE;y+=LEAF_SIZE){ 143 | //for(z=-_map_dimensions[2]+LEAF_SIZE;z<_map_dimensions[2]-LEAF_SIZE;z+=LEAF_SIZE){ 144 | //std::cout << active_count; 145 | 146 | active_count = get2DNeighbourCount(x,y); 147 | std::cout << active_count<_active_max){ 149 | //std::cout << "c" << std::endl; 150 | for(z=-_map_dimensions[2];z<_map_dimensions[2];z+=LEAF_SIZE){ 151 | _temp_acc.setValueOn(openvdb::Coord(x,y,z), 1.0); 152 | } 153 | }else if(active_count<_active_min){ 154 | //std::cout << "f" << std::endl; 155 | for(z=-_map_dimensions[2];z<_map_dimensions[2];z+=LEAF_SIZE){ 156 | _temp_acc.setValueOff(openvdb::Coord(x,y,z), 1.0); 157 | } 158 | }else{ 159 | //std::cout << "l" << std::endl; 160 | if(_cave_acc.isValueOn(openvdb::Coord(x, y,-_map_dimensions[2]))){ 161 | //std::cout << "c" << std::endl; 162 | for(z=-_map_dimensions[2];z<_map_dimensions[2];z+=LEAF_SIZE){ 163 | _temp_acc.setValueOn(openvdb::Coord(x,y,z), 1.0); 164 | } 165 | }else{ 166 | //std::cout << "l" << std::endl; 167 | for(z=-_map_dimensions[2];z<_map_dimensions[2];z+=LEAF_SIZE){ 168 | _temp_acc.setValueOff(openvdb::Coord(x,y,z), 1.0); 169 | } 170 | } 171 | } 172 | //} 173 | } 174 | } 175 | _cave_grid = _temp_grid->deepCopy(); 176 | } 177 | } 178 | 179 | void CaveGen::getRegions(){ 180 | std::cout << "Identifying Regions" << std::endl; 181 | std::vector current_region_queue; 182 | std::set current_region_coords; 183 | std::set all_region_coords; 184 | openvdb::Coord coord; 185 | openvdb::Coord current_coord; 186 | std::set::iterator it; 187 | auto _cave_acc = _cave_grid->getAccessor(); 188 | int x,y,z; 189 | for (openvdb::FloatGrid::ValueOnCIter iter = _cave_grid->cbeginValueOn(); iter.test(); ++iter) { 190 | coord = iter.getCoord(); 191 | it = all_region_coords.find(coord); 192 | if(it == all_region_coords.end()){ 193 | current_region_coords.clear(); 194 | //IF ALL REGION COORDS DOESN'T CONTAIN COORD, ADD TO QUEUE 195 | current_region_coords.insert(coord); 196 | all_region_coords.insert(coord); 197 | current_region_queue.push_back(coord); 198 | while(current_region_queue.size()>0){ 199 | current_coord = current_region_queue.back(); 200 | current_region_queue.pop_back(); 201 | for(x=-LEAF_SIZE; x<=LEAF_SIZE; x+=LEAF_SIZE*2){ 202 | coord.reset(current_coord.x()+x, current_coord.y(), current_coord.z()); 203 | if(_cave_acc.isValueOn(coord)){ 204 | it = current_region_coords.find(coord); 205 | if(it == current_region_coords.end()){ 206 | current_region_queue.push_back(coord); 207 | current_region_coords.insert(coord); 208 | all_region_coords.insert(coord); 209 | } 210 | } 211 | } 212 | for(y=-LEAF_SIZE; y<=LEAF_SIZE; y+=LEAF_SIZE*2){ 213 | coord.reset(current_coord.x(), current_coord.y()+y, current_coord.z()); 214 | if(_cave_acc.isValueOn(coord)){ 215 | it = current_region_coords.find(coord); 216 | if(it == current_region_coords.end()){ 217 | current_region_queue.push_back(coord); 218 | current_region_coords.insert(coord); 219 | all_region_coords.insert(coord); 220 | } 221 | } 222 | } 223 | for(z=-LEAF_SIZE; z<=LEAF_SIZE; z+=LEAF_SIZE*2){ 224 | coord.reset(current_coord.x(), current_coord.y(), current_coord.z()+z); 225 | if(_cave_acc.isValueOn(coord)){ 226 | it = current_region_coords.find(coord); 227 | if(it == current_region_coords.end()){ 228 | current_region_queue.push_back(coord); 229 | current_region_coords.insert(coord); 230 | all_region_coords.insert(coord); 231 | } 232 | } 233 | } 234 | } 235 | } 236 | _regions.push_back(current_region_coords); 237 | } 238 | int max_size=0; 239 | _region_index = 0; 240 | for(auto it = _regions.begin(); it != _regions.end(); it++){ 241 | if(max_sizesize()){ 242 | max_size = it->size(); 243 | _region_index = it-_regions.begin(); 244 | } 245 | } 246 | } 247 | 248 | void CaveGen::keepLargestRegion(){ 249 | std::cout << "Extracting Largest Region" << std::endl; 250 | openvdb::FloatGrid::Ptr _temp_grid = openvdb::FloatGrid::create(0.0); 251 | openvdb::FloatGrid::Accessor _temp_acc = _temp_grid->getAccessor(); 252 | _temp_grid->setTransform(openvdb::math::Transform::createLinearTransform(_grid_res)); 253 | _temp_grid->setGridClass(openvdb::GRID_LEVEL_SET); 254 | for(auto it = _regions[_region_index].begin(); it!= _regions[_region_index].end(); it++){ 255 | _temp_acc.setValueOn(*it,1.0); 256 | } 257 | _cave_grid = _temp_grid->deepCopy(); 258 | } 259 | 260 | void CaveGen::addNoise(){ 261 | 262 | } 263 | int CaveGen::getActiveNeighbourCount(int coord_x, int coord_y, int coord_z){ 264 | int x,y,z; 265 | int active_neighbour_count=0; 266 | auto _cave_acc = _cave_grid -> getAccessor(); 267 | for(x=-LEAF_SIZE;x<=LEAF_SIZE;x+=LEAF_SIZE){ 268 | for(y=-LEAF_SIZE;y<=LEAF_SIZE;y+=LEAF_SIZE){ 269 | for(z=-LEAF_SIZE;z<=LEAF_SIZE;z+=LEAF_SIZE){ 270 | if(!(x==0 && y==0 && z==0)){ 271 | if(_cave_acc.isValueOn(openvdb::Coord(coord_x+x, coord_y+y,coord_z+z))){ 272 | active_neighbour_count+=1; 273 | } 274 | } 275 | } 276 | } 277 | } 278 | return active_neighbour_count; 279 | } 280 | 281 | int CaveGen::get2DNeighbourCount(int coord_x, int coord_y){ 282 | int x,y,z; 283 | int active_neighbour_count=0; 284 | auto _cave_acc = _cave_grid -> getAccessor(); 285 | for(x=-LEAF_SIZE;x<=LEAF_SIZE;x+=LEAF_SIZE){ 286 | for(y=-LEAF_SIZE;y<=LEAF_SIZE;y+=LEAF_SIZE){ 287 | if(!(x==0 && y==0)){ 288 | std::cout << coord_x+x << std::endl; 289 | if(_cave_acc.isValueOn(openvdb::Coord(coord_x+x, coord_y+y,-_map_dimensions[2]))){ 290 | active_neighbour_count+=1; 291 | //std::cout << active_neighbour_count << std::endl; 292 | } 293 | } 294 | } 295 | } 296 | return active_neighbour_count; 297 | } 298 | 299 | void CaveGen::invertAndSave(){ 300 | std::cout << "Starting Invert and Save" << std::endl; 301 | int x,y,z; 302 | int sum; 303 | auto _cave_acc = _cave_grid->getAccessor(); 304 | for (openvdb::FloatGrid::ValueOnIter iter = _cave_grid->beginValueOn(); iter.test(); ++iter) { 305 | sum+=1; 306 | _cave_acc.addTile(1, openvdb::Coord(iter.getCoord()), 1.0 ,true); 307 | } 308 | std::cout << "Number of nodes: "<< sum << std::endl; 309 | openvdb::FloatGrid::Ptr _save_grid; 310 | _save_grid = openvdb::FloatGrid::create(0.0); 311 | //_save_grid->setTransform(openvdb::math::Transform::createLinearTransform(_grid_res)); 312 | _save_grid->setGridClass(openvdb::GRID_LEVEL_SET); 313 | 314 | auto tree = _cave_acc.getTree(); 315 | std::cout << "Voxelizing Active Tiles" << std::endl; 316 | tree->voxelizeActiveTiles(); 317 | //std::cout << "Eroding..." << std::endl; 318 | //openvdb::tools::erodeActiveValues(*tree, 2); 319 | //openvdb::tools::dilateActiveValues(*tree, 2, openvdb::tools::NN_FACE,openvdb::tools::EXPAND_TILES); 320 | std::cout << "Shelling..." << std::endl; 321 | _save_grid = openvdb::tools::topologyToLevelSet(*_cave_grid); 322 | std::cout << "Saving..." << std::endl; 323 | _save_grid->setName("grid"); 324 | openvdb::io::File(_file_out).write({_save_grid}); 325 | } 326 | 327 | void CaveGen::Save(){ 328 | int x,y,z; 329 | auto _cave_acc = _cave_grid->getAccessor(); 330 | int sum; 331 | for (openvdb::FloatGrid::ValueOnIter iter = _cave_grid->beginValueOn(); iter.test(); ++iter) { 332 | sum+=1; 333 | _cave_acc.addTile(1, openvdb::Coord(iter.getCoord()), 1.0 ,true); 334 | } 335 | _cave_grid->tree().voxelizeActiveTiles(); 336 | openvdb::io::File(_file_out).write({_cave_grid}); 337 | } 338 | 339 | void CaveGen::gen(){ 340 | populateMap(); 341 | smoothMap(); 342 | getRegions(); 343 | invertAndSave(); 344 | } 345 | 346 | 347 | void CaveGen::loadConfig(std::string file_in){ 348 | std::ifstream *input = new std::ifstream(file_in.c_str(), std::ios::in | std::ios::binary); 349 | std::string line; 350 | bool done = false; 351 | std::string seed; 352 | int random_seed; 353 | *input >> line; 354 | if (line.compare("#cavegenconfig") != 0) { 355 | std::cout << "Error: first line reads [" << line << "] instead of [#cavegenconfig]" << std::endl; 356 | delete input; 357 | return; 358 | } 359 | while(input->good() && !done) { 360 | *input >> line; 361 | if (line.compare("file_out") == 0){ 362 | *input >> _file_out; 363 | } 364 | else if (line.compare("map_size") == 0){ 365 | *input >> _map_size[0] >> _map_size[1] >> _map_size[2]; 366 | } 367 | else if (line.compare("grid_res")==0){ 368 | *input >> _grid_res; 369 | } 370 | else if (line.compare("fill_percentage")==0){ 371 | *input >> _fill_percentage; 372 | } 373 | else if (line.compare("smoothing_count")==0){ 374 | *input >> _smoothing_count; 375 | } 376 | else if (line.compare("active")==0){ 377 | *input >> _active_max >> _active_min; 378 | } 379 | else if (line.compare("random_seed")==0){ 380 | *input >> random_seed; 381 | if(random_seed ==1){ 382 | _use_rand_seed = true; 383 | }else{ 384 | _use_rand_seed = false; 385 | } 386 | } 387 | else if (line.compare("seed")==0){ 388 | *input >> seed; 389 | done = true; 390 | } 391 | if(!_use_rand_seed){ 392 | std::hash string_hash; 393 | _seed = string_hash(seed); 394 | }else{ 395 | std::hash time_hash; 396 | _seed = time_hash(time(0)); 397 | } 398 | 399 | } 400 | 401 | std::cout << "input_read" << std::endl; 402 | input->close(); 403 | } 404 | }//namespace mapgen 405 | }//namespace nanomap 406 | -------------------------------------------------------------------------------- /src/nanomap/test/test2DCaveGen.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file test2DCaveGen.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/mapgen/procedural/CaveGen.h" 10 | 11 | int main(int argc, char **argv){ 12 | std::string config_file; 13 | config_file = argv[1]; 14 | nanomap::mapgen::CaveGen generator(config_file); 15 | generator.populateMap2D(); 16 | generator.smoothMapXY(); 17 | //generator.setFloorAndCeil(); 18 | generator.getRegions(); 19 | generator.keepLargestRegion(); 20 | generator.invertAndSave(); 21 | } 22 | -------------------------------------------------------------------------------- /src/nanomap/test/test3DCaveGen.cpp: -------------------------------------------------------------------------------- 1 | // Nanomap Copyright 2 | // SPDX-License-Identifier: GPLv3 3 | 4 | /// @file test3DCaveGen.cpp 5 | /// 6 | /// @author Violet Walker 7 | /// 8 | 9 | #include "nanomap/mapgen/procedural/CaveGen.h" 10 | 11 | int main(int argc, char **argv){ 12 | std::string config_file; 13 | config_file = argv[1]; 14 | nanomap::mapgen::CaveGen generator(config_file); 15 | generator.populateMap(); 16 | generator.smoothMap(); 17 | generator.setFloorAndCeil(); 18 | generator.getRegions(); 19 | generator.keepLargestRegion(); 20 | generator.invertAndSave(); 21 | } 22 | --------------------------------------------------------------------------------