diff --git a/costmap_2d/include/costmap_2d/footprint.h b/costmap_2d/include/costmap_2d/footprint.h index 6b1d1bec1b..812273ceb3 100644 --- a/costmap_2d/include/costmap_2d/footprint.h +++ b/costmap_2d/include/costmap_2d/footprint.h @@ -44,6 +44,8 @@ #include #include +#include + namespace costmap_2d { @@ -57,15 +59,41 @@ namespace costmap_2d void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist); +typedef struct +{ + double rot_angle; + double area; + double width; + double height; + geometry_msgs::Point center; + std::array 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& 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 pt */ std::vector toPointVector(geometry_msgs::Polygon polygon); +/** + * @brief Return a list of numbers as a space-separated string. + */ +std::string toString(const std::vector& 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/footprint.cpp b/costmap_2d/src/footprint.cpp index dc51700a2f..fce827b62f 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -27,13 +27,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include + +#include #include #include #include #include #include -#include +#include namespace costmap_2d { @@ -66,6 +68,130 @@ void calculateMinAndMaxDistances(const std::vector& footpr max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); } +BoundingRect minBoundingRect(const std::vector& 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 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 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 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 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 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 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 pts) { geometry_msgs::Polygon polygon; @@ -103,6 +237,13 @@ std::vector toPointVector(geometry_msgs::Polygon polygon) return pts; } +std::string toString(const std::vector& 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& footprint_spec, std::vector& oriented_footprint) {