Add minBoundingRect function

costmap_2d/include/costmap_2d/footprint.h

@@ -44,6 +44,8 @@
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Point32.h>
 
+#include <Eigen/Dense>
+
 namespace costmap_2d
 {
 
@@ -57,15 +59,41 @@ namespace costmap_2d
 void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,
                                  double& min_dist, double& max_dist);
 
+typedef struct
+{
+  double rot_angle;
+  double area;
+  double width;
+  double height;
+  geometry_msgs::Point center;
+  std::array<geometry_msgs::Point, 4> corners;
+} BoundingRect;
+
+/**
+ * @brief Find the minimum-area bounding box of a footprint
+ * We first find the rotation angles of each edge of the convex polygon, then tests the area
+ * of a bounding box aligned with the unique angles in 90 degrees of the 1st Quadrant.
+ * C++ version of https://github.com/OmarFarag95/minimum-area-bounding-rectangle-python3
+ * @param footprint The footprint to examine
+ * @return strut containing the rotation angle, area, width, height, center and corners of the
+ * minimum-area bounding box
+ */
+BoundingRect minBoundingRect(const std::vector<geometry_msgs::Point>& points);
+
 /**
  * @brief Convert Point32 to Point
  */
 geometry_msgs::Point              toPoint(geometry_msgs::Point32 pt);
 
+/**
+ * @brief Convert Eigen Vector2d to Point
+ */
+geometry_msgs::Point              toPoint(Eigen::Vector2d pt);
+
 /**
  * @brief Convert Point to Point32
  */
-geometry_msgs::Point32            toPoint32(geometry_msgs::Point   pt);
+geometry_msgs::Point32            toPoint32(geometry_msgs::Point pt);
 
 /**
  * @brief Convert vector of Points to Polygon msg
@@ -77,6 +105,11 @@ geometry_msgs::Polygon            toPolygon(std::vector<geometry_msgs::Point> pt
  */
 std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
 
+/**
+ * @brief Return a list of numbers as a space-separated string.
+ */
+std::string                       toString(const std::vector<double>& numbers);
+
 /**
  * @brief  Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
  * @param  x The x position of the robot

costmap_2d/src/footprint.cpp

@@ -27,13 +27,15 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include<costmap_2d/costmap_math.h>
+#include <unordered_set>
+
+#include <costmap_2d/costmap_math.h>
 #include <boost/tokenizer.hpp>
 #include <boost/foreach.hpp>
 #include <boost/algorithm/string.hpp>
 #include <costmap_2d/footprint.h>
 #include <costmap_2d/array_parser.h>
-#include<geometry_msgs/Point32.h>
+#include <geometry_msgs/Point32.h>
 
 namespace costmap_2d
 {
@@ -66,6 +68,130 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
 }
 
+BoundingRect minBoundingRect(const std::vector<geometry_msgs::Point>& points)
+{
+  Eigen::MatrixX2d hull_points_2d(points.size(), 2);  // empty 2 column array
+  for (size_t i = 0; i < points.size(); ++i)
+    hull_points_2d.row(i) << points[i].x, points[i].y;
+  ROS_DEBUG_STREAM("Input convex hull points:\n" << hull_points_2d);
+
+  // Compute edges (x2-x1,y2-y1)
+  Eigen::MatrixX2d edges(hull_points_2d.rows() - 1, 2);  // empty 2 column array
+  edges.setZero();
+  for (size_t i = 0; i < edges.rows(); ++i)
+  {
+    double edge_x = hull_points_2d(i + 1, 0) - hull_points_2d(i, 0);
+    double edge_y = hull_points_2d(i + 1, 1) - hull_points_2d(i, 1);
+    edges.row(i) << edge_x, edge_y;
+  }
+  ROS_DEBUG_STREAM("Edges:\n" << edges);
+
+  // Calculate edge angles with atan2(y/x)
+  std::vector<double> edge_angles(edges.rows());  // empty 1 column array
+  for (size_t i = 0; i < edge_angles.size(); ++i)
+    edge_angles[i] = std::atan2(edges.row(i)[1], edges.row(i)[0]);
+  ROS_DEBUG_STREAM("Edge angles:\n" << toString(edge_angles));
+
+  // Check for angles in 1st quadrant
+  for (size_t i = 0; i < edge_angles.size(); ++i)
+    edge_angles[i] = std::fmod(edge_angles[i] + M_PI, M_PI_2);  // want strictly positive answers
+  ROS_DEBUG_STREAM("Edge angles in 1st Quadrant:\n" << toString(edge_angles));
+
+  // Remove duplicate angles
+  std::unordered_set<float> s;
+  auto end = std::remove_if(edge_angles.begin(), edge_angles.end(), [&s](double v) { return !s.insert(v).second; });
+  edge_angles.erase(end, edge_angles.end());
+  ROS_DEBUG_STREAM("Unique edge angles:\n" << toString(edge_angles));
+
+  // Test each angle to find bounding box with the smallest area
+  // rot_angle, area, width, height, min_x, max_x, min_y, max_y
+  std::array<double, 8> min_bbox{ 0.0, DBL_MAX, 0.0, 0.0, 0.0, 0.0, 0.0, 0 };
+  ROS_DEBUG_STREAM("Testing " << edge_angles.size() << " possible rotations for bounding box...");
+  for (size_t i = 0; i < edge_angles.size(); ++i)
+  {
+    // Create rotation matrix to shift points to baseline
+    // R = [ cos(theta)      , cos(theta-PI/2)
+    //       cos(theta+PI/2) , cos(theta)     ]
+    // clang-format off
+    Eigen::Matrix<double, 2, 2> R;
+    R << std::cos(edge_angles[i]), std::cos(edge_angles[i] - M_PI_2),
+         std::cos(edge_angles[i] + M_PI_2), std::cos(edge_angles[i]);
+    // clang-format on
+    ROS_DEBUG_STREAM("Rotation matrix for " << edge_angles[i] << " is\n" << R);
+
+    // Apply this rotation to convex hull points
+    Eigen::MatrixX2d rot_points = (R * hull_points_2d.transpose()).transpose();  // 2x2 * 2xn
+    ROS_DEBUG_STREAM("Rotated hull points are\n" << rot_points);
+
+    // Find min/max x,y points
+    const double min_x = rot_points.col(0).minCoeff();
+    const double max_x = rot_points.col(0).maxCoeff();
+    const double min_y = rot_points.col(1).minCoeff();
+    const double max_y = rot_points.col(1).maxCoeff();
+    ROS_DEBUG_STREAM("Min x: " << min_x << " Max x: " << max_x << "   Min y: " << min_y << " Max y: " << max_y);
+
+    // Calculate height/width/area of this bounding rectangle
+    const double width = max_x - min_x;
+    const double height = max_y - min_y;
+    const double area = width * height;
+    ROS_DEBUG_STREAM("Bounding box " << i << ":  width: " << width << " height: " << height << "  area: " << area);
+
+    // Store the smallest rect found first (a simple convex hull might have 2 answers with same area)
+    // Note that we require a non-neglectable difference to favor smaller rotations
+    if (min_bbox[1] - area > 1e-3)
+    {
+      ROS_DEBUG_STREAM("Area " << min_bbox[1] << " -> " << area);
+      min_bbox = { edge_angles[i], area, width, height, min_x, max_x, min_y, max_y };
+    }
+  }
+  // Re-create rotation matrix for smallest rect
+  // clang-format off
+  const double angle = min_bbox[0];
+  Eigen::Matrix<double, 2, 2> R;
+  R << std::cos(angle), std::cos(angle - M_PI_2),
+       std::cos(angle + M_PI_2), std::cos(angle);
+  // clang-format on
+  ROS_DEBUG_STREAM("Projection matrix:\n" << R);
+
+  // Project convex hull points onto rotated frame
+  Eigen::MatrixX2d proj_points = (R * hull_points_2d.transpose()).transpose();  // 2x2 * 2xn
+  ROS_DEBUG_STREAM("Project hull points are\n" << proj_points);
+
+  // min/max x,y points are against baseline
+  const double min_x = min_bbox[4];
+  const double max_x = min_bbox[5];
+  const double min_y = min_bbox[6];
+  const double max_y = min_bbox[7];
+  ROS_DEBUG_STREAM("Min x: " << min_x << " Max x: " << max_x << "   Min y: " << min_y << " Max y: " << max_y);
+
+  // Calculate center point and project onto rotated frame
+  Eigen::Vector2d center{ (min_x + max_x) / 2.0, (min_y + max_y) / 2.0 };
+  Eigen::Vector2d center_point = center.transpose() * R;
+  ROS_DEBUG_STREAM("Bounding box center point:\n" << center_point);
+
+  // Calculate corner points and project onto rotated frame
+  Eigen::Matrix<double, 4, 2> corner_points;  //// = zeros((4, 2))  // empty 2 column array
+  corner_points.row(0) = (Eigen::Vector2d{ max_x, min_y }.transpose() * R).transpose();
+  corner_points.row(1) = (Eigen::Vector2d{ min_x, min_y }.transpose() * R).transpose();
+  corner_points.row(2) = (Eigen::Vector2d{ min_x, max_y }.transpose() * R).transpose();
+  corner_points.row(3) = (Eigen::Vector2d{ max_x, max_y }.transpose() * R).transpose();
+  ROS_DEBUG_STREAM("Bounding box corner points:\n" << corner_points);
+
+  ROS_DEBUG_STREAM("Angle of rotation: " << angle << " rad  " << angle * (180 / M_PI) << " deg");
+
+  BoundingRect result;
+  result.rot_angle = angle;
+  result.area = min_bbox[1];
+  result.width = min_bbox[2];
+  result.height = min_bbox[3];
+  result.center.x = center_point.x();
+  result.center.y = center_point.y();
+  for (int i = 0; i < corner_points.rows(); ++i)
+    result.corners[i] = toPoint(corner_points.row(i));
+
+  return result;
+}
+
 geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
 {
   geometry_msgs::Point32 point32;
@@ -84,6 +210,14 @@ geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
   return point;
 }
 
+geometry_msgs::Point toPoint(Eigen::Vector2d pt)
+{
+  geometry_msgs::Point point;
+  point.x = pt.x();
+  point.y = pt.y();
+  return point;
+}
+
 geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
 {
   geometry_msgs::Polygon polygon;
@@ -103,6 +237,13 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
   return pts;
 }
 
+std::string toString(const std::vector<double>& numbers)
+{
+  std::stringstream ss;
+  std::for_each(numbers.begin(), numbers.end(), [&](double nb) { ss << nb << " "; });
+  return ss.str();
+}
+
 void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
                         std::vector<geometry_msgs::Point>& oriented_footprint)
 {