Program Listing for File interpolator_inl.h

↰ Return to documentation for file (voxblox/include/voxblox/interpolator/interpolator_inl.h)

#ifndef VOXBLOX_INTERPOLATOR_INTERPOLATOR_INL_H_
#define VOXBLOX_INTERPOLATOR_INTERPOLATOR_INL_H_

#include <iostream>

#include "voxblox/utils/evaluation_utils.h"

namespace voxblox {

template <typename VoxelType>
Interpolator<VoxelType>::Interpolator(const Layer<VoxelType>* layer)
    : layer_(layer) {}

template <typename VoxelType>
bool Interpolator<VoxelType>::getDistance(const Point& pos,
                                          FloatingPoint* distance,
                                          bool interpolate) const {
  if (interpolate) {
    return getInterpDistance(pos, distance);
  } else {
    return getNearestDistance(pos, distance);
  }
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getVoxel(const Point& pos, VoxelType* voxel,
                                       bool interpolate) const {
  if (interpolate) {
    return getInterpVoxel(pos, voxel);
  } else {
    return getNearestVoxel(pos, voxel);
  }
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getGradient(const Point& pos, Point* grad,
                                          const bool interpolate) const {
  CHECK_NOTNULL(grad);

  typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
      layer_->getBlockPtrByCoordinates(pos);
  if (block_ptr == nullptr) {
    return false;
  }
  // Now get the gradient.
  *grad = Point::Zero();
  // Iterate over all 3 D, and over negative and positive signs in central
  // difference.
  for (unsigned int i = 0u; i < 3u; ++i) {
    for (int sign = -1; sign <= 1; sign += 2) {
      Point offset = Point::Zero();
      offset(i) = sign * block_ptr->voxel_size();
      FloatingPoint offset_distance;
      if (!getDistance(pos + offset, &offset_distance, interpolate)) {
        return false;
      }
      (*grad)(i) += offset_distance * static_cast<FloatingPoint>(sign);
    }
  }
  // Scale by correct size.
  // This is central difference, so it's 2x voxel size between measurements.
  *grad /= (2 * block_ptr->voxel_size());
  return true;
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getAdaptiveDistanceAndGradient(
    const Point& pos, FloatingPoint* distance, Point* grad) const {
  // TODO(helenol): try to see how to minimize number of lookups for the
  // gradient and interpolation calculations...

  // Get the nearest neighbor distance first, we need this for the gradient
  // calculations anyway.
  FloatingPoint nearest_neighbor_distance = 0.0f;
  bool interpolate = false;
  if (!getDistance(pos, &nearest_neighbor_distance, interpolate)) {
    // Then there is no data here at all.
    return false;
  }

  // Then try to get the interpolated distance.
  interpolate = true;
  bool has_interpolated_distance = getDistance(pos, distance, interpolate);

  // Now try to estimate the gradient. Same general procedure as getGradient()
  // above, but also allow finite difference methods other than central
  // difference (left difference, right difference).
  typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
      layer_->getBlockPtrByCoordinates(pos);
  if (block_ptr == nullptr) {
    return false;
  }

  Point gradient = Point::Zero();

  // Try to get the full gradient if possible.
  bool has_interpolated_gradient = false;
  if (has_interpolated_distance) {
    has_interpolated_gradient = getGradient(pos, &gradient, interpolate);
  }

  if (!has_interpolated_gradient) {
    // Otherwise fall back to this...
    interpolate = false;
    for (unsigned int i = 0u; i < 3u; ++i) {
      // First check if we can get both sides for central difference.
      Point offset = Point::Zero();
      offset(i) = block_ptr->voxel_size();
      FloatingPoint left_distance = 0.0f, right_distance = 0.0f;
      bool left_valid = getDistance(pos - offset, &left_distance, interpolate);
      bool right_valid =
          getDistance(pos + offset, &right_distance, interpolate);

      if (left_valid && right_valid) {
        gradient(i) =
            (right_distance - left_distance) / (2.0f * block_ptr->voxel_size());
      } else if (left_valid) {
        gradient(i) = (nearest_neighbor_distance - left_distance) /
                      block_ptr->voxel_size();
      } else if (right_valid) {
        gradient(i) = (right_distance - nearest_neighbor_distance) /
                      block_ptr->voxel_size();
      } else {
        // This has no neighbors on any side in this dimension :(
        return false;
      }
    }
  }

  // If we weren't able to get the original interpolated distance value, then
  // use the computed gradient to estimate what the value should be.
  if (!has_interpolated_distance) {
    VoxelIndex voxel_index =
        block_ptr->computeTruncatedVoxelIndexFromCoordinates(pos);
    Point voxel_pos = block_ptr->computeCoordinatesFromVoxelIndex(voxel_index);

    Point voxel_offset = pos - voxel_pos;
    *distance = nearest_neighbor_distance + voxel_offset.dot(gradient);
  }

  *grad = gradient;
  return true;
}

template <typename VoxelType>
bool Interpolator<VoxelType>::setIndexes(const Point& pos,
                                         BlockIndex* block_index,
                                         InterpIndexes* voxel_indexes) const {
  // get voxel index
  *block_index = layer_->computeBlockIndexFromCoordinates(pos);
  typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
      layer_->getBlockPtrByIndex(*block_index);
  if (block_ptr == nullptr) {
    return false;
  }
  VoxelIndex voxel_index =
      block_ptr->computeTruncatedVoxelIndexFromCoordinates(pos);

  // shift index to bottom left corner voxel (makes math easier)
  Point center_offset =
      pos - block_ptr->computeCoordinatesFromVoxelIndex(voxel_index);
  for (size_t i = 0; i < static_cast<size_t>(center_offset.rows()); ++i) {
    // ensure that the point we are interpolating to is always larger than the
    // center of the voxel_index in all dimensions
    if (center_offset(i) < 0) {
      voxel_index(i)--;
      // move blocks if needed
      if (voxel_index(i) < 0) {
        (*block_index)(i)--;
        voxel_index(i) += block_ptr->voxels_per_side();
      }
    }
  }

  // get indexes of neighbors

  // FROM PAPER (http://spie.org/samples/PM159.pdf)
  // clang-format off
  (*voxel_indexes) <<
    0, 0, 0, 0, 1, 1, 1, 1,
    0, 0, 1, 1, 0, 0, 1, 1,
    0, 1, 0, 1, 0, 1, 0, 1;
  // clang-format on

  voxel_indexes->colwise() += voxel_index.array();
  return true;
}

template <typename VoxelType>
void Interpolator<VoxelType>::getQVector(const Point& voxel_pos,
                                         const Point& pos,
                                         const FloatingPoint voxel_size_inv,
                                         InterpVector* q_vector) const {
  CHECK_NOTNULL(q_vector);

  const Point voxel_offset = (pos - voxel_pos) * voxel_size_inv;

  CHECK((voxel_offset.array() >= 0).all());  // NOLINT

  // FROM PAPER (http://spie.org/samples/PM159.pdf)
  // clang-format off
  *q_vector <<
      1,
      voxel_offset[0],
      voxel_offset[1],
      voxel_offset[2],
      voxel_offset[0] * voxel_offset[1],
      voxel_offset[1] * voxel_offset[2],
      voxel_offset[2] * voxel_offset[0],
      voxel_offset[0] * voxel_offset[1] * voxel_offset[2];
  // clang-format on
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getVoxelsAndQVector(
    const BlockIndex& block_index, const InterpIndexes& voxel_indexes,
    const Point& pos, const VoxelType** voxels, InterpVector* q_vector) const {
  CHECK_NOTNULL(q_vector);

  // for each voxel index
  for (size_t i = 0; i < static_cast<size_t>(voxel_indexes.cols()); ++i) {
    typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
        layer_->getBlockPtrByIndex(block_index);
    if (block_ptr == nullptr) {
      return false;
    }

    VoxelIndex voxel_index = voxel_indexes.col(i);
    // if voxel index is too large get neighboring block and update index
    if ((voxel_index.array() >= block_ptr->voxels_per_side()).any()) {
      BlockIndex new_block_index = block_index;
      for (size_t j = 0; j < static_cast<size_t>(block_index.rows()); ++j) {
        if (voxel_index(j) >=
            static_cast<IndexElement>(block_ptr->voxels_per_side())) {
          new_block_index(j)++;
          voxel_index(j) -= block_ptr->voxels_per_side();
        }
      }
      block_ptr = layer_->getBlockPtrByIndex(new_block_index);
      if (block_ptr == nullptr) {
        return false;
      }
    }
    // use bottom left corner voxel to compute weights vector
    if (i == 0) {
      getQVector(block_ptr->computeCoordinatesFromVoxelIndex(voxel_index), pos,
                 block_ptr->voxel_size_inv(), q_vector);
    }

    const VoxelType& voxel = block_ptr->getVoxelByVoxelIndex(voxel_index);

    voxels[i] = &voxel;
    if (!utils::isObservedVoxel(voxel)) {
      return false;
    }
  }
  return true;
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getVoxelsAndQVector(
    const Point& pos, const VoxelType** voxels, InterpVector* q_vector) const {
  // get block and voxels indexes (some voxels may have negative indexes)
  BlockIndex block_index;
  InterpIndexes voxel_indexes;
  if (!setIndexes(pos, &block_index, &voxel_indexes)) {
    return false;
  }

  // get distances of 8 surrounding voxels and weights vector
  return getVoxelsAndQVector(block_index, voxel_indexes, pos, voxels, q_vector);
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getInterpDistance(const Point& pos,
                                                FloatingPoint* distance) const {
  CHECK_NOTNULL(distance);

  // get distances of 8 surrounding voxels and weights vector
  const VoxelType* voxels[8];
  InterpVector q_vector;
  if (!getVoxelsAndQVector(pos, voxels, &q_vector)) {
    return false;
  } else {
    *distance = interpMember(q_vector, voxels, &getVoxelSdf);
    return true;
  }
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getNearestDistance(
    const Point& pos, FloatingPoint* distance) const {
  CHECK_NOTNULL(distance);

  typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
      layer_->getBlockPtrByCoordinates(pos);
  if (block_ptr == nullptr) {
    return false;
  }

  const VoxelType& voxel = block_ptr->getVoxelByCoordinates(pos);

  *distance = getVoxelSdf(voxel);

  return utils::isObservedVoxel(voxel);
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getInterpVoxel(const Point& pos,
                                             VoxelType* voxel) const {
  CHECK_NOTNULL(voxel);

  // get voxels of 8 surrounding voxels and weights vector
  const VoxelType* voxels[8];
  InterpVector q_vector;
  if (!getVoxelsAndQVector(pos, voxels, &q_vector)) {
    return false;
  } else {
    *voxel = interpVoxel(q_vector, voxels);
    return true;
  }
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getNearestVoxel(const Point& pos,
                                              VoxelType* voxel) const {
  CHECK_NOTNULL(voxel);

  typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
      layer_->getBlockPtrByCoordinates(pos);
  if (block_ptr == nullptr) {
    return false;
  }

  *voxel = block_ptr->getVoxelByCoordinates(pos);

  return utils::isObservedVoxel(*voxel);
}

template <typename VoxelType>
bool Interpolator<VoxelType>::getNearestDistanceAndWeight(
    const Point& pos, FloatingPoint* distance, float* weight) const {
  CHECK_NOTNULL(distance);
  CHECK_NOTNULL(weight);

  typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
      layer_->getBlockPtrByCoordinates(pos);
  if (block_ptr == nullptr) {
    return false;
  }
  const VoxelType& voxel = block_ptr->getVoxelByCoordinates(pos);
  *distance = getVoxelSdf(voxel);
  *weight = getVoxelWeight(voxel);
  return true;
}

template <>
inline float Interpolator<TsdfVoxel>::getVoxelSdf(const TsdfVoxel& voxel) {
  return voxel.distance;
}

template <>
inline float Interpolator<EsdfVoxel>::getVoxelSdf(const EsdfVoxel& voxel) {
  return voxel.distance;
}

template <>
inline float Interpolator<TsdfVoxel>::getVoxelWeight(const TsdfVoxel& voxel) {
  return voxel.weight;
}

template <>
inline float Interpolator<EsdfVoxel>::getVoxelWeight(const EsdfVoxel& voxel) {
  return voxel.observed ? 1.0f : 0.0f;
}

template <>
inline uint8_t Interpolator<TsdfVoxel>::getRed(const TsdfVoxel& voxel) {
  return voxel.color.r;
}

template <>
inline uint8_t Interpolator<TsdfVoxel>::getGreen(const TsdfVoxel& voxel) {
  return voxel.color.g;
}

template <>
inline uint8_t Interpolator<TsdfVoxel>::getBlue(const TsdfVoxel& voxel) {
  return voxel.color.b;
}

template <>
inline uint8_t Interpolator<TsdfVoxel>::getAlpha(const TsdfVoxel& voxel) {
  return voxel.color.a;
}

template <typename VoxelType>
template <typename TGetter>
inline FloatingPoint Interpolator<VoxelType>::interpMember(
    const InterpVector& q_vector, const VoxelType** voxels,
    TGetter (*getter)(const VoxelType&)) {
  InterpVector data;
  for (int i = 0; i < data.size(); ++i) {
    data[i] = static_cast<FloatingPoint>((*getter)(*voxels[i]));
  }

  // FROM PAPER (http://spie.org/samples/PM159.pdf)
  // clang-format off
  static const InterpTable interp_table =
      (InterpTable() <<
        1,  0,  0,  0,  0,  0,  0,  0,
       -1,  0,  0,  0,  1,  0,  0,  0,
       -1,  0,  1,  0,  0,  0,  0,  0,
       -1,  1,  0,  0,  0,  0,  0,  0,
        1,  0, -1,  0, -1,  0,  1,  0,
        1, -1, -1,  1,  0,  0,  0,  0,
        1, -1,  0,  0, -1,  1,  0,  0,
       -1,  1,  1, -1,  1, -1, -1,  1
       )
          .finished();
  // clang-format on
  return q_vector * (interp_table * data.transpose());
}

template <>
inline TsdfVoxel Interpolator<TsdfVoxel>::interpVoxel(
    const InterpVector& q_vector, const TsdfVoxel** voxels) {
  TsdfVoxel voxel;
  voxel.distance = interpMember(q_vector, voxels, &getVoxelSdf);
  voxel.weight = interpMember(q_vector, voxels, &getVoxelWeight);

  voxel.color.r = interpMember(q_vector, voxels, &getRed);
  voxel.color.g = interpMember(q_vector, voxels, &getGreen);
  voxel.color.b = interpMember(q_vector, voxels, &getBlue);
  voxel.color.a = interpMember(q_vector, voxels, &getAlpha);

  return voxel;
}

}  // namespace voxblox

#endif  // VOXBLOX_INTERPOLATOR_INTERPOLATOR_INL_H_