diff --git a/grid_map_sdf/CMakeLists.txt b/grid_map_sdf/CMakeLists.txt index 9294fb3e3..068c6293f 100644 --- a/grid_map_sdf/CMakeLists.txt +++ b/grid_map_sdf/CMakeLists.txt @@ -39,6 +39,7 @@ set(dependencies ## Declare a cpp library add_library(${PROJECT_NAME} + src/SignedDistance2d.cpp src/SignedDistanceField.cpp ) diff --git a/grid_map_sdf/include/grid_map_sdf/PixelBorderDistance.hpp b/grid_map_sdf/include/grid_map_sdf/PixelBorderDistance.hpp new file mode 100644 index 000000000..5cd2baebf --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/PixelBorderDistance.hpp @@ -0,0 +1,104 @@ +/* + * PixelBorderDistance.h + * + * Created on: Aug 7, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +#include +#include +#include + +namespace grid_map { +namespace signed_distance_field { + +/** + * Returns distance between the center of a pixel and the border of an other pixel. + * Returns zero if the center is inside the other pixel. + * Pixels are assumed to have size 1.0F + * @param i : location of pixel 1 + * @param j : location of pixel 2 + * @return : absolute distance between center of pixel 1 and the border of pixel 2 + */ +inline float pixelBorderDistance(float i, float j) { + return std::max(std::abs(i - j) - 0.5F, 0.0F); +} + +/** + * Returns square pixelBorderDistance, adding offset f. + */ +inline float squarePixelBorderDistance(float i, float j, float f) { + const float d{pixelBorderDistance(i, j)}; + return d * d + f; +} + +namespace internal { + +/** + * Return equidistancepoint between origin and pixel p (with p > 0) with offset fp + */ +inline float intersectionPointRightSideOfOrigin(float p, float fp) { + /* + * There are 5 different solutions + * In decreasing order: + * sol 1 in [p^2, inf] + * sol 2 in [bound, p^2] + * sol 3 in [-bound, bound] + * sol 4 in [-p^2, -bound] + * sol 5 in [-inf, -p^2] + */ + const float pSquared{p * p}; + if (fp > pSquared) { + return (pSquared + p + fp) / (2.0F * p); // sol 1 + } else if (fp < -pSquared) { + return (pSquared - p + fp) / (2.0F * p); // sol 5 + } else { + const float bound{pSquared - 2.0F * p + 1.0F}; // Always positive because (p > 0) + if (fp > bound) { + return 0.5F + std::sqrt(fp); // sol 2 + } else if (fp < -bound) { + return p - 0.5F - std::sqrt(-fp); // sol 4 + } else { + return (pSquared - p + fp) / (2.0F * p - 2.0F); // sol 3 + } + } +} + +/** + * Return equidistancepoint between origin and pixel p with offset fp + */ +inline float intersectionOffsetFromOrigin(float p, float fp) { + if (p > 0.0F) { + return intersectionPointRightSideOfOrigin(p, fp); + } else { + // call with negative p and flip the result + return -intersectionPointRightSideOfOrigin(-p, fp); + } +} + +} // namespace internal + +/** + * Return the point s in pixel space that is equally far from p and q (taking into account offsets fp, and fq) + * It is the solution to the following equation: + * squarePixelBorderDistance(s, q, fq) == squarePixelBorderDistance(s, p, fp) + */ +inline float equidistancePoint(float q, float fq, float p, float fp) { + assert(q == p || + std::abs(q - p) >= 1.0F); // Check that q and p are proper pixel locations: either the same pixel or non-overlapping pixels + assert((q == p) ? fp == fq : true); // Check when q and p are equal, the offsets are also equal + + if (fp == fq) { // quite common case when both pixels are of the same class (occupied / free) + return 0.5F * (p + q); + } else { + float df{fp - fq}; + float dr{p - q}; + return internal::intersectionOffsetFromOrigin(dr, df) + q; + } +} + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/SignedDistance2d.hpp b/grid_map_sdf/include/grid_map_sdf/SignedDistance2d.hpp new file mode 100644 index 000000000..1646d0bbb --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/SignedDistance2d.hpp @@ -0,0 +1,59 @@ +/* + * SignedDistance2d.h + * + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +#include + +#include + +#include "Utils.hpp" + +namespace grid_map { +namespace signed_distance_field { + +/** + * Computes the signed distance field at a specified height for a given elevation map. + * + * @param elevationMap : elevation data. + * @param height : height to generate the signed distance at. + * @param resolution : resolution of the elevation map. (The true distance [m] between cells in world frame) + * @param minHeight : the lowest height contained in elevationMap + * @param maxHeight : the maximum height contained in elevationMap + * @return The signed distance field at the query height. + */ +Matrix signedDistanceAtHeight(const Matrix& elevationMap, float height, float resolution, float minHeight, float maxHeight); + +/** + * Same as above, but returns the sdf in transposed form. + * Also takes temporary variables from outside to prevent memory allocation. + * + * @param elevationMap : elevation data. + * @param sdfTranspose : [output] resulting sdf in transposed form (automatically allocated if of wrong size) + * @param tmp : temporary of size elevationMap (automatically allocated if of wrong size) + * @param tmpTranspose : temporary of size elevationMap transpose (automatically allocated if of wrong size) + * @param height : height to generate the signed distance at. + * @param resolution : resolution of the elevation map. (The true distance [m] between cells in world frame) + * @param minHeight : the lowest height contained in elevationMap + * @param maxHeight : the maximum height contained in elevationMap + */ +void signedDistanceAtHeightTranspose(const Matrix& elevationMap, Matrix& sdfTranspose, Matrix& tmp, Matrix& tmpTranspose, float height, + float resolution, float minHeight, float maxHeight); + +/** + * Gets the 2D signed distance from an occupancy grid. + * Returns +INF if there are no obstacles, and -INF if there are only obstacles + * + * @param occupancyGrid : occupancy grid with true = obstacle, false = free space + * @param resolution : resolution of the grid. + * @return signed distance for each point in the grid to the occupancy border. + */ +Matrix signedDistanceFromOccupancy(const Eigen::Matrix& occupancyGrid, float resolution); + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/Utils.hpp b/grid_map_sdf/include/grid_map_sdf/Utils.hpp new file mode 100644 index 000000000..32d03c452 --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/Utils.hpp @@ -0,0 +1,21 @@ +/* + * Utils.h + * + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +namespace grid_map { +namespace signed_distance_field { + +// Check existence of inf +static_assert(std::numeric_limits::has_infinity, "float does not support infinity"); + +//! Distance value that is considered infinite. +constexpr float INF = std::numeric_limits::infinity(); + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/src/SignedDistance2d.cpp b/grid_map_sdf/src/SignedDistance2d.cpp new file mode 100644 index 000000000..c3ee519f5 --- /dev/null +++ b/grid_map_sdf/src/SignedDistance2d.cpp @@ -0,0 +1,290 @@ +/* +* SignedDistance2d.cpp +* +* Created on: Jul 10, 2020 +* Author: Ruben Grandia +* Institute: ETH Zurich +*/ + +#include "grid_map_sdf/SignedDistance2d.hpp" + +#include "grid_map_sdf/PixelBorderDistance.hpp" + +namespace grid_map { +namespace signed_distance_field { + +namespace internal { +struct DistanceLowerBound { + float v; // origin of bounding function + float f; // functional offset at the origin + float z_lhs; // lhs of interval where this bound holds + float z_rhs; // rhs of interval where this lower bound holds +}; + +/** +* 1D Euclidean Distance Transform based on: http://cs.brown.edu/people/pfelzens/dt/ +* Adapted to work on Eigen objects directly +* Optimized computation of s. +* Some optimization to not keep track of bounds that lie fully outside the grid. +*/ +std::vector::iterator fillLowerBounds(const Eigen::Ref& squareDistance1d, + std::vector& lowerBounds, Eigen::Index start) { + const auto n = squareDistance1d.size(); + const auto nFloat = static_cast(n); + const auto startFloat = static_cast(start); + + // Initialize + auto rhsBoundIt = lowerBounds.begin(); + *rhsBoundIt = DistanceLowerBound{startFloat, squareDistance1d[start], -INF, INF}; + auto firstBoundIt = lowerBounds.begin(); + + // Compute bounds to the right of minimum + float qFloat = rhsBoundIt->v + 1.0F; + for (Eigen::Index q = start + 1; q < n; ++q) { + // Storing this by value gives better performance (removed indirection?) + const float fq = squareDistance1d[q]; + + float s = equidistancePoint(qFloat, fq, rhsBoundIt->v, rhsBoundIt->f); + if (s < nFloat) { // Can ignore the lower bound derived from this point if it is only active outsize of [start, n] + // Search backwards in bounds until s is within [z_lhs, z_rhs] + while (s < rhsBoundIt->z_lhs) { + --rhsBoundIt; + s = equidistancePoint(qFloat, fq, rhsBoundIt->v, rhsBoundIt->f); + } + if (s > startFloat) { // Intersection is within [start, n]. Adjust current lowerbound and insert the new one after + rhsBoundIt->z_rhs = s; // Update the bound that comes before + ++rhsBoundIt; // insert new bound after. + *rhsBoundIt = DistanceLowerBound{qFloat, fq, s, INF}; // Valid from s till infinity + } else { // Intersection is outside [0, n]. This means that the new bound dominates all previous bounds + *rhsBoundIt = DistanceLowerBound{qFloat, fq, -INF, INF}; + firstBoundIt = rhsBoundIt; // No need to keep other bounds, so update the first bound iterator to this one. + } + } + + // Increment to follow loop counter as float + qFloat += 1.0F; + } + + return firstBoundIt; +} + +void extractSquareDistances(Eigen::Ref squareDistance1d, std::vector::const_iterator lowerBoundIt, + Eigen::Index start) { + const auto n = squareDistance1d.size(); + + // Store active bound by value to remove indirection + auto lastz = lowerBoundIt->z_rhs; + + auto qFloat = static_cast(start); + for (Eigen::Index q = start; q < n; ++q) { + if (squareDistance1d[q] > 0.0F) { // Observe that if squareDistance1d[q] == 0.0, this is already the minimum and it will stay 0.0 + // Find the new active lower bound if q no longer belongs to current interval + if (qFloat > lastz) { + do { + ++lowerBoundIt; + } while (lowerBoundIt->z_rhs < qFloat); + lastz = lowerBoundIt->z_rhs; + } + + squareDistance1d[q] = squarePixelBorderDistance(qFloat, lowerBoundIt->v, lowerBoundIt->f); + } + + qFloat += 1.0F; + } +} + +/** +* Same as extractSquareDistances, but takes the sqrt as a final step. +* Because several cells will have a value of 0.0 (obstacle / free space label), we can skip the sqrt computation for those. +*/ +void extractDistances(Eigen::Ref squareDistance1d, + std::vector::const_iterator lowerBoundIt, Eigen::Index start) { + const auto n = squareDistance1d.size(); + + // Store active bound by value to remove indirection + auto lastz = lowerBoundIt->z_rhs; + + auto qFloat = static_cast(start); + for (Eigen::Index q = start; q < n; ++q) { + if (squareDistance1d[q] > 0.0F) { // Observe that if squareDistance1d[q] == 0.0, this is already the minimum and it will stay 0.0 + // Find the new active lower bound if q no longer belongs to current interval + if (qFloat > lastz) { + do { + ++lowerBoundIt; + } while (lowerBoundIt->z_rhs < qFloat); + lastz = lowerBoundIt->z_rhs; + } + + squareDistance1d[q] = std::sqrt(squarePixelBorderDistance(qFloat, lowerBoundIt->v, lowerBoundIt->f)); + } + + qFloat += 1.0F; + } +} + +/** +* Find the location of the last zero value from the front +*/ +Eigen::Index lastZeroFromFront(const Eigen::Ref& squareDistance1d) { + const auto n = squareDistance1d.size(); + + for (Eigen::Index q = 0; q < n; ++q) { + if (squareDistance1d[q] > 0.0F) { + if (q > 0) { + return q - 1; + } else { + return 0; + } + } + } + return n; +} + +inline void squaredDistanceTransform_1d_inplace(Eigen::Ref squareDistance1d, + std::vector& lowerBounds) { + auto start = lastZeroFromFront(squareDistance1d); + + // Only need to process line if there are nonzero elements. Also the first zeros stay untouched. + if (start < squareDistance1d.size()) { + auto startIt = fillLowerBounds(squareDistance1d, lowerBounds, start); + extractSquareDistances(squareDistance1d, startIt, start); + } +} + +/** +* Same as above, but takes sqrt as final step (within the same loop) +* @param squareDistance1d : input as squared distance, output is the distance after sqrt. +* @param lowerBounds : work vector +*/ +inline void distanceTransform_1d_inplace(Eigen::Ref squareDistance1d, std::vector& lowerBounds) { + auto start = lastZeroFromFront(squareDistance1d); + + // Only need to process line if there are nonzero elements. Also the first zeros stay untouched. + if (start < squareDistance1d.size()) { + auto startIt = fillLowerBounds(squareDistance1d, lowerBounds, start); + extractDistances(squareDistance1d, startIt, start); + } +} + +void computePixelDistance2dTranspose(Matrix& input, Matrix& distanceTranspose) { + const auto n = input.rows(); + const auto m = input.cols(); + + // Allocate a buffer big enough for processing both rowise and columnwise + std::vector lowerBounds(std::max(n, m)); + + // Process columns + for (Eigen::Index i = 0; i < m; ++i) { + squaredDistanceTransform_1d_inplace(input.col(i), lowerBounds); + } + + // Process rows (= columns after transpose). + distanceTranspose = input.transpose(); + for (Eigen::Index i = 0; i < n; ++i) { + // Fuses square distance algorithm and taking sqrt. + distanceTransform_1d_inplace(distanceTranspose.col(i), lowerBounds); + } +} + +// Initialize with square distance in height direction in pixel units if above the surface +void initializeObstacleDistance(const Matrix& elevationMap, Matrix& result, float height, float resolution) { + /* Vectorized implementation of: + * if (height > elevation) { + * const auto diff = (height - elevation) / resolution; + * return diff * diff; + * } else { + * return 0.0F; + * } + */ + result = ((1.0F / resolution) * (height - elevationMap.array()).cwiseMax(0.0F)).square(); +} + +// Initialize with square distance in height direction in pixel units if below the surface +void initializeObstacleFreeDistance(const Matrix& elevationMap, Matrix& result, float height, float resolution) { + /* Vectorized implementation of: + * if (height < elevation) { + * const auto diff = (height - elevation) / resolution; + * return diff * diff; + * } else { + * return 0.0F; + * } + */ + result = ((1.0F / resolution) * (height - elevationMap.array()).cwiseMin(0.0F)).square(); +} + +void pixelDistanceToFreeSpaceTranspose(const Matrix& elevationMap, Matrix& sdfObstacleFree, Matrix& tmp, float height, float resolution) { + internal::initializeObstacleFreeDistance(elevationMap, tmp, height, resolution); + internal::computePixelDistance2dTranspose(tmp, sdfObstacleFree); +} + +void pixelDistanceToObstacleTranspose(const Matrix& elevationMap, Matrix& sdfObstacleTranspose, Matrix& tmp, float height, + float resolution) { + internal::initializeObstacleDistance(elevationMap, tmp, height, resolution); + internal::computePixelDistance2dTranspose(tmp, sdfObstacleTranspose); +} + +Matrix signedDistanceFromOccupancyTranspose(const Eigen::Matrix& occupancyGrid, float resolution) { + // Compute pixel distance to obstacles + Matrix sdfObstacle; + Matrix init = occupancyGrid.unaryExpr([=](bool val) { return (val) ? 0.0F : INF; }); + internal::computePixelDistance2dTranspose(init, sdfObstacle); + + // Compute pixel distance to obstacle free space + Matrix sdfObstacleFree; + init = occupancyGrid.unaryExpr([=](bool val) { return (val) ? INF : 0.0F; }); + internal::computePixelDistance2dTranspose(init, sdfObstacleFree); + + return resolution * (sdfObstacle - sdfObstacleFree); +} + +} // namespace internal + +void signedDistanceAtHeightTranspose(const Matrix& elevationMap, Matrix& sdfTranspose, Matrix& tmp, Matrix& tmpTranspose, float height, + float resolution, float minHeight, float maxHeight) { + const bool allPixelsAreObstacles = height < minHeight; + const bool allPixelsAreFreeSpace = height > maxHeight; + + if (allPixelsAreObstacles) { + internal::pixelDistanceToFreeSpaceTranspose(elevationMap, sdfTranspose, tmp, height, resolution); + + sdfTranspose *= -resolution; + } else if (allPixelsAreFreeSpace) { + internal::pixelDistanceToObstacleTranspose(elevationMap, sdfTranspose, tmp, height, resolution); + + sdfTranspose *= resolution; + } else { // This layer contains a mix of obstacles and free space + internal::pixelDistanceToObstacleTranspose(elevationMap, sdfTranspose, tmp, height, resolution); + internal::pixelDistanceToFreeSpaceTranspose(elevationMap, tmpTranspose, tmp, height, resolution); + + sdfTranspose = resolution * (sdfTranspose - tmpTranspose); + } +} + +Matrix signedDistanceAtHeight(const Matrix& elevationMap, float height, float resolution, float minHeight, float maxHeight) { + Matrix sdfTranspose; + Matrix tmp; + Matrix tmpTranspose; + + signedDistanceAtHeightTranspose(elevationMap, sdfTranspose, tmp, tmpTranspose, height, resolution, minHeight, maxHeight); + return sdfTranspose.transpose(); +} + +Matrix signedDistanceFromOccupancy(const Eigen::Matrix& occupancyGrid, float resolution) { + auto obstacleCount = occupancyGrid.count(); + bool hasObstacles = obstacleCount > 0; + if (hasObstacles) { + bool hasFreeSpace = obstacleCount < occupancyGrid.size(); + if (hasFreeSpace) { + return internal::signedDistanceFromOccupancyTranspose(occupancyGrid, resolution).transpose(); + } else { + // Only obstacles -> distance is minus infinity everywhere + return Matrix::Constant(occupancyGrid.rows(), occupancyGrid.cols(), -INF); + } + } else { + // No obstacles -> planar distance is infinite + return Matrix::Constant(occupancyGrid.rows(), occupancyGrid.cols(), INF); + } +} + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file