Backport SignedDistance2d 2D SDF helpers from master to kilted

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
 )
 

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 <algorithm>
+#include <cassert>
+#include <cmath>
+
+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

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 <vector>
+
+#include <grid_map_core/TypeDefs.hpp>
+
+#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<bool, -1, -1>& occupancyGrid, float resolution);
+
+}  // namespace signed_distance_field
+}  // namespace grid_map

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<float>::has_infinity, "float does not support infinity");
+
+//! Distance value that is considered infinite.
+constexpr float INF = std::numeric_limits<float>::infinity();
+
+}  // namespace signed_distance_field
+}  // namespace grid_map