SignedDistanceField.cpp
Go to the documentation of this file.
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 #include <grid_map_sdf/SignedDistanceField.hpp>
00010 #include <grid_map_sdf/distance_transform/dt.h>
00011 
00012 #include <grid_map_core/GridMap.hpp>
00013 
00014 #include <limits>
00015 
00016 using namespace distance_transform;
00017 
00018 namespace grid_map {
00019 
00020 SignedDistanceField::SignedDistanceField()
00021     : maxDistance_(std::numeric_limits<float>::max()),
00022       zIndexStartHeight_(0.0),
00023       resolution_(0.0),
00024       lowestHeight_(-1e5) // We need some precision.
00025 {
00026 }
00027 
00028 SignedDistanceField::~SignedDistanceField()
00029 {
00030 }
00031 
00032 void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer,
00033                                                        const double heightClearance)
00034 {
00035   data_.clear();
00036   resolution_ = gridMap.getResolution();
00037   position_ = gridMap.getPosition();
00038   size_ = gridMap.getSize();
00039   Matrix map = gridMap.get(layer); // Copy!
00040 
00041   float minHeight = map.minCoeffOfFinites();
00042   if (!std::isfinite(minHeight)) minHeight = lowestHeight_;
00043   float maxHeight = map.maxCoeffOfFinites();
00044   if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_;
00045 
00046   const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option).
00047   for (size_t i = 0; i < map.size(); ++i) {
00048     if (std::isnan(map(i))) map(i) = valueForEmptyCells;
00049   }
00050 
00051   // Height range of the signed distance field is higher than the max height.
00052   maxHeight += heightClearance;
00053 
00054   Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_;
00055   Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols());
00056   std::vector<Matrix> sdf;
00057   zIndexStartHeight_ = minHeight;
00058 
00059   // Calculate signed distance field from bottom.
00060   for (float h = minHeight; h < maxHeight; h += resolution_) {
00061     Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h;
00062     Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1;
00063     Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField);
00064     Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField);
00065     Matrix sdf2d;
00066     // If 2d sdfObstacleFree calculation failed, neglect this SDF
00067     // to avoid extreme small distances (-INF).
00068     if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle;
00069     else sdf2d = sdfObstacle - sdfObstacleFree;
00070     sdf2d *= resolution_;
00071     for (size_t i = 0; i < sdfElevationAbove.size(); ++i) {
00072       if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i);
00073       else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_;
00074       if (sdf2d(i) == 0) sdfLayer(i) = h - map(i);
00075       else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h));
00076       else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));
00077     }
00078     data_.push_back(sdfLayer);
00079   }
00080 }
00081 
00082 grid_map::Matrix SignedDistanceField::getPlanarSignedDistanceField(Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>& data) const
00083 {
00084   image<uchar> *input = new image<uchar>(data.rows(), data.cols(), true);
00085 
00086   for (int y = 0; y < input->height(); y++) {
00087     for (int x = 0; x < input->width(); x++) {
00088       imRef(input, x, y) = data(x, y);
00089     }
00090   }
00091 
00092   // Compute dt.
00093   image<float> *out = dt(input);
00094 
00095   Matrix result(data.rows(), data.cols());
00096 
00097   // Take square roots.
00098   for (int y = 0; y < out->height(); y++) {
00099     for (int x = 0; x < out->width(); x++) {
00100       result(x, y) = sqrt(imRef(out, x, y));
00101     }
00102   }
00103   return result;
00104 }
00105 
00106 double SignedDistanceField::getDistanceAt(const Position3& position) const
00107 {
00108   double xCenter = size_.x() / 2.0;
00109   double yCenter = size_.y() / 2.0;
00110   int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
00111   int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
00112   int k = std::round((position.z() - zIndexStartHeight_) / resolution_);
00113   i = std::max(i, 0);
00114   i = std::min(i, size_.x() - 1);
00115   j = std::max(j, 0);
00116   j = std::min(j, size_.y() - 1);
00117   k = std::max(k, 0);
00118   k = std::min(k, (int)data_.size() - 1);
00119   return data_[k](i, j);
00120 }
00121 
00122 double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) const
00123 {
00124   double xCenter = size_.x() / 2.0;
00125   double yCenter = size_.y() / 2.0;
00126   int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
00127   int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
00128   int k = std::round((position.z() - zIndexStartHeight_) / resolution_);
00129   i = std::max(i, 0);
00130   i = std::min(i, size_.x() - 1);
00131   j = std::max(j, 0);
00132   j = std::min(j, size_.y() - 1);
00133   k = std::max(k, 0);
00134   k = std::min(k, (int)data_.size() - 1);
00135   Vector3 gradient = getDistanceGradientAt(position);
00136   double xp = position_.x() + ((size_.x() - i) - xCenter) * resolution_;
00137   double yp = position_.y() + ((size_.y() - j) - yCenter) * resolution_;
00138   double zp = zIndexStartHeight_ + k * resolution_;
00139   Vector3 error = position - Vector3(xp, yp, zp);
00140   return data_[k](i, j) + gradient.dot(error);
00141 }
00142 
00143 Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) const
00144 {
00145   double xCenter = size_.x() / 2.0;
00146   double yCenter = size_.y() / 2.0;
00147   int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
00148   int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
00149   int k = std::round((position.z() - zIndexStartHeight_) / resolution_);
00150   i = std::max(i, 1);
00151   i = std::min(i, size_.x() - 2);
00152   j = std::max(j, 1);
00153   j = std::min(j, size_.y() - 2);
00154   k = std::max(k, 1);
00155   k = std::min(k, (int)data_.size() - 2);
00156   double dx = (data_[k](i - 1, j) - data_[k](i + 1, j)) / (2 * resolution_);
00157   double dy = (data_[k](i, j - 1) - data_[k](i, j + 1)) / (2 * resolution_);
00158   double dz = (data_[k + 1](i, j) - data_[k - 1](i, j)) / (2 * resolution_);
00159   return Vector3(dx, dy, dz);
00160 }
00161 
00162 void SignedDistanceField::convertToPointCloud(pcl::PointCloud<pcl::PointXYZI>& points) const
00163 {
00164   double xCenter = size_.x() / 2.0;
00165   double yCenter = size_.y() / 2.0;
00166   for (int z = 0; z < data_.size(); z++){
00167     for (int y = 0; y < size_.y(); y++) {
00168       for (int x = 0; x < size_.x(); x++) {
00169         double xp = position_.x() + ((size_.x() - x) - xCenter) * resolution_;
00170         double yp = position_.y() + ((size_.y() - y) - yCenter) * resolution_;
00171         double zp = zIndexStartHeight_ + z * resolution_;
00172         pcl::PointXYZI p;
00173         p.x = xp;
00174         p.y = yp;
00175         p.z = zp;
00176         p.intensity = data_[z](x, y);
00177         points.push_back(p);
00178       }
00179     }
00180   }
00181   return;
00182 }
00183 
00184 } /* namespace */


grid_map_sdf
Author(s): Takahiro Miki , Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:34