SignedDistanceField.hpp

Go to the documentation of this file.

/*
 * SignedDistanceField.h
 *
 *  Created on: Jul 10, 2020
 *      Author: Ruben Grandia
 *   Institute: ETH Zurich
 */

#pragma once

#include <vector>

#include <Eigen/Dense>

#include <grid_map_core/GridMap.hpp>
#include <grid_map_core/TypeDefs.hpp>

#include "Gridmap3dLookup.hpp"

namespace grid_map {

class SignedDistanceField {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Derivative3 = Eigen::Vector3d;

  SignedDistanceField(const GridMap& gridMap, const std::string& elevationLayer, double minHeight, double maxHeight);

  double value(const Position3& position) const noexcept;

  Derivative3 derivative(const Position3& position) const noexcept;

  std::pair<double, Derivative3> valueAndDerivative(const Position3& position) const noexcept;

  size_t size() const noexcept;

  const std::string& getFrameId() const noexcept;

  Time getTime() const noexcept;

  void filterPoints(std::function<void(const Position3&, float, const Derivative3&)> func, size_t decimation = 1) const;

 private:
  void computeSignedDistance(const Matrix& elevation);

  void computeLayerSdfandDeltaX(const Matrix& elevation, Matrix& currentLayer, Matrix& dxTranspose, Matrix& sdfTranspose, Matrix& tmp,
                                Matrix& tmpTranspose, float height, float resolution, float minHeight, float maxHeight) const;

  void emplacebackLayerData(const Matrix& signedDistance, const Matrix& dxTranspose, const Matrix& dy, const Matrix& dz);

  using node_data_t = std::array<float, 4>;

  static double distance(const node_data_t& nodeData) noexcept { return nodeData[0]; }

  static float distanceFloat(const node_data_t& nodeData) noexcept { return nodeData[0]; }

  static Derivative3 derivative(const node_data_t& nodeData) noexcept { return {nodeData[1], nodeData[2], nodeData[3]}; }

  signed_distance_field::Gridmap3dLookup gridmap3DLookup_;

  std::vector<node_data_t> data_;

  std::string frameId_;

  Time timestamp_;
};

}  // namespace grid_map