From 9676f703d754d801f14a3351c82d1291346fabfe Mon Sep 17 00:00:00 2001 From: corot Date: Fri, 29 Jul 2022 11:40:07 +0200 Subject: [PATCH 1/5] Add function to calculate the orientation at which the footprint sweeps the smallest area --- costmap_2d/include/costmap_2d/costmap_math.h | 2 ++ costmap_2d/include/costmap_2d/footprint.h | 8 +++++ costmap_2d/src/costmap_math.cpp | 7 +++++ costmap_2d/src/footprint.cpp | 33 ++++++++++++++++++++ 4 files changed, 50 insertions(+) diff --git a/costmap_2d/include/costmap_2d/costmap_math.h b/costmap_2d/include/costmap_2d/costmap_math.h index 71fe96c45f..81cdfc3a08 100644 --- a/costmap_2d/include/costmap_2d/costmap_math.h +++ b/costmap_2d/include/costmap_2d/costmap_math.h @@ -66,4 +66,6 @@ bool intersects(std::vector& polygon, float testx, float t bool intersects(std::vector& polygon1, std::vector& polygon2); +double orientation(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 index 6b1d1bec1b..01911b97a3 100644 --- a/costmap_2d/include/costmap_2d/footprint.h +++ b/costmap_2d/include/costmap_2d/footprint.h @@ -57,6 +57,14 @@ namespace costmap_2d void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist); +/** + * @brief Calculate the orientation at which the footprint will sweep the smallest area when moving along +x direction + + * @param footprint The footprint to examine + * @return Minimum footprint sweeping area orientation + */ +double minSweepingAreaOrientation(const std::vector& footprint); + /** * @brief Convert Point32 to Point */ diff --git a/costmap_2d/src/costmap_math.cpp b/costmap_2d/src/costmap_math.cpp index 97f7f50699..f7e978d9bd 100644 --- a/costmap_2d/src/costmap_math.cpp +++ b/costmap_2d/src/costmap_math.cpp @@ -87,3 +87,10 @@ bool intersects(std::vector& polygon1, std::vector& footpr max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); } +double minSweepingAreaOrientation(const std::vector& footprint) +{ + double min_dist = std::numeric_limits::max(); + std::vector closest_edge; + + if (footprint.size() <= 2) + { + 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) + { + min_dist = edge_dist; + closest_edge = { footprint[i], footprint[i + 1] }; + } + } + + // 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); +} + geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt) { geometry_msgs::Point32 point32; From 87b85881c82032c3b1d798b08c6211d327a0aca6 Mon Sep 17 00:00:00 2001 From: corot Date: Mon, 1 Aug 2022 10:16:56 +0200 Subject: [PATCH 2/5] Address PR reviews comments --- costmap_2d/include/costmap_2d/footprint.h | 4 +++- costmap_2d/src/footprint.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/costmap_2d/include/costmap_2d/footprint.h b/costmap_2d/include/costmap_2d/footprint.h index 01911b97a3..407318974e 100644 --- a/costmap_2d/include/costmap_2d/footprint.h +++ b/costmap_2d/include/costmap_2d/footprint.h @@ -59,7 +59,9 @@ void calculateMinAndMaxDistances(const std::vector& footpr /** * @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 * @param footprint The footprint to examine * @return Minimum footprint sweeping area orientation */ diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index cf036c75a8..d9d3558fcf 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -69,7 +69,7 @@ void calculateMinAndMaxDistances(const std::vector& footpr double minSweepingAreaOrientation(const std::vector& footprint) { double min_dist = std::numeric_limits::max(); - std::vector closest_edge; + std::array closest_edge; if (footprint.size() <= 2) { From f4a1877f2caacbc1b581a39766faa8f66c69acf3 Mon Sep 17 00:00:00 2001 From: corot Date: Fri, 9 Sep 2022 19:16:00 +0900 Subject: [PATCH 3/5] Calculate using rotating calipers instead --- costmap_2d/include/costmap_2d/costmap_math.h | 2 + costmap_2d/src/costmap_math.cpp | 9 ++- costmap_2d/src/footprint.cpp | 73 +++++++++++++++++++- 3 files changed, 82 insertions(+), 2 deletions(-) diff --git a/costmap_2d/include/costmap_2d/costmap_math.h b/costmap_2d/include/costmap_2d/costmap_math.h index 81cdfc3a08..b6d7eca33e 100644 --- a/costmap_2d/include/costmap_2d/costmap_math.h +++ b/costmap_2d/include/costmap_2d/costmap_math.h @@ -68,4 +68,6 @@ bool intersects(std::vector& polygon1, std::vector& 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> getAllAntipodalPairs(const std::vector& footprint) +{ + std::vector> 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()) + { + 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; + } + } + + return antipodal_pairs; +} + double minSweepingAreaOrientation(const std::vector& footprint) { double min_dist = std::numeric_limits::max(); @@ -96,7 +144,30 @@ double minSweepingAreaOrientation(const std::vector& footp // 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); +// 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> 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 << " " < Date: Mon, 12 Sep 2022 11:38:30 +0900 Subject: [PATCH 4/5] Add rotating calipers code file --- costmap_2d/CMakeLists.txt | 1 + costmap_2d/src/rotating_calipers.cpp | 278 +++++++++++++++++++++++++++ 2 files changed, 279 insertions(+) create mode 100644 costmap_2d/src/rotating_calipers.cpp diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt index 39ec3360ff..33529de3d4 100644 --- a/costmap_2d/CMakeLists.txt +++ b/costmap_2d/CMakeLists.txt @@ -93,6 +93,7 @@ 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/src/rotating_calipers.cpp b/costmap_2d/src/rotating_calipers.cpp new file mode 100644 index 0000000000..57d216eac8 --- /dev/null +++ b/costmap_2d/src/rotating_calipers.cpp @@ -0,0 +1,278 @@ +/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of OpenCV Foundation may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the OpenCV Foundation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +// + +#include + +#include + +#include +using Point = geometry_msgs::Point; + +#include + +namespace costmap_2d +{ + +struct MinAreaState +{ + int bottom; + int left; + float height; + float width; + float base_a; + float base_b; +}; + +enum +{ + CALIPERS_MAXHEIGHT = 0, + CALIPERS_MINAREARECT = 1, + CALIPERS_MAXDIST = 2 +}; + +/*F/////////////////////////////////////////////////////////////////////////////////////// + // Name: rotatingCalipers + // Purpose: + // Rotating calipers algorithm with some applications + // + // Context: + // Parameters: + // points - convex hull vertices ( any orientation ) + // n - number of vertices + // mode - concrete application of algorithm + // can be CV_CALIPERS_MAXDIST or + // CV_CALIPERS_MINAREARECT + // left, bottom, right, top - indexes of extremal points + // out - output info. + // In case CV_CALIPERS_MAXDIST it points to float value - + // maximal height of polygon. + // In case CV_CALIPERS_MINAREARECT + // ((CvPoint2D32f*)out)[0] - corner + // ((CvPoint2D32f*)out)[1] - vector1 + // ((CvPoint2D32f*)out)[0] - corner2 + // + // ^ + // | + // vector2 | + // | + // |____________\ + // corner / + // vector1 + // + // Returns: + // Notes: + //F*/ + +/* we will use usual cartesian coordinates */ +void rotatingCalipers(const std::vector& points) +{ + float min_area = FLT_MAX; + float max_dist = 0; + char buffer[32] = {}; + int i, k; + /* modern equivalents + std::vector abuf(points.size() * 3); + std::vector& inv_vect_length = abuf; + std::vector vect(inv_vect_length + n); + */ + float abuf[points.size() * 3]; + float* inv_vect_length = abuf; + Point* vect = (Point*)(inv_vect_length + points.size()); + int left = 0, bottom = 0, right = 0, top = 0; + int seq[4] = { -1, -1, -1, -1 }; + + /* rotating calipers sides will always have coordinates + (a,b) (-b,a) (-a,-b) (b, -a) + */ + /* this is a first base vector (a,b) initialized by (1,0) */ + float orientation = 0; + float base_a; + float base_b = 0; + + float left_x, right_x, top_y, bottom_y; + Point pt0 = points[0]; + + left_x = right_x = pt0.x; + top_y = bottom_y = pt0.y; + + for (i = 0; i < points.size(); i++) + { + double dx, dy; + + if (pt0.x < left_x) + left_x = pt0.x, left = i; + + if (pt0.x > right_x) + right_x = pt0.x, right = i; + + if (pt0.y > top_y) + top_y = pt0.y, top = i; + + if (pt0.y < bottom_y) + bottom_y = pt0.y, bottom = i; + + Point pt = points[(i + 1) & (i + 1 < points.size() ? -1 : 0)]; + + dx = pt.x - pt0.x; + dy = pt.y - pt0.y; + + vect[i].x = (float)dx; + vect[i].y = (float)dy; + inv_vect_length[i] = (float)(1. / std::sqrt(dx * dx + dy * dy)); + + pt0 = pt; + } + + // find convex hull orientation + { + double ax = vect[points.size() - 1].x; + double ay = vect[points.size() - 1].y; + + for (i = 0; i < points.size(); i++) + { + double bx = vect[i].x; + double by = vect[i].y; + + double convexity = ax * by - ay * bx; + + if (convexity != 0) + { + orientation = (convexity > 0) ? 1.f : (-1.f); + break; + } + ax = bx; + ay = by; + } + ROS_ASSERT(orientation != 0); + } + base_a = orientation; + + /*****************************************************************************************/ + /* init calipers position */ + seq[0] = bottom; + seq[1] = right; + seq[2] = top; + seq[3] = left; + /*****************************************************************************************/ + /* Main loop - evaluate angles and rotate calipers */ + + /* all of edges will be checked while rotating calipers by 90 degrees */ + for (k = 0; k < points.size(); k++) + { + /* sinus of minimal angle */ + /*float sinus;*/ + + /* compute cosine of angle between calipers side and polygon edge */ + /* dp - dot product */ + float dp0 = base_a * vect[seq[0]].x + base_b * vect[seq[0]].y; + float dp1 = -base_b * vect[seq[1]].x + base_a * vect[seq[1]].y; + float dp2 = -base_a * vect[seq[2]].x - base_b * vect[seq[2]].y; + float dp3 = base_b * vect[seq[3]].x - base_a * vect[seq[3]].y; + + float cosalpha = dp0 * inv_vect_length[seq[0]]; + float maxcos = cosalpha; + + /* number of calipers edges, that has minimal angle with edge */ + int main_element = 0; + + /* choose minimal angle */ + cosalpha = dp1 * inv_vect_length[seq[1]]; + maxcos = (cosalpha > maxcos) ? (main_element = 1, cosalpha) : maxcos; + cosalpha = dp2 * inv_vect_length[seq[2]]; + maxcos = (cosalpha > maxcos) ? (main_element = 2, cosalpha) : maxcos; + cosalpha = dp3 * inv_vect_length[seq[3]]; + maxcos = (cosalpha > maxcos) ? (main_element = 3, cosalpha) : maxcos; + + /*rotate calipers*/ + { + // get next base + int pindex = seq[main_element]; + float lead_x = vect[pindex].x * inv_vect_length[pindex]; + float lead_y = vect[pindex].y * inv_vect_length[pindex]; + switch (main_element) + { + case 0: + base_a = lead_x; + base_b = lead_y; + break; + case 1: + base_a = lead_y; + base_b = -lead_x; + break; + case 2: + base_a = -lead_x; + base_b = -lead_y; + break; + case 3: + base_a = -lead_y; + base_b = lead_x; + break; + default: + throw ros::Exception("main_element should be 0, 1, 2 or 3"); + } + } + /* change base point of main edge */ + seq[main_element] += 1; + seq[main_element] = (seq[main_element] == points.size()) ? 0 : seq[main_element]; + + /* now main element lies on edge aligned to calipers side */ + + /* find opposite element i.e. transform */ + /* 0->2, 1->3, 2->0, 3->1 */ + int opposite_el = main_element ^ 2; + + float dx = points[seq[opposite_el]].x - points[seq[main_element]].x; + float dy = points[seq[opposite_el]].y - points[seq[main_element]].y; + float dist; + + if (main_element & 1) + dist = (float)fabs(dx * base_a + dy * base_b); + else + dist = (float)fabs(dx * (-base_b) + dy * base_a); + + if (dist > max_dist) + max_dist = dist; + } + + // out[0] = max_dist; +} + +} // namespace costmap_2d From cecace277fb759d215f60257cec1807be38c0a81 Mon Sep 17 00:00:00 2001 From: corot Date: Wed, 14 Sep 2022 11:07:37 +0900 Subject: [PATCH 5/5] Add minBoundingRect function --- costmap_2d/CMakeLists.txt | 1 - costmap_2d/include/costmap_2d/costmap_math.h | 4 - costmap_2d/include/costmap_2d/footprint.h | 37 ++- costmap_2d/src/costmap_math.cpp | 14 - costmap_2d/src/footprint.cpp | 227 ++++++++------- costmap_2d/src/rotating_calipers.cpp | 278 ------------------- 6 files changed, 162 insertions(+), 399 deletions(-) delete mode 100644 costmap_2d/src/rotating_calipers.cpp diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt index 33529de3d4..39ec3360ff 100644 --- 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 index b6d7eca33e..71fe96c45f 100644 --- 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& polygon, float testx, float t bool intersects(std::vector& polygon1, std::vector& 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 index 407318974e..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,25 +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 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. + * C++ version of https://github.com/OmarFarag95/minimum-area-bounding-rectangle-python3 * @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& footprint); +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 @@ -87,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/costmap_math.cpp b/costmap_2d/src/costmap_math.cpp index 9af26a7e49..97f7f50699 100644 --- a/costmap_2d/src/costmap_math.cpp +++ b/costmap_2d/src/costmap_math.cpp @@ -87,17 +87,3 @@ bool intersects(std::vector& polygon1, std::vector +#include + +#include #include #include #include #include #include -#include +#include namespace costmap_2d { @@ -66,107 +68,127 @@ void calculateMinAndMaxDistances(const std::vector& 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> getAllAntipodalPairs(const std::vector& footprint) +BoundingRect minBoundingRect(const std::vector& points) { - std::vector> 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_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) { - 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& footprint) -{ - double min_dist = std::numeric_limits::max(); - std::array closest_edge; - - if (footprint.size() <= 2) + 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) { - 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 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) { - min_dist = edge_dist; - closest_edge = { footprint[i], footprint[i + 1] }; + 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)); - // 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> 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 << " " < pts) { geometry_msgs::Polygon polygon; @@ -207,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) { diff --git a/costmap_2d/src/rotating_calipers.cpp b/costmap_2d/src/rotating_calipers.cpp deleted file mode 100644 index 57d216eac8..0000000000 --- a/costmap_2d/src/rotating_calipers.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2000, Intel Corporation, all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of OpenCV Foundation may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the OpenCV Foundation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -// - -#include - -#include - -#include -using Point = geometry_msgs::Point; - -#include - -namespace costmap_2d -{ - -struct MinAreaState -{ - int bottom; - int left; - float height; - float width; - float base_a; - float base_b; -}; - -enum -{ - CALIPERS_MAXHEIGHT = 0, - CALIPERS_MINAREARECT = 1, - CALIPERS_MAXDIST = 2 -}; - -/*F/////////////////////////////////////////////////////////////////////////////////////// - // Name: rotatingCalipers - // Purpose: - // Rotating calipers algorithm with some applications - // - // Context: - // Parameters: - // points - convex hull vertices ( any orientation ) - // n - number of vertices - // mode - concrete application of algorithm - // can be CV_CALIPERS_MAXDIST or - // CV_CALIPERS_MINAREARECT - // left, bottom, right, top - indexes of extremal points - // out - output info. - // In case CV_CALIPERS_MAXDIST it points to float value - - // maximal height of polygon. - // In case CV_CALIPERS_MINAREARECT - // ((CvPoint2D32f*)out)[0] - corner - // ((CvPoint2D32f*)out)[1] - vector1 - // ((CvPoint2D32f*)out)[0] - corner2 - // - // ^ - // | - // vector2 | - // | - // |____________\ - // corner / - // vector1 - // - // Returns: - // Notes: - //F*/ - -/* we will use usual cartesian coordinates */ -void rotatingCalipers(const std::vector& points) -{ - float min_area = FLT_MAX; - float max_dist = 0; - char buffer[32] = {}; - int i, k; - /* modern equivalents - std::vector abuf(points.size() * 3); - std::vector& inv_vect_length = abuf; - std::vector vect(inv_vect_length + n); - */ - float abuf[points.size() * 3]; - float* inv_vect_length = abuf; - Point* vect = (Point*)(inv_vect_length + points.size()); - int left = 0, bottom = 0, right = 0, top = 0; - int seq[4] = { -1, -1, -1, -1 }; - - /* rotating calipers sides will always have coordinates - (a,b) (-b,a) (-a,-b) (b, -a) - */ - /* this is a first base vector (a,b) initialized by (1,0) */ - float orientation = 0; - float base_a; - float base_b = 0; - - float left_x, right_x, top_y, bottom_y; - Point pt0 = points[0]; - - left_x = right_x = pt0.x; - top_y = bottom_y = pt0.y; - - for (i = 0; i < points.size(); i++) - { - double dx, dy; - - if (pt0.x < left_x) - left_x = pt0.x, left = i; - - if (pt0.x > right_x) - right_x = pt0.x, right = i; - - if (pt0.y > top_y) - top_y = pt0.y, top = i; - - if (pt0.y < bottom_y) - bottom_y = pt0.y, bottom = i; - - Point pt = points[(i + 1) & (i + 1 < points.size() ? -1 : 0)]; - - dx = pt.x - pt0.x; - dy = pt.y - pt0.y; - - vect[i].x = (float)dx; - vect[i].y = (float)dy; - inv_vect_length[i] = (float)(1. / std::sqrt(dx * dx + dy * dy)); - - pt0 = pt; - } - - // find convex hull orientation - { - double ax = vect[points.size() - 1].x; - double ay = vect[points.size() - 1].y; - - for (i = 0; i < points.size(); i++) - { - double bx = vect[i].x; - double by = vect[i].y; - - double convexity = ax * by - ay * bx; - - if (convexity != 0) - { - orientation = (convexity > 0) ? 1.f : (-1.f); - break; - } - ax = bx; - ay = by; - } - ROS_ASSERT(orientation != 0); - } - base_a = orientation; - - /*****************************************************************************************/ - /* init calipers position */ - seq[0] = bottom; - seq[1] = right; - seq[2] = top; - seq[3] = left; - /*****************************************************************************************/ - /* Main loop - evaluate angles and rotate calipers */ - - /* all of edges will be checked while rotating calipers by 90 degrees */ - for (k = 0; k < points.size(); k++) - { - /* sinus of minimal angle */ - /*float sinus;*/ - - /* compute cosine of angle between calipers side and polygon edge */ - /* dp - dot product */ - float dp0 = base_a * vect[seq[0]].x + base_b * vect[seq[0]].y; - float dp1 = -base_b * vect[seq[1]].x + base_a * vect[seq[1]].y; - float dp2 = -base_a * vect[seq[2]].x - base_b * vect[seq[2]].y; - float dp3 = base_b * vect[seq[3]].x - base_a * vect[seq[3]].y; - - float cosalpha = dp0 * inv_vect_length[seq[0]]; - float maxcos = cosalpha; - - /* number of calipers edges, that has minimal angle with edge */ - int main_element = 0; - - /* choose minimal angle */ - cosalpha = dp1 * inv_vect_length[seq[1]]; - maxcos = (cosalpha > maxcos) ? (main_element = 1, cosalpha) : maxcos; - cosalpha = dp2 * inv_vect_length[seq[2]]; - maxcos = (cosalpha > maxcos) ? (main_element = 2, cosalpha) : maxcos; - cosalpha = dp3 * inv_vect_length[seq[3]]; - maxcos = (cosalpha > maxcos) ? (main_element = 3, cosalpha) : maxcos; - - /*rotate calipers*/ - { - // get next base - int pindex = seq[main_element]; - float lead_x = vect[pindex].x * inv_vect_length[pindex]; - float lead_y = vect[pindex].y * inv_vect_length[pindex]; - switch (main_element) - { - case 0: - base_a = lead_x; - base_b = lead_y; - break; - case 1: - base_a = lead_y; - base_b = -lead_x; - break; - case 2: - base_a = -lead_x; - base_b = -lead_y; - break; - case 3: - base_a = -lead_y; - base_b = lead_x; - break; - default: - throw ros::Exception("main_element should be 0, 1, 2 or 3"); - } - } - /* change base point of main edge */ - seq[main_element] += 1; - seq[main_element] = (seq[main_element] == points.size()) ? 0 : seq[main_element]; - - /* now main element lies on edge aligned to calipers side */ - - /* find opposite element i.e. transform */ - /* 0->2, 1->3, 2->0, 3->1 */ - int opposite_el = main_element ^ 2; - - float dx = points[seq[opposite_el]].x - points[seq[main_element]].x; - float dy = points[seq[opposite_el]].y - points[seq[main_element]].y; - float dist; - - if (main_element & 1) - dist = (float)fabs(dx * base_a + dy * base_b); - else - dist = (float)fabs(dx * (-base_b) + dy * base_a); - - if (dist > max_dist) - max_dist = dist; - } - - // out[0] = max_dist; -} - -} // namespace costmap_2d