Program Listing for File neighbor_tools.h

Return to documentation for file (voxblox/include/voxblox/utils/neighbor_tools.h)

#ifndef VOXBLOX_UTILS_NEIGHBOR_TOOLS_H_
#define VOXBLOX_UTILS_NEIGHBOR_TOOLS_H_

#include "voxblox/core/common.h"
#include "voxblox/core/layer.h"

namespace voxblox {

enum Connectivity : unsigned int {
  kSix = 6u,
  kEighteen = 18u,
  kTwentySix = 26u
};

class NeighborhoodLookupTables {
 public:
  typedef Eigen::Matrix<LongIndexElement, 3, Connectivity::kTwentySix>
      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 = Connectivity::kTwentySix>
class Neighborhood : public NeighborhoodLookupTables {
 public:
  typedef Eigen::Matrix<LongIndexElement, 3, kConnectivity> IndexMatrix;

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

  static void getFromBlockAndVoxelIndexAndDirection(
      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) {
    CHECK_GT(voxels_per_side, 0u);
    CHECK_NOTNULL(neighbor_block_index);
    CHECK_NOTNULL(neighbor_voxel_index);

    *neighbor_block_index = block_index;
    *neighbor_voxel_index = voxel_index + direction;

    for (unsigned int i = 0u; i < 3u; ++i) {
      while ((*neighbor_voxel_index)(i) < 0) {
        (*neighbor_block_index)(i)--;
        (*neighbor_voxel_index)(i) += voxels_per_side;
      }
      while ((*neighbor_voxel_index)(i) >=
             static_cast<IndexElement>(voxels_per_side)) {
        (*neighbor_block_index)(i)++;
        (*neighbor_voxel_index)(i) -= voxels_per_side;
      }
    }
  }

  static void getFromBlockAndVoxelIndex(
      const BlockIndex& block_index, const VoxelIndex& voxel_index,
      const size_t voxels_per_side, AlignedVector<VoxelKey>* neighbors_ptr) {
    CHECK_NOTNULL(neighbors_ptr)->resize(kConnectivity);

    AlignedVector<VoxelKey>& neighbors = *neighbors_ptr;
    for (unsigned int i = 0u; i < kConnectivity; ++i) {
      VoxelKey& neighbor = neighbors[i];
      getFromBlockAndVoxelIndexAndDirection(block_index, voxel_index,
                                            kOffsets.col(i), voxels_per_side,
                                            &neighbor.first, &neighbor.second);
    }
  }

  static SignedIndex getOffsetBetweenVoxels(const BlockIndex& start_block_index,
                                            const VoxelIndex& start_voxel_index,
                                            const BlockIndex& end_block_index,
                                            const VoxelIndex& end_voxel_index,
                                            const size_t voxels_per_side) {
    CHECK_NE(voxels_per_side, 0u);
    return (end_voxel_index - start_voxel_index) +
           (end_block_index - start_block_index) * voxels_per_side;
  }
};
}  // namespace voxblox

#endif  // VOXBLOX_UTILS_NEIGHBOR_TOOLS_H_