Add minBoundingRect function

corot · corot · commit fe4cabd12063 · 2022-09-14T11:34:44.000+09:00
diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt
@@ -93,7 +93,6 @@ add_library(costmap_2d
   src/costmap_math.cpp
   src/footprint.cpp
   src/costmap_layer.cpp
-  src/rotating_calipers.cpp
 )
 add_dependencies(costmap_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(costmap_2d
diff --git a/costmap_2d/include/costmap_2d/costmap_math.h b/costmap_2d/include/costmap_2d/costmap_math.h
@@ -66,8 +66,4 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t
 
 bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
 
-double orientation(double x0, double y0, double x1, double y1);
-
-double positiveAngle(double x0, double y0, double x1, double y1);
-
 #endif  // COSTMAP_2D_COSTMAP_MATH_H_
diff --git a/costmap_2d/include/costmap_2d/footprint.h b/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,25 +59,40 @@ 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 Calculate the orientation at which the footprint will sweep the smallest area when moving along +x direction
- * @warning This function only works under two assumptions:
- *          * the footprint is symmetric wrt the x axis
- *          * the closest edge is approximately parallel to either x or y axis
+ * @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.
  * @param footprint The footprint to examine
- * @return Minimum footprint sweeping area orientation
+ * @return strut containing the rotation angle, area, width, height, center and corners of the
+ * minimum-area bounding box
  */
-double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint);
+ 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
@@ -87,6 +104,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
diff --git a/costmap_2d/src/costmap_math.cpp b/costmap_2d/src/costmap_math.cpp
@@ -87,17 +87,3 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometr
 {
   return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
 }
-
-double orientation(double x0, double y0, double x1, double y1)
-{
-  const double dx = x1 - x0;
-  const double dy = y1 - y0;
-  return atan2(dy, dx);
-}
-
-double positiveAngle(double x0, double y0, double x1, double y1)
-{
-  const double angle1 = atan2(y0, x0);
-  const double angle2 = atan2(y1, x1);
-  return std::remainder(angle2 - angle1, M_PI);
-}
diff --git a/costmap_2d/src/footprint.cpp b/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,107 +68,130 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
 }
 
-/* p[] is in standard form, ie, counterclockwise order,
-     distinct vertices, no collinear vertices.
-   ANGLE(m, n) is a procedure that returns the clockwise angle
-     swept out by a ray as it rotates from a position parallel
-     to the directed segment Pm,Pm+1 to a position parallel to Pn, Pn+1
-   We assume all indices are reduced to mod N (so that N+1 = 1).
-*/
-std::vector<std::pair<int, int>> getAllAntipodalPairs(const std::vector<geometry_msgs::Point>& footprint)
+BoundingRect minBoundingRect(const std::vector<geometry_msgs::Point>& points)
 {
-  std::vector<std::pair<int, int>> antipodal_pairs;
-
-  // Find first antipodal pair by locating vertex opposite P1
-  int i = 0;
-  int j = 1;
-  while (positiveAngle(footprint[i].x, footprint[i].y, footprint[j].x, footprint[j].y) < M_PI)
-    ++j;
-  antipodal_pairs.push_back({ i, j });
-
-  // Loop on j until all of P has been scanned
-  while (j < footprint.size())
+  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_INFO_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)
   {
-    bool last_pt = j == (footprint.size() - 1);
-    double a = 2 * M_PI - positiveAngle(footprint[i].x, footprint[i].y, footprint[j].x, footprint[j].y);
-    if (a == M_PI)  // Pi Pi+1 and Pj Pj+1 are parallel
-    {
-      antipodal_pairs.push_back({ i + 1, j });
-      antipodal_pairs.push_back({ i, last_pt ? 0 : j + 1 });
-      antipodal_pairs.push_back({ i + 1, last_pt ? 0 : j + 1 });
-
-      // Notice that (i, j) has been added to the result before being the pivots, so no need to yield i,j
-      ++i;
-      ++j;
-    }
-    else if (a < M_PI)  // Will touch Pi Pi+1 first
-    {
-      antipodal_pairs.push_back({ i + 1, j });
-      ++i;
-    }
-    else
-    {
-      antipodal_pairs.push_back({ i, last_pt ? 0 : j + 1 });  // Will touch Pj Pj+1 first
-      ++j;
-    }
+    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;
   }
-
-  return antipodal_pairs;
-}
-
-double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint)
-{
-  double min_dist = std::numeric_limits<double>::max();
-  std::array<geometry_msgs::Point, 2> closest_edge;
-
-  if (footprint.size() <= 2)
+  ROS_INFO_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_INFO_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_INFO_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());
+  //  std::sort(edge_angles.begin(), edge_angles.end());
+  //  auto ip = std::unique(edge_angles.begin(), edge_angles.end());
+  //edge_angles.resize(std::distance(edge_angles.begin(), ip));
+  ROS_INFO_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_INFO_STREAM("Testing " << edge_angles.size() << " possible rotations for bounding box...");
+  for (size_t i = 0; i < edge_angles.size(); ++i)
   {
-    return NAN;
-  }
-
-  // check the distance from the robot center point to each footprint edged and keep the closest one
-  for (unsigned int i = 0; i < footprint.size() - 1; ++i)
-  {
-    double edge_dist = distanceToLine(0, 0, footprint[i].x, footprint[i].y, footprint[i + 1].x, footprint[i + 1].y);
-    if (edge_dist < min_dist)
+    // 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_INFO_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_INFO_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_INFO_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_INFO_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)
     {
-      min_dist = edge_dist;
-      closest_edge = { footprint[i], footprint[i + 1] };
+      ROS_INFO_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_INFO_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_INFO_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_INFO_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_INFO_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_INFO_STREAM("Bounding box corner points:\n" << corner_points);
+
+  ROS_INFO_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));
 
-  // we also need to do the last vertex and the first vertex
-  if (distanceToLine(0, 0, footprint.back().x, footprint.back().y, footprint.front().x, footprint.front().y) < min_dist)
-  {
-    closest_edge = { footprint.back(), footprint.front() };
-  }
-
-  // return the orientation of the closest edge, directed from back to front (+x axis direction)
-  std::sort(closest_edge.begin(), closest_edge.end(),
-            [](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { return p1.x < p2.x; });
-//  return orientation(closest_edge.front().x, closest_edge.front().y, closest_edge.back().x, closest_edge.back().y);
-  double result1 = orientation(closest_edge.front().x, closest_edge.front().y, closest_edge.back().x, closest_edge.back().y);
-
-  std::vector<std::pair<int, int>> antipodal_pairs = getAllAntipodalPairs(footprint);
-  double footprint_width = INFINITY;
-  size_t closest_ap_pair = INFINITY;
-  for (int i = 0; i < antipodal_pairs.size(); ++i)
-  {
-    const auto& ap_pair = antipodal_pairs[i];
-    const geometry_msgs::Point& p1 = footprint[ap_pair.first];
-    const geometry_msgs::Point& p2 = footprint[ap_pair.second];
-    const double dist = distance(p1.x, p1.y, p2.x, p2.y);
-    if (dist < footprint_width)
-    {
-      footprint_width = dist;
-      closest_ap_pair = i;
-    }
-  }
-  const geometry_msgs::Point& p1 = footprint[antipodal_pairs[closest_ap_pair].first];
-  const geometry_msgs::Point& p2 = footprint[antipodal_pairs[closest_ap_pair].second];
-  double result = orientation(p1.x, p1.y, p2.x, p2.y);
-
-  ROS_WARN_STREAM(result1 <<   "    " <<result);
   return result;
 }
 
@@ -188,6 +213,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;
@@ -207,6 +240,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)
 {

Original file line number	Diff line number	Diff line change
`@@ -93,7 +93,6 @@ add_library(costmap_2d`
`93`	`93`	`src/costmap_math.cpp`
`94`	`94`	`src/footprint.cpp`
`95`	`95`	`src/costmap_layer.cpp`
`96`		`- src/rotating_calipers.cpp`
`97`	`96`	`)`
`98`	`97`	`add_dependencies(costmap_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})`
`99`	`98`	`target_link_libraries(costmap_2d`