Prob voxelworks

packages/gpu_voxels/src/examples/CMakeLists.txt

@@ -189,6 +189,68 @@ ICMAKER_EXTERNAL_DEPENDENCIES(
 
 ICMAKER_BUILD_PROGRAM()
 
+#------------- Example Program to deal with probabilistic voxels ------------
+ICMAKER_SET("prob_voxels" IDE_FOLDER ${EXAMPLES_IDE_FOLDER})
+
+ICMAKER_ADD_HEADERS(
+  )
+
+ICMAKER_ADD_SOURCES(
+  ProbVoxels.cpp
+  )
+
+ICMAKER_ADD_CUDA_FILES(
+  )
+
+ICMAKER_LOCAL_CPPDEFINES(-DGPU_VOXELS_EXPORT_SYMBOLS -Wno-unknown-pragmas)
+ICMAKER_GLOBAL_CPPDEFINES(-D_IC_BUILDER_GPU_VOXELS_EXAMPLES_VOXELMAP_SANDBOX_)
+ICMAKER_INCLUDE_DIRECTORIES(${GPU_VOXELS_INCLUDE_DIRS})
+
+ICMAKER_INTERNAL_DEPENDENCIES(
+  icl_core
+  icl_core_config
+  icl_core_logging
+  gpu_voxels
+  Boost_SYSTEM
+  )
+
+ICMAKER_EXTERNAL_DEPENDENCIES(
+  CUDA
+  )
+
+ICMAKER_BUILD_PROGRAM()
+
+
+#------------- Example Program to create a distance map from a prob map ------------
+ICMAKER_SET("prob2distmap" IDE_FOLDER ${EXAMPLES_IDE_FOLDER})
+
+ICMAKER_ADD_HEADERS(
+  )
+
+ICMAKER_ADD_SOURCES(
+  ProbMap2DistMap.cpp
+  )
+
+ICMAKER_ADD_CUDA_FILES(
+  )
+
+ICMAKER_LOCAL_CPPDEFINES(-DGPU_VOXELS_EXPORT_SYMBOLS -Wno-unknown-pragmas)
+ICMAKER_GLOBAL_CPPDEFINES(-D_IC_BUILDER_GPU_VOXELS_EXAMPLES_VOXELMAP_SANDBOX_)
+ICMAKER_INCLUDE_DIRECTORIES(${GPU_VOXELS_INCLUDE_DIRS})
+
+ICMAKER_INTERNAL_DEPENDENCIES(
+  icl_core
+  icl_core_config
+  icl_core_logging
+  gpu_voxels
+  Boost_SYSTEM
+  )
+
+ICMAKER_EXTERNAL_DEPENDENCIES(
+  CUDA
+  )
+
+ICMAKER_BUILD_PROGRAM()
 
 #------------- Example Program about loading URDFs ------------
 ICMAKER_SET("urdf_loader" IDE_FOLDER ${EXAMPLES_IDE_FOLDER})

packages/gpu_voxels/src/examples/ProbMap2DistMap.cpp

@@ -0,0 +1,98 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// This file is part of the GPU Voxels Software Library.
+//
+// This program is free software licensed under the CDDL
+// (COMMON DEVELOPMENT AND DISTRIBUTION LICENSE Version 1.0).
+// You can find a copy of this license in LICENSE.txt in the top
+// directory of the source code.
+//
+// © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
+//
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+*
+* \author  Andreas Hermann <hermann@fzi.de>
+* \date    2018-03-13
+*
+* This program demonstrates how to insert sensor data into a probabilistic map,
+* while using raycasting to mark the free space.
+*
+*/
+//----------------------------------------------------------------------
+#include <cstdlib>
+#include <signal.h>
+
+#include <gpu_voxels/GpuVoxels.h>
+#include <gpu_voxels/helpers/GeometryGeneration.h>
+#include <gpu_voxels/logging/logging_gpu_voxels.h>
+
+using namespace gpu_voxels;
+using namespace voxelmap;
+using namespace geometry_generation;
+
+GpuVoxelsSharedPtr gvl;
+
+void ctrlchandler(int)
+{
+  gvl.reset();
+  exit(EXIT_SUCCESS);
+}
+void killhandler(int)
+{
+  gvl.reset();
+  exit(EXIT_SUCCESS);
+}
+
+int main(int argc, char* argv[])
+{
+
+  uint32_t m1 = 1;
+  uint32_t m2 = 1;
+  uint32_t m3 = 4;
+
+  signal(SIGINT, ctrlchandler);
+  signal(SIGTERM, killhandler);
+
+  icl_core::logging::initialize(argc, argv);
+
+  gvl = GpuVoxels::getInstance();
+
+  Vector3ui dim(128, 128, 128);
+  float side_length = 1.0; // voxel side length
+  gvl->initialize(dim.x, dim.y, dim.z, side_length);
+
+  gvl->addMap(MT_PROBAB_VOXELMAP, "myProbVoxelMap");
+  gvl->addMap(MT_DISTANCE_VOXELMAP, "myDistanceMap");
+  boost::shared_ptr<ProbVoxelMap> prob_map(gvl->getMap("myProbVoxelMap")->as<ProbVoxelMap>());
+  boost::shared_ptr<DistanceVoxelMap> dist_map(gvl->getMap("myDistanceMap")->as<DistanceVoxelMap>());
+
+  std::vector<Vector3f> this_testpoints1;
+  std::vector<Vector3f> this_testpoints2;
+
+  // create two partly overlapping cubes
+  this_testpoints1 = createBoxOfPoints( Vector3f(40, 40, 40), Vector3f(60, 60, 60), 0.9);
+  this_testpoints2 = createBoxOfPoints( Vector3f(50, 50, 50), Vector3f(70, 70, 70), 0.9);
+
+  // insert them as free and occupied space.
+  prob_map->insertPointCloud(this_testpoints1, eBVM_MAX_OCC_PROB);  // Prob = 0.9843 due to available encoding precision
+  prob_map->insertPointCloud(this_testpoints2, eBVM_MAX_FREE_PROB); // Prob = 0.0157 due to available encoding precision
+  // the overlapping section of the cube will have a probability of 0.5 for occupancy.
+
+  // but we generat the distance map only from voxels with an occupancy higher than 0.55, so this section gets not converted
+  dist_map->mergeOccupied(prob_map, Vector3ui(0), 0.55);
+
+  dist_map->parallelBanding3D(m1, m2, m3, PBA_DEFAULT_M1_BLOCK_SIZE, PBA_DEFAULT_M2_BLOCK_SIZE, PBA_DEFAULT_M3_BLOCK_SIZE, 1);
+
+  while(true)
+  {
+    gvl->visualizeMap("myProbVoxelMap");
+    gvl->visualizeMap("myDistanceMap");
+    sleep(1);
+  }
+
+  return 0;
+}

packages/gpu_voxels/src/examples/ProbMap2DistMapExample.cpp

@@ -0,0 +1,98 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// This file is part of the GPU Voxels Software Library.
+//
+// This program is free software licensed under the CDDL
+// (COMMON DEVELOPMENT AND DISTRIBUTION LICENSE Version 1.0).
+// You can find a copy of this license in LICENSE.txt in the top
+// directory of the source code.
+//
+// © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
+//
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+*
+* \author  Andreas Hermann <hermann@fzi.de>
+* \date    2018-03-13
+*
+* This program demonstrates how to insert sensor data into a probabilistic map,
+* while using raycasting to mark the free space.
+*
+*/
+//----------------------------------------------------------------------
+#include <cstdlib>
+#include <signal.h>
+
+#include <gpu_voxels/GpuVoxels.h>
+#include <gpu_voxels/helpers/GeometryGeneration.h>
+#include <gpu_voxels/logging/logging_gpu_voxels.h>
+
+using namespace gpu_voxels;
+using namespace voxelmap;
+using namespace geometry_generation;
+
+GpuVoxelsSharedPtr gvl;
+
+void ctrlchandler(int)
+{
+  gvl.reset();
+  exit(EXIT_SUCCESS);
+}
+void killhandler(int)
+{
+  gvl.reset();
+  exit(EXIT_SUCCESS);
+}
+
+int main(int argc, char* argv[])
+{
+
+  uint32_t m1 = 1;
+  uint32_t m2 = 1;
+  uint32_t m3 = 4;
+
+  signal(SIGINT, ctrlchandler);
+  signal(SIGTERM, killhandler);
+
+  icl_core::logging::initialize(argc, argv);
+
+  gvl = GpuVoxels::getInstance();
+
+  Vector3ui dim(128, 128, 128);
+  float side_length = 1.0; // voxel side length
+  gvl->initialize(dim.x, dim.y, dim.z, side_length);
+
+  gvl->addMap(MT_PROBAB_VOXELMAP, "myProbVoxelMap");
+  gvl->addMap(MT_DISTANCE_VOXELMAP, "myDistanceMap");
+  boost::shared_ptr<ProbVoxelMap> prob_map(gvl->getMap("myProbVoxelMap")->as<ProbVoxelMap>());
+  boost::shared_ptr<DistanceVoxelMap> dist_map(gvl->getMap("myDistanceMap")->as<DistanceVoxelMap>());
+
+  std::vector<Vector3f> this_testpoints1;
+  std::vector<Vector3f> this_testpoints2;
+
+  // create two partly overlapping cubes
+  this_testpoints1 = createBoxOfPoints( Vector3f(40, 40, 40), Vector3f(60, 60, 60), 0.9);
+  this_testpoints2 = createBoxOfPoints( Vector3f(50, 50, 50), Vector3f(70, 70, 70), 0.9);
+
+  // insert them as free and occupied space.
+  // as the sensormodel rates occupation higher than free measurements, the overlapping cube will have more than 0.5 probability for occupancy.
+  prob_map->insertPointCloud(this_testpoints1, eBVM_MAX_OCC_PROB);
+  prob_map->insertPointCloud(this_testpoints2, eBVM_MAX_FREE_PROB);
+
+  // so the generated distance map contains obstacles  ?!?!??! TODO: CHECK RESULT!!
+  dist_map->mergeOccupied(prob_map, Vector3ui(0), 0.55);
+
+  dist_map->parallelBanding3D(m1, m2, m3, PBA_DEFAULT_M1_BLOCK_SIZE, PBA_DEFAULT_M2_BLOCK_SIZE, PBA_DEFAULT_M3_BLOCK_SIZE, 1);
+
+  while(true)
+  {
+    gvl->visualizeMap("myProbVoxelMap");
+    gvl->visualizeMap("myDistanceMap");
+    sleep(1);
+  }
+
+  return 0;
+}

packages/gpu_voxels/src/examples/ProbVoxels.cpp

@@ -0,0 +1,121 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// This file is part of the GPU Voxels Software Library.
+//
+// This program is free software licensed under the CDDL
+// (COMMON DEVELOPMENT AND DISTRIBUTION LICENSE Version 1.0).
+// You can find a copy of this license in LICENSE.txt in the top
+// directory of the source code.
+//
+// © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
+//
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+*
+* \author  Andreas Hermann <hermann@fzi.de>
+* \date    2018-04-01
+*
+* This program demonstrates how work with probabilistic Voxels.
+*
+*/
+//----------------------------------------------------------------------
+#include <cstdlib>
+#include <signal.h>
+
+#include <gpu_voxels/GpuVoxels.h>
+#include <gpu_voxels/helpers/GeometryGeneration.h>
+#include <gpu_voxels/logging/logging_gpu_voxels.h>
+
+using namespace gpu_voxels;
+using namespace voxelmap;
+using namespace geometry_generation;
+
+GpuVoxelsSharedPtr gvl;
+
+void ctrlchandler(int)
+{
+  gvl.reset();
+  exit(EXIT_SUCCESS);
+}
+void killhandler(int)
+{
+  gvl.reset();
+  exit(EXIT_SUCCESS);
+}
+
+int main(int argc, char* argv[])
+{
+
+  signal(SIGINT, ctrlchandler);
+  signal(SIGTERM, killhandler);
+
+  icl_core::logging::initialize(argc, argv);
+
+  gvl = GpuVoxels::getInstance();
+
+  Vector3ui dim(136, 136, 136);
+  float side_length = 1.0; // voxel side length
+  gvl->initialize(dim.x, dim.y, dim.z, side_length);
+
+  gvl->addMap(MT_PROBAB_VOXELMAP, "myProbVoxelMap");
+  boost::shared_ptr<ProbVoxelMap> prob_map(gvl->getMap("myProbVoxelMap")->as<ProbVoxelMap>());
+
+  std::vector<Vector3f> boxpoints;
+  boxpoints = createBoxOfPoints( Vector3f(10, 10, 30), Vector3f(30, 30, 50), 0.9); // choose large delta, so that only 1 point falls into each voxel (mostly)
+
+
+  PointCloud box(boxpoints);
+  Matrix4f shift_diag_up(Matrix4f::createFromRotationAndTranslation(Matrix3f::createIdentity(), Vector3f(0, 15, 15)));
+  Matrix4f shift_down(Matrix4f::createFromRotationAndTranslation(Matrix3f::createIdentity(), Vector3f(0, 15, -15)));
+  Matrix4f shift_diag_down(Matrix4f::createFromRotationAndTranslation(Matrix3f::createIdentity(), Vector3f(0, -15, -15)));
+
+
+  // insert cube into map
+  prob_map->insertPointCloud(box, eBVM_MAX_OCC_PROB); // = 254
+  box.transformSelf(&shift_diag_up);
+  prob_map->insertPointCloud(box, eBVM_MAX_OCC_PROB); // = 254
+  box.transformSelf(&shift_diag_up);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(229));
+  box.transformSelf(&shift_diag_up);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(204));
+  box.transformSelf(&shift_diag_up);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(179));
+  box.transformSelf(&shift_diag_up);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(154));
+  box.transformSelf(&shift_down);
+
+                // eBVM_UNCERTAIN_OCC_PROB); // = 129
+
+  prob_map->insertPointCloud(box, BitVoxelMeaning(104));
+  box.transformSelf(&shift_diag_down);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(79));
+  box.transformSelf(&shift_diag_down);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(54));
+  box.transformSelf(&shift_diag_down);
+  prob_map->insertPointCloud(box, BitVoxelMeaning(29));
+  box.transformSelf(&shift_diag_down);
+  prob_map->insertPointCloud(box, eBVM_MAX_FREE_PROB); // = 4
+  box.transformSelf(&shift_diag_down);
+  prob_map->insertPointCloud(box, eBVM_MAX_FREE_PROB); // = 4
+
+
+  boxpoints = createBoxOfPoints( Vector3f(10, 5, 5), Vector3f(12, 130, 130), 0.9); // choose large delta, so that only 1 point falls into each voxel (mostly)
+  box = PointCloud(boxpoints);
+  prob_map->insertPointCloud(box, eBVM_UNCERTAIN_OCC_PROB); // = 129
+  // this will not influence voxels which were also set to other probabilities, as it converts to adding 0 to their values.
+
+  while(true)
+  {
+    gvl->visualizeMap("myProbVoxelMap");
+
+    // this will show partly overlapping cubes, whose occupancy values will get summed up.
+    // all "unknown" voxels will not get drawn, but the uncertain ones will (unkown = initialization value, uncertain = 0.5 probability)
+
+    sleep(1);
+  }
+
+  return 0;
+}