Merge branch 'feature/rewrite_esdf' into feature/more_efficient_neigh…

…borhood_lookup

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "tools/pybind11"]
-	path = tools/pybind11
-	url = https://github.com/pybind/pybind11.git

voxblox/CMakeLists.txt

@@ -7,11 +7,6 @@ catkin_simple()
 set(CMAKE_MACOSX_RPATH 0)
 add_definitions(-std=c++11 -Wall -Wextra)
 
-if (EXISTS "${CMAKE_CURRENT_LIST_DIR}/../tools/pybind11/CMakeLists.txt")
-  set(HAVE_PYBIND11 TRUE)
-  message(STATUS "Found pybind11; generating Python bindings")
-endif()
-
 ############
 # PROTOBUF #
 ############
@@ -90,35 +85,6 @@ cs_add_library(${PROJECT_NAME}
 )
 target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_proto ${PROTOBUF_LIBRARIES})
 
-###################
-# PYTHON BINDINGS #
-###################
-if(HAVE_PYBIND11)
-  catkin_python_setup()
-
-  add_subdirectory(../tools/pybind11 pybind11)
-  message("Building Python bindings for voxblox")
-  pybind11_add_module(voxbloxpy
-    ${${PROJECT_NAME}_SRCS}
-    src/pybind11/esdf_map_bind.cc
-    src/pybind11/tsdf_map_bind.cc
-    src/pybind11/esdf_integrator_bind.cc
-    src/pybind11/tsdf_integrator_bind.cc
-    src/pybind11/voxel_bind.cc
-    src/pybind11/layer_bind.cc
-    src/pybind11/block_bind.cc
-    src/pybind11/layer_io_bind.cc
-    src/pybind11/planning_utils_bind.cc
-    src/pybind11/bind.cc)
-  set_target_properties(voxbloxpy PROPERTIES LINKER_LANGUAGE CXX)
-  target_link_libraries(voxbloxpy PUBLIC ${PROJECT_NAME}_proto
-                                         ${PROTOBUF_LIBRARIES})
-
-  set_target_properties(voxbloxpy
-    PROPERTIES LIBRARY_OUTPUT_DIRECTORY
-  ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/../)
-endif()
-
 ############
 # BINARIES #
 ############

voxblox/include/voxblox/integrator/tsdf_integrator.h

@@ -25,13 +25,27 @@
 
 namespace voxblox {
 
+enum class TsdfIntegratorType : int {
+  kSimple = 1,
+  kMerged = 2,
+  kFast = 3,
+};
+
+static constexpr size_t kNumTsdfIntegratorTypes = 3u;
+
+const std::array<std::string, kNumTsdfIntegratorTypes>
+    kTsdfIntegratorTypeNames = {{/*kSimple*/ "simple",
+                                 /*kMerged*/ "merged",
+                                 /*kFast*/ "fast"}};
+
 // Note most functions state if they are thread safe. Unless explicitly stated
 // otherwise, this thread safety is based on the assumption that any pointers
 // passed to the functions point to objects that are guaranteed to not be
 // accessed by other threads.
 class TsdfIntegratorBase {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  typedef std::shared_ptr<TsdfIntegratorBase> Ptr;
 
   struct Config {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -134,6 +148,16 @@ class TsdfIntegratorBase {
   ApproxHashArray<12, std::mutex, GlobalIndex, LongIndexHash> mutexes_;
 };
 
+class TsdfIntegratorFactory {
+ public:
+  static TsdfIntegratorBase::Ptr create(
+      const std::string& integrator_type_name,
+      const TsdfIntegratorBase::Config& config, Layer<TsdfVoxel>* layer);
+  static TsdfIntegratorBase::Ptr create(
+      const TsdfIntegratorType integrator_type,
+      const TsdfIntegratorBase::Config& config, Layer<TsdfVoxel>* layer);
+};
+
 class SimpleTsdfIntegrator : public TsdfIntegratorBase {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

voxblox/include/voxblox/utils/neighbor_tools.h

@@ -16,19 +16,42 @@ enum Connectivity : unsigned int {
 class NeighborhoodLookupTables {
  public:
   typedef Eigen::Matrix<LongIndexElement, 3, Connectivity::kTwentySix>
-      IndexOffsets;
+      LongIndexOffsets;
+  typedef Eigen::Matrix<IndexElement, 3, Connectivity::kTwentySix> IndexOffsets;
   typedef Eigen::Matrix<float, 1, Connectivity::kTwentySix> Distances;
 
+  // Stores the distances to the 6, 18, and 26 neighborhood, in that order.
+  // These distances need to be scaled by the voxel distance to get metric
+  // distances.
   static const Distances kDistances;
+
+  // Lookup table for the offsets between a index and its 6, 18, and 26
+  // neighborhood, in that order. These two offset tables are the same except
+  // for the type, this saves casting the offset when used with either global
+  // index (long) or local index (int) in the neighborhood lookup.
   static const IndexOffsets kOffsets;
+  static const LongIndexOffsets kLongOffsets;
 };
 
 template <Connectivity kConnectivity>
 class Neighborhood : public NeighborhoodLookupTables {
  public:
   typedef Eigen::Matrix<LongIndexElement, 3, kConnectivity> IndexMatrix;
 
-  static void getHierarchicalIndexFromDirection(
+  // Get the global index of all (6, 18, or 26) neighbors of the input index.
+  static void getFromGlobalIndex(const GlobalIndex& global_index,
+                                 IndexMatrix* neighbors) {
+    CHECK_NOTNULL(neighbors);
+    for (unsigned int i = 0u; i < kConnectivity; ++i) {
+      neighbors->col(i) = global_index + kLongOffsets.col(i);
+    }
+  }
+
+  // Get the hierarchical index (block idx, local voxel index) a voxel described
+  // by its hierarchical index and a direction. The main purpose of this
+  // function is to solve the cross-block indexing that happens when looking up
+  // neighbors at the block boundaries.
+  static void getFromHierarchicalIndexAndDirection(
       const BlockIndex& block_index, const VoxelIndex& voxel_index,
       const SignedIndex& direction, const size_t voxels_per_side,
       BlockIndex* neighbor_block_index, VoxelIndex* neighbor_voxel_index) {
@@ -52,6 +75,10 @@ class Neighborhood : public NeighborhoodLookupTables {
     }
   }
 
+  // Get the hierarchical indices (block idx, local voxel index) for all
+  // neighbors (6, 18, or 26 neighborhood) of a hierarcical index. This function
+  // solves the cross-block indexing that happens when looking up neighbors at
+  // the block boundary.
   static void getFromHierarchicalIndex(const BlockIndex& block_index,
                                        const VoxelIndex& voxel_index,
                                        const size_t voxels_per_side,
@@ -61,17 +88,9 @@ class Neighborhood : public NeighborhoodLookupTables {
     AlignedVector<VoxelKey>& neighbors = *neighbors_ptr;
     for (unsigned int i = 0u; i < kConnectivity; ++i) {
       VoxelKey& neighbor = neighbors[i];
-      getHierarchicalIndexFromDirection(block_index, voxel_index,
-                                        kOffsets.col(i), voxels_per_side,
-                                        &neighbor.first, &neighbor.second);
-    }
-  }
-
-  static void getFromGlobalIndex(const GlobalIndex& global_index,
-                                 IndexMatrix* neighbors) {
-    CHECK_NOTNULL(neighbors);
-    for (unsigned int i = 0u; i < kConnectivity; ++i) {
-      neighbors->col(i) = global_index + kOffsets.col(i);
+      getFromHierarchicalIndexAndDirection(block_index, voxel_index,
+                                           kOffsets.col(i), voxels_per_side,
+                                           &neighbor.first, &neighbor.second);
     }
   }
 };

voxblox/src/integrator/esdf_integrator.cc

@@ -372,6 +372,10 @@ void EsdfIntegrator::processOpenSet() {
     // Go through the neighbors and see if we can update any of them.
     for (unsigned int idx = 0u; idx < neigbor_indices.cols(); ++idx) {
       const GlobalIndex& neighbor_index = neigbor_indices.col(idx);
+      const SignedIndex& direction =
+          NeighborhoodLookupTables::kOffsets.col(idx);
+      FloatingPoint distance =
+          NeighborhoodLookupTables::kDistances[idx] * voxel_size_;
 
       EsdfVoxel* neighbor_voxel =
           esdf_layer_->getVoxelPtrByGlobalIndex(neighbor_index);
@@ -385,17 +389,15 @@ void EsdfIntegrator::processOpenSet() {
         continue;
       }
 
-      SignedIndex direction = (neighbor_index - global_index).cast<int>();
       SignedIndex new_parent = -direction;
-      FloatingPoint distance =
-          direction.cast<FloatingPoint>().norm() * voxel_size_;
       if (config_.full_euclidean_distance) {
         // In this case, the new parent is is actually the parent of the
         // current voxel.
         // And the distance is... Well, complicated.
         new_parent = voxel->parent - direction;
         distance = voxel_size_ * (new_parent.cast<FloatingPoint>().norm() -
                                   voxel->parent.cast<FloatingPoint>().norm());
+
         if (distance < 0.0) {
           continue;
         }
@@ -482,6 +484,8 @@ bool EsdfIntegrator::updateVoxelFromNeighbors(const GlobalIndex& global_index) {
   // Go through the neighbors and see if we can update any of them.
   for (unsigned int idx = 0u; idx < neigbor_indices.cols(); ++idx) {
     const GlobalIndex& neighbor_index = neigbor_indices.col(idx);
+    const FloatingPoint distance =
+        Neighborhood<Connectivity::kTwentySix>::kDistances[idx];
 
     EsdfVoxel* neighbor_voxel =
         esdf_layer_->getVoxelPtrByGlobalIndex(neighbor_index);

voxblox/src/integrator/tsdf_integrator.cc

@@ -1,9 +1,48 @@
-#include "voxblox/integrator/tsdf_integrator.h"
 #include <iostream>
 #include <list>
+#include "voxblox/integrator/tsdf_integrator.h"
 
 namespace voxblox {
 
+TsdfIntegratorBase::Ptr TsdfIntegratorFactory::create(
+    const std::string& integrator_type_name,
+    const TsdfIntegratorBase::Config& config, Layer<TsdfVoxel>* layer) {
+  CHECK(!integrator_type_name.empty());
+
+  int integrator_type = 1;
+  for (const std::string& valid_integrator_type_name :
+       kTsdfIntegratorTypeNames) {
+    if (integrator_type_name == valid_integrator_type_name) {
+      return create(static_cast<TsdfIntegratorType>(integrator_type), config,
+                    layer);
+    }
+    ++integrator_type;
+  }
+  LOG(FATAL) << "Unknown TSDF integrator type: " << integrator_type_name;
+}
+
+TsdfIntegratorBase::Ptr TsdfIntegratorFactory::create(
+    const TsdfIntegratorType integrator_type,
+    const TsdfIntegratorBase::Config& config, Layer<TsdfVoxel>* layer) {
+  CHECK_NOTNULL(layer);
+  switch (integrator_type) {
+    case TsdfIntegratorType::kSimple:
+      return TsdfIntegratorBase::Ptr(new SimpleTsdfIntegrator(config, layer));
+      break;
+    case TsdfIntegratorType::kMerged:
+      return TsdfIntegratorBase::Ptr(new MergedTsdfIntegrator(config, layer));
+      break;
+    case TsdfIntegratorType::kFast:
+      return TsdfIntegratorBase::Ptr(new FastTsdfIntegrator(config, layer));
+      break;
+    default:
+      LOG(FATAL) << "Unknown TSDF integrator type: "
+                 << static_cast<int>(integrator_type);
+      break;
+  }
+  return TsdfIntegratorBase::Ptr();
+}
+
 // Note many functions state if they are thread safe. Unless explicitly stated
 // otherwise, this thread safety is based on the assumption that any pointers
 // passed to the functions point to objects that are guaranteed to not be