00001 /* 00002 * SignedDistanceField.hpp 00003 * 00004 * Created on: Aug 16, 2017 00005 * Authors: Takahiro Miki, Peter Fankhauser 00006 * Institute: ETH Zurich, Robotic Systems Lab 00007 */ 00008 00009 #pragma once 00010 00011 #include <grid_map_core/GridMap.hpp> 00012 00013 #include <pcl/point_types.h> 00014 #include <pcl/conversions.h> 00015 00016 #include <string> 00017 #include <vector> 00018 00019 namespace grid_map { 00020 00021 class SignedDistanceField 00022 { 00023 public: 00024 SignedDistanceField(); 00025 virtual ~SignedDistanceField(); 00026 00027 void calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, const double heightClearance); 00028 double getDistanceAt(const Position3& position) const; 00029 Vector3 getDistanceGradientAt(const Position3& position) const; 00030 double getInterpolatedDistanceAt(const Position3& position) const; 00031 void convertToPointCloud(pcl::PointCloud<pcl::PointXYZI>& points) const; 00032 00033 private: 00034 Matrix getPlanarSignedDistanceField(Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>& data) const; 00035 00036 double resolution_; 00037 Size size_; 00038 Position position_; 00039 std::vector<Matrix> data_; 00040 float zIndexStartHeight_; 00041 float maxDistance_; 00042 const float lowestHeight_; 00043 }; 00044 00045 } /* namespace */