Program Listing for File tsdf_integrator.h

↰ Return to documentation for file (voxblox/include/voxblox/integrator/tsdf_integrator.h)

#ifndef VOXBLOX_INTEGRATOR_TSDF_INTEGRATOR_H_
#define VOXBLOX_INTEGRATOR_TSDF_INTEGRATOR_H_

#include <algorithm>
#include <atomic>
#include <cmath>
#include <deque>
#include <limits>
#include <memory>
#include <mutex>
#include <queue>
#include <string>
#include <thread>
#include <utility>
#include <vector>

#include <glog/logging.h>
#include <Eigen/Core>

#include "voxblox/core/block_hash.h"
#include "voxblox/core/common.h"
#include "voxblox/core/layer.h"
#include "voxblox/core/voxel.h"
#include "voxblox/integrator/integrator_utils.h"
#include "voxblox/utils/approx_hash_array.h"
#include "voxblox/utils/timing.h"

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"}};

class TsdfIntegratorBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef std::shared_ptr<TsdfIntegratorBase> Ptr;

  struct Config {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    float default_truncation_distance = 0.1;
    float max_weight = 10000.0;
    bool voxel_carving_enabled = true;
    FloatingPoint min_ray_length_m = 0.1;
    FloatingPoint max_ray_length_m = 5.0;
    bool use_const_weight = false;
    bool allow_clear = true;
    bool use_weight_dropoff = true;
    bool use_sparsity_compensation_factor = false;
    float sparsity_compensation_factor = 1.0f;
    size_t integrator_threads = std::thread::hardware_concurrency();

    bool enable_anti_grazing = false;

    float start_voxel_subsampling_factor = 2.0f;
    int max_consecutive_ray_collisions = 2;
    int clear_checks_every_n_frames = 1;
    float max_integration_time_s = std::numeric_limits<float>::max();
  };

  TsdfIntegratorBase(const Config& config, Layer<TsdfVoxel>* layer);

  virtual void integratePointCloud(const Transformation& T_G_C,
                                   const Pointcloud& points_C,
                                   const Colors& colors,
                                   const bool freespace_points = false) = 0;

  const Config& getConfig() const { return config_; }

  void setLayer(Layer<TsdfVoxel>* layer);

 protected:
  inline bool isPointValid(const Point& point_C, const bool freespace_point,
                           bool* is_clearing) const;

  TsdfVoxel* allocateStorageAndGetVoxelPtr(const GlobalIndex& global_voxel_idx,
                                           Block<TsdfVoxel>::Ptr* last_block,
                                           BlockIndex* last_block_idx);

  void updateLayerWithStoredBlocks();

  void updateTsdfVoxel(const Point& origin, const Point& point_G,
                       const GlobalIndex& global_voxel_index,
                       const Color& color, const float weight,
                       TsdfVoxel* tsdf_voxel);

  float computeDistance(const Point& origin, const Point& point_G,
                        const Point& voxel_center) const;

  float getVoxelWeight(const Point& point_C) const;

  Config config_;

  Layer<TsdfVoxel>* layer_;

  // Cached map config.
  FloatingPoint voxel_size_;
  size_t voxels_per_side_;
  FloatingPoint block_size_;

  // Derived types.
  FloatingPoint voxel_size_inv_;
  FloatingPoint voxels_per_side_inv_;
  FloatingPoint block_size_inv_;

  std::mutex temp_block_mutex_;
  Layer<TsdfVoxel>::BlockHashMap temp_block_map_;

  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

  SimpleTsdfIntegrator(const Config& config, Layer<TsdfVoxel>* layer)
      : TsdfIntegratorBase(config, layer) {}

  void integratePointCloud(const Transformation& T_G_C,
                           const Pointcloud& points_C, const Colors& colors,
                           const bool freespace_points = false);

  void integrateFunction(const Transformation& T_G_C,
                         const Pointcloud& points_C, const Colors& colors,
                         const bool freespace_points,
                         ThreadSafeIndex* index_getter);
};

class MergedTsdfIntegrator : public TsdfIntegratorBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  MergedTsdfIntegrator(const Config& config, Layer<TsdfVoxel>* layer)
      : TsdfIntegratorBase(config, layer) {}

  void integratePointCloud(const Transformation& T_G_C,
                           const Pointcloud& points_C, const Colors& colors,
                           const bool freespace_points = false);

 protected:
  void bundleRays(const Transformation& T_G_C, const Pointcloud& points_C,
                  const bool freespace_points, ThreadSafeIndex* index_getter,
                  LongIndexHashMapType<AlignedVector<size_t>>::type* voxel_map,
                  LongIndexHashMapType<AlignedVector<size_t>>::type* clear_map);

  void integrateVoxel(
      const Transformation& T_G_C, const Pointcloud& points_C,
      const Colors& colors, bool enable_anti_grazing, bool clearing_ray,
      const std::pair<GlobalIndex, AlignedVector<size_t>>& kv,
      const LongIndexHashMapType<AlignedVector<size_t>>::type& voxel_map);

  void integrateVoxels(
      const Transformation& T_G_C, const Pointcloud& points_C,
      const Colors& colors, bool enable_anti_grazing, bool clearing_ray,
      const LongIndexHashMapType<AlignedVector<size_t>>::type& voxel_map,
      const LongIndexHashMapType<AlignedVector<size_t>>::type& clear_map,
      size_t thread_idx);

  void integrateRays(
      const Transformation& T_G_C, const Pointcloud& points_C,
      const Colors& colors, bool enable_anti_grazing, bool clearing_ray,
      const LongIndexHashMapType<AlignedVector<size_t>>::type& voxel_map,
      const LongIndexHashMapType<AlignedVector<size_t>>::type& clear_map);
};

class FastTsdfIntegrator : public TsdfIntegratorBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  FastTsdfIntegrator(const Config& config, Layer<TsdfVoxel>* layer)
      : TsdfIntegratorBase(config, layer) {}

  void integrateFunction(const Transformation& T_G_C,
                         const Pointcloud& points_C, const Colors& colors,
                         const bool freespace_points,
                         ThreadSafeIndex* index_getter);

  void integratePointCloud(const Transformation& T_G_C,
                           const Pointcloud& points_C, const Colors& colors,
                           const bool freespace_points = false);

 private:
  static constexpr size_t masked_bits_ = 20;
  static constexpr size_t full_reset_threshold_ = 10000;

  ApproxHashSet<masked_bits_, full_reset_threshold_, GlobalIndex, LongIndexHash>
      start_voxel_approx_set_;

  ApproxHashSet<masked_bits_, full_reset_threshold_, GlobalIndex, LongIndexHash>
      voxel_observed_approx_set_;

  std::chrono::time_point<std::chrono::steady_clock> integration_start_time_;
};

}  // namespace voxblox

#endif  // VOXBLOX_INTEGRATOR_TSDF_INTEGRATOR_H_