| 30 | } |
| 31 | |
| 32 | void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, |
| 33 | const double heightClearance) |
| 34 | { |
| 35 | data_.clear(); |
| 36 | resolution_ = gridMap.getResolution(); |
| 37 | position_ = gridMap.getPosition(); |
| 38 | size_ = gridMap.getSize(); |
| 39 | Matrix map = gridMap.get(layer); // Copy! |
| 40 | |
| 41 | float minHeight = map.minCoeffOfFinites(); |
| 42 | if (!std::isfinite(minHeight)) minHeight = lowestHeight_; |
| 43 | float maxHeight = map.maxCoeffOfFinites(); |
| 44 | if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_; |
| 45 | |
| 46 | const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option). |
| 47 | for (size_t i = 0; i < map.size(); ++i) { |
| 48 | if (std::isnan(map(i))) map(i) = valueForEmptyCells; |
| 49 | } |
| 50 | |
| 51 | // Height range of the signed distance field is higher than the max height. |
| 52 | maxHeight += heightClearance; |
| 53 | |
| 54 | Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_; |
| 55 | Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols()); |
| 56 | std::vector<Matrix> sdf; |
| 57 | zIndexStartHeight_ = minHeight; |
| 58 | |
| 59 | // Calculate signed distance field from bottom. |
| 60 | for (float h = minHeight; h < maxHeight; h += resolution_) { |
| 61 | Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h; |
| 62 | Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1; |
| 63 | Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField); |
| 64 | Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField); |
| 65 | Matrix sdf2d; |
| 66 | // If 2d sdfObstacleFree calculation failed, neglect this SDF |
| 67 | // to avoid extreme small distances (-INF). |
| 68 | if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle; |
| 69 | else sdf2d = sdfObstacle - sdfObstacleFree; |
| 70 | sdf2d *= resolution_; |
| 71 | for (size_t i = 0; i < sdfElevationAbove.size(); ++i) { |
| 72 | if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i); |
| 73 | else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_; |
| 74 | if (sdf2d(i) == 0) sdfLayer(i) = h - map(i); |
| 75 | else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h)); |
| 76 | else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i)); |
| 77 | } |
| 78 | data_.push_back(sdfLayer); |
| 79 | } |
| 80 | } |
| 81 | |
| 82 | grid_map::Matrix SignedDistanceField::getPlanarSignedDistanceField(Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>& data) const |
| 83 | { |