fast_terrain_map.h

#ifndef FAST_TERRAIN_MAP_H
#define FAST_TERRAIN_MAP_H
 
#include <quad_utils/function_timer.h>
#include <ros/ros.h>
 
#include <chrono>
#include <eigen3/Eigen/Eigen>
#include <grid_map_core/grid_map_core.hpp>
 
 
class FastTerrainMap {
 public:
  FastTerrainMap();
 
  void loadData(int x_size, int y_size, std::vector<double> x_data,
                std::vector<double> y_data,
                std::vector<std::vector<double>> z_data,
                std::vector<std::vector<double>> nx_data,
                std::vector<std::vector<double>> ny_data,
                std::vector<std::vector<double>> nz_data,
                std::vector<std::vector<double>> z_data_filt,
                std::vector<std::vector<double>> nx_data_filt,
                std::vector<std::vector<double>> ny_data_filt,
                std::vector<std::vector<double>> nz_data_filt);
 
  void loadFlat();
 
  void loadFlatElevated(double height);
 
  void loadSlope(double grade);
 
  void loadStep(double height);
 
  void loadDataFromGridMap(const grid_map::GridMap map);
 
  bool isInRange(const double x, const double y) const;
 
  double getGroundHeight(const double x, const double y) const;
 
  std::array<double, 3> getSurfaceNormal(const double x, const double y) const;
 
  double getGroundHeightFiltered(const double x, const double y) const;
 
  std::array<double, 3> getSurfaceNormalFiltered(const double x,
                                                 const double y) const;
 
  Eigen::Vector3d getSurfaceNormalFilteredEigen(const double x,
                                                const double y) const;
 
  Eigen::Vector3d projectToMap(const Eigen::Vector3d point,
                               const Eigen::Vector3d direction);
 
  std::vector<double> getXData() const;
 
  std::vector<double> getYData() const;
 
  bool isEmpty() const;
 
 private:
  inline int getXIndex(const double x) const {
    return std::max(
        std::min((int)floor((x - x_data_[0]) / x_diff_), x_size_ - 2), 0);
  }
 
  inline int getYIndex(const double y) const {
    return std::max(
        std::min((int)floor((y - y_data_[0]) / y_diff_), y_size_ - 2), 0);
  }
 
  int x_size_ = 0;
 
  int y_size_ = 0;
 
  double x_diff_;
 
  double y_diff_;
 
  std::vector<double> x_data_;
 
  std::vector<double> y_data_;
 
  std::vector<std::vector<double>> z_data_;
 
  std::vector<std::vector<double>> nx_data_;
 
  std::vector<std::vector<double>> ny_data_;
 
  std::vector<std::vector<double>> nz_data_;
 
  std::vector<std::vector<double>> z_data_filt_;
 
  std::vector<std::vector<double>> nx_data_filt_;
 
  std::vector<std::vector<double>> ny_data_filt_;
 
  std::vector<std::vector<double>> nz_data_filt_;
};
 
#endif  // FAST_TERRAIN_MAP_H