Program Listing for File esdf_integrator.h

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

#ifndef VOXBLOX_INTEGRATOR_ESDF_INTEGRATOR_H_
#define VOXBLOX_INTEGRATOR_ESDF_INTEGRATOR_H_

#include <glog/logging.h>
#include <Eigen/Core>
#include <algorithm>
#include <queue>
#include <utility>
#include <vector>

#include "voxblox/core/layer.h"
#include "voxblox/core/voxel.h"
#include "voxblox/integrator/integrator_utils.h"
#include "voxblox/utils/bucket_queue.h"
#include "voxblox/utils/neighbor_tools.h"
#include "voxblox/utils/timing.h"

namespace voxblox {

class EsdfIntegrator {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  struct Config {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    bool full_euclidean_distance = false;
    FloatingPoint max_distance_m = 2.0;
    FloatingPoint min_distance_m = 0.2;
    FloatingPoint default_distance_m = 2.0;
    FloatingPoint min_diff_m = 0.001;
    float min_weight = 1e-6;
    int num_buckets = 20;
    bool multi_queue = false;
    bool add_occupied_crust = false;

    FloatingPoint clear_sphere_radius = 1.5;
    FloatingPoint occupied_sphere_radius = 5.0;
  };

  EsdfIntegrator(const Config& config, Layer<TsdfVoxel>* tsdf_layer,
                 Layer<EsdfVoxel>* esdf_layer);

  void addNewRobotPosition(const Point& position);

  void updateFromTsdfLayerBatch();
  void updateFromTsdfLayer(bool clear_updated_flag);

  void updateFromTsdfBlocks(const BlockIndexList& tsdf_blocks,
                            bool incremental = false);

  void processRaiseSet();

  void processOpenSet();

  bool updateVoxelFromNeighbors(const GlobalIndex& global_index);

  // Convenience functions.
  inline bool isFixed(FloatingPoint dist_m) const {
    return std::abs(dist_m) < config_.min_distance_m;
  }
  void clear() {
    updated_blocks_.clear();
    open_.clear();
    raise_ = AlignedQueue<GlobalIndex>();
  }
  float getEsdfMaxDistance() const { return config_.max_distance_m; }
  void setEsdfMaxDistance(float max_distance) {
    config_.max_distance_m = max_distance;
    if (config_.default_distance_m < max_distance) {
      config_.default_distance_m = max_distance;
    }
  }
  bool getFullEuclidean() const { return config_.full_euclidean_distance; }
  void setFullEuclidean(bool full_euclidean) {
    config_.full_euclidean_distance = full_euclidean;
  }

 protected:
  Config config_;

  Layer<TsdfVoxel>* tsdf_layer_;
  Layer<EsdfVoxel>* esdf_layer_;

  BucketQueue<GlobalIndex> open_;

  AlignedQueue<GlobalIndex> raise_;

  size_t voxels_per_side_;
  FloatingPoint voxel_size_;

  IndexSet updated_blocks_;
};

}  // namespace voxblox

#endif  // VOXBLOX_INTEGRATOR_ESDF_INTEGRATOR_H_