Bev version

.gitignore

@@ -1,3 +1,4 @@
 build
 export
 CMakeLists.txt.user
+external_link_path

README.md

@@ -60,6 +60,12 @@ This is important if you are still using ROS Indigo and need to compile without
 - If the ROS dependency was found, but the GPU-Voxels URDF features are still unabailable, run `source /opt/ros/YOUR_ROS_DISTRO/setup.bash` before running cmake.
 - Eigen 3 issues: can be fixed by cloning a more current unstable Eigen version and placing it in CMAKE_PREFIX_PATH
   + on Ubuntu 18.04 with CUDA 10.0: "math_functions.hpp not found"
+  ```bash
+  git clone https://gitlab.com/libeigen/eigen.git
+  sudo cp -r signature_of_eigen3_matrix_library /usr/include/eigen3
+  sudo cp -r unsupported/ /usr/include/eigen3
+  sudo cp -r Eigen/ /usr/include/eigen3
+  ```
   + Eigen 3.3.4 and 3.3.5 with CUDA 9.0, 9.1, 9.2: Error: class "Eigen::half" has no member "x"
   + see http://eigen.tuxfamily.org/index.php?title=Main_Page#Download
 - Cuda 8.0: Code compiled with Cuda 8.0 works fine with older GPU drivers such as 375.66, but there are runtime errors with driver 384.111 and newer ("PTX JIT compilation failed"). Easy fix: use Cuda 10 with a current 410 or newer driver version. Cuda 10 is also available for Ubuntu 14.04 and 16.04.

packages/gpu_voxels/CMakeLists.txt

@@ -199,7 +199,7 @@ ICMAKER_CONFIGURE_PACKAGE()
 
 ###############################################################################
 # Build examples
-ADD_SUBDIRECTORY(src/examples)
+# ADD_SUBDIRECTORY(src/examples)
 
 IF(BUILD_TESTS)
 ENDIF(BUILD_TESTS)

packages/gpu_voxels/src/examples/RaycastingSensorExample.cpp

@@ -96,8 +96,8 @@ int main(int argc, char* argv[])
   for(size_t pos = 50; pos < 100; ++pos)
   {
     // We perform the insertion and the raycasting twice, so that in the visualizer we can display either all probabilistic voxels or only the occupied ones (without the free ones)
-    prob_map->insertSensorData(obstacle, Vector3f(pos, 2, pos/2.0), true, true, eBVM_OCCUPIED, rob_map->getDeviceDataPtr());
-    prob_map2->insertSensorData(obstacle, Vector3f(pos, 2, pos/2.0), true, false, eBVM_OCCUPIED, rob_map->getDeviceDataPtr());
+    prob_map->insertSensorData(obstacle, Vector3f(pos, 2, pos/2.0), true, true, eBVM_OCCUPIED, MAX_PROBABILITY, rob_map->getDeviceDataPtr());
+    prob_map2->insertSensorData(obstacle, Vector3f(pos, 2, pos/2.0), true, false, eBVM_OCCUPIED, MAX_PROBABILITY, rob_map->getDeviceDataPtr());
   }
 
   while(true)

packages/gpu_voxels/src/gpu_voxels/helpers/PointCloud.cu

@@ -25,6 +25,7 @@
 #include <gpu_voxels/logging/logging_gpu_voxels.h>
 #include <gpu_voxels/helpers/kernels/HelperOperations.h>
 #include <gpu_voxels/helpers/PointcloudFileHandler.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
 
 namespace gpu_voxels
 {
@@ -357,4 +358,35 @@ void PointCloud::print()
   std::cout << std::endl << std::endl;
 }
 
+sensor_msgs::PointCloud2 PointCloud::getPointCloud2()
+{
+  sensor_msgs::PointCloud2 pointcloud_msg;
+
+  Vector3f* tmp = this->getPoints();
+  size_t mPointCloudSize = this->getPointCloudSize();
+
+  pointcloud_msg.header.frame_id = "GPU_voxels";//voxel_point_cloud_frame; // use same reference as in transform_...
+  pointcloud_msg.header.stamp = ros::Time::now();
+  pointcloud_msg.height = 1;
+  pointcloud_msg.width = mPointCloudSize;
+  pointcloud_msg.row_step = pointcloud_msg.width * pointcloud_msg.point_step;
+
+  sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg);
+  modifier.setPointCloud2FieldsByString(1, "xyz");
+
+  sensor_msgs::PointCloud2Iterator<float>iter_x(pointcloud_msg, "x");
+  sensor_msgs::PointCloud2Iterator<float>iter_y(pointcloud_msg, "y");
+  sensor_msgs::PointCloud2Iterator<float>iter_z(pointcloud_msg, "z");
+
+  for(size_t i=0; i<mPointCloudSize; i++)
+  {
+      *iter_x = tmp[i].x;
+      *iter_y = tmp[i].y;
+      *iter_z = tmp[i].z;
+
+      ++iter_x; ++iter_y; ++iter_z;
+  }
+  return pointcloud_msg;
+}
+
 }// end namespace gpu_voxels

packages/gpu_voxels/src/gpu_voxels/helpers/PointCloud.h

@@ -34,6 +34,8 @@
 #include <gpu_voxels/helpers/cuda_handling.hpp>
 #include <gpu_voxels/helpers/MathHelpers.h>
 
+#include <sensor_msgs/PointCloud2.h>
+
 namespace gpu_voxels
 {
 
@@ -141,6 +143,8 @@ class PointCloud
   //for testing
   void print();
 
+  sensor_msgs::PointCloud2 getPointCloud2();
+
 private:
 
   //! Only allocates memory

packages/gpu_voxels/src/gpu_voxels/helpers/cuda_vectors.h

@@ -231,6 +231,24 @@ struct Vector3f
     return *this;
   }
 
+  __device__ __host__
+  inline Vector3f& operator*=(const float& b)
+  {
+    x *= b;
+    y *= b;
+    z *= b;
+    return *this;
+  }
+
+  __device__ __host__
+  inline Vector3f& operator/=(const float& b)
+  {
+    x /= b;
+    y /= b;
+    z /= b;
+    return *this;
+  }
+
   __device__ __host__
   inline bool operator==(const Vector3f& b) const
   {

packages/gpu_voxels/src/gpu_voxels/voxel/AbstractVoxel.h

@@ -54,6 +54,8 @@ class AbstractVoxel
   __host__ __device__
   bool isOccupied(float col_threshold) const;
 
+public:
+
 };
 
 } // end of ns

packages/gpu_voxels/src/gpu_voxels/voxel/DistanceVoxel.h

@@ -58,6 +58,9 @@ class DistanceVoxel //: public AbstractVoxel
   __host__ __device__
   const Vector3ui getObstacle() const;
 
+  __host__ __device__
+  const uint32_t getObstacleID() const;
+
   /**
    * @brief get squared distance (optimization: don't compute square root; distances are not accumulated during DVM computation)
    *

packages/gpu_voxels/src/gpu_voxels/voxel/DistanceVoxel.hpp

@@ -36,6 +36,11 @@ const Vector3ui DistanceVoxel::getObstacle() const {
   return Vector3ui(x, y, z);
 }
 
+__host__ __device__
+const uint32_t DistanceVoxel::getObstacleID() const {
+  return m_obstacle;
+}
+
 __host__ __device__
 DistanceVoxel::pba_dist_t DistanceVoxel::squaredObstacleDistance(Vector3i this_position) const{
   Vector3ui obstacle = getObstacle();

packages/gpu_voxels/src/gpu_voxels/voxel/ProbabilisticVoxel.hpp

@@ -82,7 +82,7 @@ void ProbabilisticVoxel::insert(const BitVoxelMeaning voxel_meaning)
       break;
     case eBVM_COLLISION :
     case eBVM_OCCUPIED : 
-	updateOccupancy(cSENSOR_MODEL_OCCUPIED);
+      updateOccupancy(cSENSOR_MODEL_OCCUPIED);
       break;
     case eBVM_UNKNOWN :
     case eBVM_UNDEFINED :

packages/gpu_voxels/src/gpu_voxels/voxelmap/ProbVoxelMap.h

@@ -29,6 +29,7 @@
 #include <gpu_voxels/helpers/common_defines.h>
 #include <gpu_voxels/helpers/cuda_datatypes.h>
 #include <gpu_voxels/helpers/CollisionInterfaces.h>
+#include <sensor_msgs/PointCloud2.h>
 
 namespace gpu_voxels {
 namespace voxelmap {
@@ -46,7 +47,7 @@ class ProbVoxelMap: public TemplateVoxelMap<ProbabilisticVoxel>,
 
   template<std::size_t length>
   void insertSensorData(const PointCloud &global_points, const Vector3f &sensor_pose, const bool enable_raycasting, const bool cut_real_robot,
-                        const BitVoxelMeaning robot_voxel_meaning, BitVoxel<length>* robot_map = NULL);
+                        const BitVoxelMeaning robot_voxel_meaning, const Probability prob, BitVoxel<length>* robot_map = NULL);
 
   virtual bool insertMetaPointCloudWithSelfCollisionCheck(const MetaPointCloud *robot_links,
                                                           const std::vector<BitVoxelMeaning>& voxel_meanings = std::vector<BitVoxelMeaning>(),
@@ -61,6 +62,10 @@ class ProbVoxelMap: public TemplateVoxelMap<ProbabilisticVoxel>,
 
   size_t collideWith(const voxelmap::BitVectorVoxelMap* map, float coll_threshold = 1.0, const Vector3i &offset = Vector3i());
   size_t collideWith(const voxelmap::ProbVoxelMap* map, float coll_threshold = 1.0, const Vector3i &offset = Vector3i());
+
+  virtual void moveInto(ProbVoxelMap& dest, const Vector3f offset) const;
+  virtual void move(Voxel* dest_data, const Voxel* src_data, const Vector3f offset) const;
+  virtual void publishPointcloud(sensor_msgs::PointCloud2* pointcloud_msg, const float occupancyThreshold=0.5);
 };
 
 } // end of namespace

packages/gpu_voxels/src/gpu_voxels/voxelmap/ProbVoxelMap.hpp

@@ -28,6 +28,7 @@
 #include <gpu_voxels/voxelmap/kernels/VoxelMapOperations.hpp>
 #include <gpu_voxels/voxel/BitVoxel.hpp>
 #include <gpu_voxels/voxel/ProbabilisticVoxel.hpp>
+#include <gpu_voxels/helpers/PointCloud.h>
 
 namespace gpu_voxels {
 namespace voxelmap {
@@ -51,7 +52,7 @@ ProbVoxelMap::~ProbVoxelMap()
 
 template<std::size_t length>
 void ProbVoxelMap::insertSensorData(const PointCloud &global_points, const Vector3f &sensor_pose, const bool enable_raycasting,
-                                    const bool cut_real_robot, const BitVoxelMeaning robot_voxel_meaning,
+                                    const bool cut_real_robot, const BitVoxelMeaning robot_voxel_meaning, const Probability prob,
                                     BitVoxel<length>* robot_map)
 {
   lock_guard guard(this->m_mutex);
@@ -63,19 +64,49 @@ void ProbVoxelMap::insertSensorData(const PointCloud &global_points, const Vecto
   {
     kernelInsertSensorData<<<m_blocks, m_threads>>>(
         m_dev_data, m_voxelmap_size, m_dim, m_voxel_side_length, sensor_pose,
-        global_points.getConstDevicePointer(), global_points.getPointCloudSize(), cut_real_robot, robot_map, robot_voxel_meaning, RayCaster());
+        global_points.getConstDevicePointer(), global_points.getPointCloudSize(), cut_real_robot, robot_map, robot_voxel_meaning, prob, RayCaster());
     CHECK_CUDA_ERROR();
   }
   else
   {
     kernelInsertSensorData<<<m_blocks, m_threads>>>(
         m_dev_data, m_voxelmap_size, m_dim, m_voxel_side_length, sensor_pose,
-        global_points.getConstDevicePointer(), global_points.getPointCloudSize(), cut_real_robot, robot_map, robot_voxel_meaning, DummyRayCaster());
+        global_points.getConstDevicePointer(), global_points.getPointCloudSize(), cut_real_robot, robot_map, robot_voxel_meaning, prob, DummyRayCaster());
     CHECK_CUDA_ERROR();
   }
   HANDLE_CUDA_ERROR(cudaDeviceSynchronize());
 }
 
+/**
+ * Convert a ProbVoxelMap as a pointcloud in ROS.
+ *
+ * In RViz: add the pointcloud, set render settings to Boxes, scale to voxel_side_length
+ */
+void ProbVoxelMap::publishPointcloud(sensor_msgs::PointCloud2* pointcloud_msg, const float occupancyThreshold)
+{
+  Vector3f *m_points;
+  HANDLE_CUDA_ERROR( cudaMalloc((void** ) &m_points, sizeof(Vector3f)*(m_voxelmap_size-1) ));
+
+  size_t cloudSize = 0;
+  size_t *m_cloudSize;
+  HANDLE_CUDA_ERROR( cudaMalloc((void** ) &m_cloudSize, sizeof(size_t)));
+  HANDLE_CUDA_ERROR( cudaMemcpy(m_cloudSize, &cloudSize, sizeof(size_t), cudaMemcpyHostToDevice));
+
+  HANDLE_CUDA_ERROR( cudaDeviceSynchronize());
+  kernelGetProbabilisticPointCloud<<<m_blocks, m_threads>>>(this->m_dev_data, m_points, occupancyThreshold, m_voxelmap_size, m_voxel_side_length, m_dim, m_cloudSize);
+  HANDLE_CUDA_ERROR( cudaDeviceSynchronize());
+  HANDLE_CUDA_ERROR( cudaMemcpy(&cloudSize, m_cloudSize, sizeof(size_t), cudaMemcpyDeviceToHost));
+
+  Vector3f *points = new Vector3f[cloudSize];;
+  HANDLE_CUDA_ERROR( cudaMemcpy(points, m_points, sizeof(Vector3f)*cloudSize, cudaMemcpyDeviceToHost));
+  PointCloud pointcloud(points, cloudSize);
+
+  (*pointcloud_msg) = pointcloud.getPointCloud2();
+
+  HANDLE_CUDA_ERROR(cudaFree(m_points));
+  HANDLE_CUDA_ERROR(cudaFree(m_cloudSize));
+}
+
 bool ProbVoxelMap::insertMetaPointCloudWithSelfCollisionCheck(const MetaPointCloud *robot_links,
                                                               const std::vector<BitVoxelMeaning>& voxel_meanings,
                                                               const std::vector<BitVector<BIT_VECTOR_LENGTH> >& collision_masks,
@@ -108,6 +139,20 @@ size_t ProbVoxelMap::collideWith(const ProbVoxelMap *map, float coll_threshold,
   return collisionCheckWithCounterRelativeTransform((TemplateVoxelMap*)map, collider, offset); //does the locking
 }
 
+void ProbVoxelMap::move(Voxel* dest_data, const Voxel* src_data, const Vector3f offset) const
+{
+  HANDLE_CUDA_ERROR(cudaDeviceSynchronize());
+  kernelMoveMap<<<m_blocks, m_threads>>>(dest_data, src_data, m_voxelmap_size, m_voxel_side_length, this->m_dim, offset);
+  HANDLE_CUDA_ERROR(cudaDeviceSynchronize());
+  CHECK_CUDA_ERROR();
+}
+
+void ProbVoxelMap::moveInto(ProbVoxelMap& dest, const Vector3f offset) const
+{
+  assert(this->m_dim == dest.m_dim);
+  move(dest.m_dev_data, this->m_dev_data, offset);
+}
+
 } // end of namespace
 } // end of namespace
 

packages/gpu_voxels/src/gpu_voxels/voxelmap/TemplateVoxelMap.h

@@ -283,7 +283,7 @@ class TemplateVoxelMap : public AbstractVoxelMap
 
   //! result array for collision check with counter on device
   uint16_t* m_dev_collision_check_results_counter;
-
+  
   void erode(Voxel* d_dest_data, const Voxel* d_src_data, float erode_threshold, float occupied_threshold) const;
 
   void insertDilatedCoordinateList(Voxel* d_dest_data, const Vector3ui* d_src_coordinates, uint32_t size, const BitVoxelMeaning voxel_meaning);

packages/gpu_voxels/src/gpu_voxels/voxelmap/TemplateVoxelMap.hpp

@@ -1050,6 +1050,12 @@ bool TemplateVoxelMap<Voxel>::merge(const GpuVoxelsMapSharedPtr other, const Vec
   if (voxel_offset != Vector3i()) {
     LOGGING_ERROR_C(VoxelmapLog, TemplateVoxelMap, "You tried to apply an offset while merging a VoxelList into a DistanceVoxelMap (not supported yet)!" << endl);
     return false;
+
+  }
+
+  BitVoxelMeaning meaning = eBVM_OCCUPIED;
+  if (new_meaning != NULL) {
+  	meaning = *new_meaning;
   }
 
   boost::lock(this->m_mutex, other->m_mutex);
@@ -1061,26 +1067,26 @@ bool TemplateVoxelMap<Voxel>::merge(const GpuVoxelsMapSharedPtr other, const Vec
     case MT_BITVECTOR_VOXELLIST:
     {
       voxellist::TemplateVoxelList<BitVectorVoxel, MapVoxelID>* voxelList = other->as<voxellist::TemplateVoxelList<BitVectorVoxel, MapVoxelID> >();
-      insertCoordinateList(voxelList->getDeviceCoordPtr(), voxelList->m_dev_coord_list.size(), eBVM_OCCUPIED);
+      insertCoordinateList(voxelList->getDeviceCoordPtr(), voxelList->m_dev_coord_list.size(), meaning);
       return true;
     }
 
     case MT_PROBAB_VOXELLIST:
     {
       voxellist::TemplateVoxelList<ProbabilisticVoxel, MapVoxelID>* voxelList = other->as<voxellist::TemplateVoxelList<ProbabilisticVoxel, MapVoxelID> >();
-      insertCoordinateList(voxelList->getDeviceCoordPtr(), voxelList->m_dev_coord_list.size(), eBVM_OCCUPIED);
+      insertCoordinateList(voxelList->getDeviceCoordPtr(), voxelList->m_dev_coord_list.size(), meaning);
       return true;
     }
 
     case MT_COUNTING_VOXELLIST:
     {
       voxellist::TemplateVoxelList<CountingVoxel, MapVoxelID>* voxelList = other->as<voxellist::TemplateVoxelList<CountingVoxel, MapVoxelID> >();
-      insertCoordinateList(voxelList->getDeviceCoordPtr(), voxelList->m_dev_coord_list.size(), eBVM_OCCUPIED);
+      insertCoordinateList(voxelList->getDeviceCoordPtr(), voxelList->m_dev_coord_list.size(), meaning);
       return true;
     }
 
     default:
-      LOGGING_ERROR_C(VoxelmapLog, TemplateVoxelMap, GPU_VOXELS_MAP_OPERATION_NOT_YET_SUPPORTED << endl);
+      LOGGING_ERROR_C(VoxelmapLog, TemplateVoxelMap, GPU_VOXELS_MAP_OPERATION_NOT_YET_SUPPORTED << " MapType: " << other->getMapType() << endl);
       return false;
   }
 }

packages/gpu_voxels/src/gpu_voxels/voxelmap/VoxelMap.cu

@@ -37,7 +37,7 @@ template uint32_t TemplateVoxelMap<ProbabilisticVoxel>::collisionCheckWithCounte
 // ############################### ProbVoxelMap (inherits from TemplateVoxelMap) ######################################
 // Explicitly instantiate template methods to enable GCC to link agains NVCC compiled objects
 template void ProbVoxelMap::insertSensorData<BIT_VECTOR_LENGTH>(const PointCloud&, const Vector3f&, const bool, const bool,
-                                                                const BitVoxelMeaning, BitVoxel<BIT_VECTOR_LENGTH>*);
+                                                                const BitVoxelMeaning, const Probability, BitVoxel<BIT_VECTOR_LENGTH>*);
 
 
 

packages/gpu_voxels/src/gpu_voxels/voxelmap/kernels/VoxelMapOperations.h

@@ -28,6 +28,7 @@
 #include <gpu_voxels/voxel/BitVoxel.h>
 #include <gpu_voxels/voxel/ProbabilisticVoxel.h>
 #include <gpu_voxels/voxel/DistanceVoxel.h>
+#include <gpu_voxels/helpers/PointCloud.h>
 
 #include <thrust/transform.h>
 
@@ -389,7 +390,7 @@ __global__
 void kernelInsertSensorData(ProbabilisticVoxel* voxelmap, const uint32_t voxelmap_size,
                             const Vector3ui dimensions, const float voxel_side_length, const Vector3f sensor_pose,
                             const Vector3f* sensor_data, const size_t num_points, const bool cut_real_robot,
-                            BitVoxel<length>* robotmap, const uint32_t bit_index, RayCasting rayCaster);
+                            BitVoxel<length>* robotmap, const uint32_t bit_index, const Probability prob, RayCasting rayCaster);
 
 /*!
  * Collide two voxel maps.
@@ -446,6 +447,13 @@ void kernelInsertDilatedCoordinateTuples(Voxel* voxelmap, const Vector3ui dimens
                                   const Vector3ui *coordinates, const std::size_t sizePoints, const BitVoxelMeaning voxel_meaning,
                                   bool *points_outside_map);
 
+__global__
+void kernelMoveMap(ProbabilisticVoxel* voxelmap_out, const ProbabilisticVoxel* voxelmap_in, const uint32_t voxelmap_size, const float voxel_side_length, const Vector3ui dimensions, const Vector3f offset);
+
+__global__
+void kernelGetProbabilisticPointCloud(const ProbabilisticVoxel* voxelmap, Vector3f* pointCloud, const float occupancyThreshold,
+                                  const uint32_t voxelmap_size, const float voxel_side_length, const Vector3ui dimensions, size_t *cloudSize);
+
 template<class Voxel>
 __global__
 void kernelErode(Voxel* voxelmap_out, const Voxel* voxelmap_in, const Vector3ui dimensions, float occupied_threshold, float erode_threshold);