Program Listing for File esdf_occ_integrator.h

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

#ifndef VOXBLOX_INTEGRATOR_ESDF_OCC_INTEGRATOR_H_
#define VOXBLOX_INTEGRATOR_ESDF_OCC_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/timing.h"

namespace voxblox {

class EsdfOccIntegrator {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  struct Config {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    FloatingPoint max_distance_m = 2.0;
    FloatingPoint default_distance_m = 2.0;
    int num_buckets = 20;
  };

  EsdfOccIntegrator(const Config& config, Layer<OccupancyVoxel>* occ_layer,
                    Layer<EsdfVoxel>* esdf_layer);

  void updateFromOccLayerBatch();
  void updateFromOccBlocks(const BlockIndexList& occ_blocks);

  void processOpenSet();

  void getNeighborsAndDistances(
      const BlockIndex& block_index, const VoxelIndex& voxel_index,
      AlignedVector<VoxelKey>* neighbors, AlignedVector<float>* distances,
      AlignedVector<Eigen::Vector3i>* directions) const;
  void getNeighbor(const BlockIndex& block_index, const VoxelIndex& voxel_index,
                   const Eigen::Vector3i& direction,
                   BlockIndex* neighbor_block_index,
                   VoxelIndex* neighbor_voxel_index) const;

 protected:
  Config config_;

  Layer<OccupancyVoxel>* occ_layer_;
  Layer<EsdfVoxel>* esdf_layer_;

  BucketQueue<VoxelKey> open_;

  AlignedQueue<VoxelKey> raise_;

  size_t esdf_voxels_per_side_;
  FloatingPoint esdf_voxel_size_;
};

}  // namespace voxblox

#endif  // VOXBLOX_INTEGRATOR_ESDF_OCC_INTEGRATOR_H_