Merge branch 'master' into devel/maplab_changes

.gitmodules

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

voxblox/CMakeLists.txt

@@ -45,13 +45,14 @@ endif()
 ####################
 
 set("${PROJECT_NAME}_SRCS"
+  src/alignment/icp.cc
   src/core/block.cc
   src/core/esdf_map.cc
   src/core/tsdf_map.cc
   src/integrator/esdf_integrator.cc
   src/integrator/esdf_occ_integrator.cc
   src/integrator/integrator_utils.cc
-  src/integrator/merge_integration.cc
+  src/integrator/intensity_integrator.cc
   src/integrator/tsdf_integrator.cc
   src/io/mesh_ply.cc
   src/io/sdf_ply.cc
@@ -63,6 +64,7 @@ set("${PROJECT_NAME}_SRCS"
   src/utils/layer_utils.cc
   src/utils/protobuf_utils.cc
   src/utils/timing.cc
+  src/utils/voxel_utils.cc
 )
 
 #############
@@ -177,6 +179,11 @@ catkin_add_gtest(test_layer_utils
 )
 target_link_libraries(test_layer_utils ${PROJECT_NAME})
 
+catkin_add_gtest(test_sdf_integrators
+  test/test_sdf_integrators.cc
+)
+target_link_libraries(test_sdf_integrators ${PROJECT_NAME})
+
 ##########
 # EXPORT #
 ##########

voxblox/include/voxblox/alignment/icp.h

@@ -0,0 +1,154 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+#ifndef VOXBLOX_ALIGNMENT_ICP_H_
+#define VOXBLOX_ALIGNMENT_ICP_H_
+
+#include <algorithm>
+#include <memory>
+#include <thread>
+
+#include "voxblox/core/block_hash.h"
+#include "voxblox/core/common.h"
+#include "voxblox/core/layer.h"
+#include "voxblox/integrator/integrator_utils.h"
+#include "voxblox/interpolator/interpolator.h"
+#include "voxblox/utils/approx_hash_array.h"
+
+namespace voxblox {
+
+class ICP {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  struct Config {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    bool refine_roll_pitch = false;
+    int mini_batch_size = 20;
+    FloatingPoint min_match_ratio = 0.8;
+    FloatingPoint subsample_keep_ratio = 0.5;
+    FloatingPoint inital_translation_weighting = 100.0;
+    FloatingPoint inital_rotation_weighting = 100.0;
+    size_t num_threads = std::thread::hardware_concurrency();
+  };
+
+  explicit ICP(const Config& config);
+
+  // returns number of successful mini batches
+  size_t runICP(const Layer<TsdfVoxel>& tsdf_layer, const Pointcloud& points,
+                const Transformation& inital_T_tsdf_sensor,
+                Transformation* refined_T_tsdf_sensor,
+                const unsigned seed = std::chrono::system_clock::now()
+                                          .time_since_epoch()
+                                          .count());
+
+  bool refiningRollPitch() { return config_.refine_roll_pitch; }
+
+ private:
+  typedef Transformation::Vector6 Vector6;
+
+  template <size_t dim>
+  static bool getRotationFromMatchedPoints(const PointsMatrix& src_demean,
+                                           const PointsMatrix& tgt_demean,
+                                           Rotation* R_tgt_src) {
+    static_assert((dim == 3) || (dim == 2),
+                  "Rotation calculation is only meaningful for 2D or 3D data");
+    CHECK_NOTNULL(R_tgt_src);
+
+    SquareMatrix<3> rotation_matrix = SquareMatrix<3>::Identity();
+
+    SquareMatrix<dim> H =
+        src_demean.topRows<dim>() * tgt_demean.topRows<dim>().transpose();
+
+    // Compute the Singular Value Decomposition
+    Eigen::JacobiSVD<SquareMatrix<dim>> svd(
+        H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    SquareMatrix<dim> u = svd.matrixU();
+    SquareMatrix<dim> v = svd.matrixV();
+
+    // Compute R = V * U'
+    if (u.determinant() * v.determinant() < 0.0) {
+      v.col(dim - 1) *= -1.0;
+    }
+
+    rotation_matrix.topLeftCorner<dim, dim>() = v * u.transpose();
+
+    *R_tgt_src = Rotation(rotation_matrix);
+
+    // not caught by is valid check
+    if (!std::isfinite(rotation_matrix.sum())) {
+      return false;
+    }
+
+    return Rotation::isValidRotationMatrix(rotation_matrix);
+  }
+
+  static bool getTransformFromMatchedPoints(const PointsMatrix& src,
+                                            const PointsMatrix& tgt,
+                                            const bool refine_roll_pitch,
+                                            Transformation* T_tsdf_sensor);
+
+  static void addNormalizedPointInfo(const Point& point,
+                                     const Point& normalized_point_normal,
+                                     Vector6* info_vector);
+
+  void matchPoints(const Pointcloud& points, const size_t start_idx,
+                   const Transformation& T_tsdf_sensor, PointsMatrix* src,
+                   PointsMatrix* tgt, Vector6* info_vector);
+
+  bool stepICP(const Pointcloud& points, const size_t start_idx,
+               const Transformation& inital_T_tsdf_sensor,
+               Transformation* refined_T_tsdf_sensor, Vector6* info_vector);
+
+  void runThread(const Pointcloud& points,
+                 Transformation* current_T_tsdf_sensor,
+                 Vector6* base_info_vector, size_t* num_updates);
+
+  Config config_;
+
+  std::atomic<size_t> atomic_idx_;
+  std::mutex mutex_;
+
+  FloatingPoint voxel_size_;
+  FloatingPoint voxel_size_inv_;
+  std::shared_ptr<Interpolator<TsdfVoxel>> interpolator_;
+};
+
+}  // namespace voxblox
+
+#endif  // VOXBLOX_ALIGNMENT_ICP_H_

voxblox/include/voxblox/core/block.h

@@ -57,25 +57,37 @@ class Block {
   // index that is within this block if you pass a coordinate outside the range
   // of this block. Try not to use this function if there is an alternative to
   // directly address the voxels via precise integer indexing math.
-  inline VoxelIndex computeVoxelIndexFromCoordinates(
+  inline VoxelIndex computeTruncatedVoxelIndexFromCoordinates(
       const Point& coords) const {
     const IndexElement max_value = voxels_per_side_ - 1;
     VoxelIndex voxel_index =
-        getGridIndexFromPoint(coords - origin_, voxel_size_inv_);
+        getGridIndexFromPoint<VoxelIndex>(coords - origin_, voxel_size_inv_);
     // check is needed as getGridIndexFromPoint gives results that have a tiny
     // chance of being outside the valid voxel range.
     return VoxelIndex(std::max(std::min(voxel_index.x(), max_value), 0),
                       std::max(std::min(voxel_index.y(), max_value), 0),
                       std::max(std::min(voxel_index.z(), max_value), 0));
   }
 
+  // NOTE: This function is also dangerous, use in combination with
+  // Block::isValidVoxelIndex function.
+  // This function doesn't truncate the voxel index to the [0, voxels_per_side]
+  // range when the coordinate is outside the range of this block, unlike the
+  // function above.
+  inline VoxelIndex computeVoxelIndexFromCoordinates(
+      const Point& coords) const {
+    VoxelIndex voxel_index =
+        getGridIndexFromPoint<VoxelIndex>(coords - origin_, voxel_size_inv_);
+    return voxel_index;
+  }
+
   // NOTE: This function is dangerous, it will truncate the voxel index to an
   // index that is within this block if you pass a coordinate outside the range
   // of this block. Try not to use this function if there is an alternative to
   // directly address the voxels via precise integer indexing math.
   inline size_t computeLinearIndexFromCoordinates(const Point& coords) const {
     return computeLinearIndexFromVoxelIndex(
-        computeVoxelIndexFromCoordinates(coords));
+        computeTruncatedVoxelIndexFromCoordinates(coords));
   }
 
   // Returns CENTER point of voxel.
@@ -111,14 +123,27 @@ class Block {
     return voxels_[computeLinearIndexFromVoxelIndex(index)];
   }
 
-  // NOTE: This function is dangerous, it will truncate the voxel index to an
-  // index that is within this block if you pass a coordinate outside the range
-  // of this block. Try not to use this function if there is an alternative to
-  // directly address the voxels via precise integer indexing math.
+  // NOTE: The following three functions are dangerous, they will truncate the
+  // voxel index to an index that is within this block if you pass a coordinate
+  // outside the range of this block. Try not to use this function if there is
+  // an alternative to directly address the voxels via precise integer indexing
+  // math.
   inline const VoxelType& getVoxelByCoordinates(const Point& coords) const {
     return voxels_[computeLinearIndexFromCoordinates(coords)];
   }
 
+  inline VoxelType& getVoxelByCoordinates(const Point& coords) {
+    return voxels_[computeLinearIndexFromCoordinates(coords)];
+  }
+
+  inline VoxelType* getVoxelPtrByCoordinates(const Point& coords) {
+    return &voxels_[computeLinearIndexFromCoordinates(coords)];
+  }
+
+  inline const VoxelType* getVoxelPtrByCoordinates(const Point& coords) const {
+    return &voxels_[computeLinearIndexFromCoordinates(coords)];
+  }
+
   inline VoxelType& getVoxelByLinearIndex(size_t index) {
     DCHECK_LT(index, num_voxels_);
     return voxels_[index];
@@ -128,14 +153,6 @@ class Block {
     return voxels_[computeLinearIndexFromVoxelIndex(index)];
   }
 
-  inline VoxelType& getVoxelByCoordinates(const Point& coords) {
-    return voxels_[computeLinearIndexFromCoordinates(coords)];
-  }
-
-  inline VoxelType* getVoxelPtrByCoordinates(const Point& coords) {
-    return &voxels_[computeLinearIndexFromCoordinates(coords)];
-  }
-
   inline bool isValidVoxelIndex(const VoxelIndex& index) const {
     if (index.x() < 0 ||
         index.x() >= static_cast<IndexElement>(voxels_per_side_)) {
@@ -160,7 +177,7 @@ class Block {
   }
 
   BlockIndex block_index() const {
-    return getGridIndexFromOriginPoint(origin_, block_size_inv_);
+    return getGridIndexFromOriginPoint<BlockIndex>(origin_, block_size_inv_);
   }
 
   // Basic function accessors.
@@ -186,7 +203,7 @@ class Block {
   void serializeToIntegers(std::vector<uint32_t>* data) const;
   void deserializeFromIntegers(const std::vector<uint32_t>& data);
 
-  bool mergeBlock(const Block<VoxelType>& other_block);
+  void mergeBlock(const Block<VoxelType>& other_block);
 
   size_t getMemorySize() const;
 

voxblox/include/voxblox/core/block_hash.h

@@ -15,13 +15,12 @@ namespace voxblox {
 struct AnyIndexHash {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  static constexpr size_t prime1 = 73856093;
-  static constexpr size_t prime2 = 19349663;
-  static constexpr size_t prime3 = 83492791;
+  static constexpr size_t sl = 17191;
+  static constexpr size_t sl2 = sl * sl;
 
   std::size_t operator()(const AnyIndex& index) const {
-    return (static_cast<unsigned int>(index.x()) * prime1 ^ index.y() * prime2 ^
-            index.z() * prime3);
+    return static_cast<unsigned int>(index.x() + index.y() * sl +
+                                     index.z() * sl2);
   }
 };
 
@@ -49,13 +48,12 @@ typedef typename HierarchicalIndexMap::value_type HierarchicalIndex;
 struct LongIndexHash {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  static constexpr size_t prime1 = 73856093;
-  static constexpr size_t prime2 = 19349663;
-  static constexpr size_t prime3 = 83492791;
+  static constexpr size_t sl = 17191;
+  static constexpr size_t sl2 = sl * sl;
 
   std::size_t operator()(const LongIndex& index) const {
-    return (static_cast<unsigned int>(index.x()) * prime1 ^ index.y() * prime2 ^
-            index.z() * prime3);
+    return static_cast<unsigned int>(index.x() + index.y() * sl +
+                                     index.z() * sl2);
   }
 };
 
@@ -69,6 +67,10 @@ struct LongIndexHashMapType {
       type;
 };
 
+typedef std::unordered_set<LongIndex, LongIndexHash, std::equal_to<LongIndex>,
+                           Eigen::aligned_allocator<LongIndex> >
+    LongIndexSet;
+
 }  // namespace voxblox
 
 #endif  // VOXBLOX_CORE_BLOCK_HASH_H_

voxblox/include/voxblox/core/block_inl.h

@@ -1,10 +1,11 @@
 #ifndef VOXBLOX_CORE_BLOCK_INL_H_
 #define VOXBLOX_CORE_BLOCK_INL_H_
 
-#include "./Block.pb.h"
-
 #include <vector>
 
+#include "./Block.pb.h"
+#include "voxblox/utils/voxel_utils.h"
+
 namespace voxblox {
 
 template <typename VoxelType>
@@ -46,9 +47,23 @@ void Block<VoxelType>::getProto(BlockProto* proto) const {
 }
 
 template <typename VoxelType>
-bool Block<VoxelType>::mergeBlock(const Block<VoxelType>& /*other_block*/) {
-  LOG(FATAL) << "Not implemented for this voxel type!";
-  return false;
+void Block<VoxelType>::mergeBlock(const Block<VoxelType>& other_block) {
+  CHECK_EQ(other_block.voxel_size(), voxel_size());
+  CHECK_EQ(other_block.voxels_per_side(), voxels_per_side());
+
+  if (!other_block.has_data()) {
+    return;
+  } else {
+    has_data() = true;
+    updated() = true;
+
+    for (IndexElement voxel_idx = 0;
+         voxel_idx < static_cast<IndexElement>(num_voxels()); ++voxel_idx) {
+      mergeVoxelAIntoVoxelB<VoxelType>(
+          other_block.getVoxelByLinearIndex(voxel_idx),
+          &(getVoxelByLinearIndex(voxel_idx)));
+    }
+  }
 }
 
 template <typename VoxelType>