From cdc35c884da934af04175d501bbe0c0d187c272e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 10:33:15 +0200 Subject: [PATCH 01/11] Add clion gitignores. --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 259148f..7065ac8 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,6 @@ *.exe *.out *.app + +# clion +.idea/ \ No newline at end of file From cdf15ff8baf8e906733994bffae0a23827cf7168 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 20:25:39 +0200 Subject: [PATCH 02/11] Method to condition adjacency matrix. --- .../polygon_coverage_solvers/graph_base.h | 29 ++++++--- .../impl/graph_base_impl.h | 63 +++++++++++++++++-- 2 files changed, 79 insertions(+), 13 deletions(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h index 9c51f72..e47d2f7 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h @@ -108,14 +108,6 @@ class GraphBase { // transforming cost into milli int. std::vector> getAdjacencyMatrix() const; - // Preserving three decimal digits. - inline int doubleToMilliInt(double in) const { - return static_cast(std::round(in * kToMilli)); - } - inline double milliIntToDouble(int in) const { - return static_cast(in) * kFromMilli; - } - protected: // Called from addNode. Creates all edges to the node at the back of the // graph. @@ -138,6 +130,27 @@ class GraphBase { size_t goal_idx_; bool is_created_; }; + +// Recursive function to search for largest x between lower and upper bound. +inline double searchLargestFeasibleScale(const double low, const double high, + const double condition, + const double eps = 0.1) { + // Early abort. + if (high < low) return low; + // Terminate recursion. + if ((high - low) < eps) return low; + + const double mid = (high + low) / 2.0; + + if (mid < condition) { + // mid satisfies condition. Find even higher scale. + return searchLargestFeasibleScale(mid, high, condition, eps); + } else { + // mid does not satisfy condition. Find smaller scale that does. + return searchLargestFeasibleScale(low, mid, condition, eps); + } +} + } // namespace polygon_coverage_planning #include "polygon_coverage_solvers/impl/graph_base_impl.h" diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index 25478bc..9a88947 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -20,12 +20,12 @@ #ifndef POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_IMPL_H_ #define POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_IMPL_H_ -#include -#include - #include #include +#include +#include + namespace polygon_coverage_planning { template @@ -324,15 +324,68 @@ Solution GraphBase::reconstructSolution( template std::vector> GraphBase::getAdjacencyMatrix() const { + // First scale the matrix such that when converting the cost to integers two + // costs can always be differentiated (except for if they are the same). Find + // the minimum absolute difference between any two values to normalize the + // adjacency matrix. + // https://afteracademy.com/blog/minimum-absolute-difference-in-an-array + std::vector sorted_cost; + sorted_cost.reserve(graph_.size() * graph_.size()); + for (size_t i = 0; i < graph_.size(); ++i) { + for (size_t j = 0; j < graph_[i].size(); ++j) { + const EdgeId edge(i, j); + double cost; + if (edgeExists(edge) && getEdgeCost(edge, &cost)) { + sorted_cost.push_back(cost); + } + } + } + sort(sorted_cost.begin(), sorted_cost.end()); + auto min_diff = std::numeric_limits::max(); + for (size_t i = 0; i < sorted_cost.size() - 2; i++) { + auto diff = std::fabs(sorted_cost[i + 1] - sorted_cost[i]); + if (diff > std::numeric_limits::epsilon() && diff < min_diff) + min_diff = diff; + } + + // Only scale with min_diff if this does not blow up the cost. + // TODO(rikba): Find a tighter upper bound. + double total_cost_upper_bound = sorted_cost.back() * graph_.size(); + double max_scale = 1.1 / min_diff; + double min_scale = + std::numeric_limits::max() / (total_cost_upper_bound + 1.0); + double scale = 1.0; + if (max_scale * total_cost_upper_bound < std::numeric_limits::max()) { + ROS_DEBUG("Optimal scale applied."); + scale = max_scale; + } else if (min_scale * total_cost_upper_bound + 1 > + std::numeric_limits::max()) { + // Recursive search for largest scale between min_scale and max_scale. + ROS_WARN( + "The adjacency matrix is ill conditioned. Adjacency matrix " + "coefficients are reduced to guarantee upper cost bound. Consider " + "removing small details in the polygon to have even decomposition " + "size."); + ROS_WARN("Recursively searching for best scale."); + scale = searchLargestFeasibleScale( + min_scale, max_scale, + std::numeric_limits::max() / total_cost_upper_bound + 1); + + ROS_WARN_COND(min_diff < 1.0, "Loss of optimality."); + } + + ROS_DEBUG("The minimum cost difference is: %.9f", min_diff); + ROS_INFO("The adjacency matrix scale is: %.9f", scale); + + // Create scale adjacency matrix. std::vector> m(graph_.size(), std::vector(graph_.size())); - for (size_t i = 0; i < m.size(); ++i) { for (size_t j = 0; j < m[i].size(); ++j) { const EdgeId edge(i, j); double cost; if (edgeExists(edge) && getEdgeCost(edge, &cost)) { - m[i][j] = doubleToMilliInt(cost); + m[i][j] = static_cast(cost * scale); } else { m[i][j] = std::numeric_limits::max(); } From 4b3bbd3334a79729e6768986cb711e39ba77af73 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 21:24:28 +0200 Subject: [PATCH 03/11] Monitor sweep line candidates. --- polygon_coverage_geometry/src/decomposition.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/polygon_coverage_geometry/src/decomposition.cc b/polygon_coverage_geometry/src/decomposition.cc index 9a9bf5b..707a796 100644 --- a/polygon_coverage_geometry/src/decomposition.cc +++ b/polygon_coverage_geometry/src/decomposition.cc @@ -106,6 +106,7 @@ bool computeBestBCDFromPolygonWithHoles(const PolygonWithHoles& pwh, // Get all possible decomposition directions. std::vector directions = findPerpEdgeDirections(pwh); + ROS_DEBUG("Number of perpendicular edge directions: %d", directions.size()); // For all possible rotations: for (const auto& dir : directions) { From eb8388449b6a3962270e68c38566cb1d92520b00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 21:24:37 +0200 Subject: [PATCH 04/11] Fix test name. --- polygon_coverage_geometry/test/bcd-test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/polygon_coverage_geometry/test/bcd-test.cpp b/polygon_coverage_geometry/test/bcd-test.cpp index 864a302..fad6f37 100644 --- a/polygon_coverage_geometry/test/bcd-test.cpp +++ b/polygon_coverage_geometry/test/bcd-test.cpp @@ -26,7 +26,7 @@ using namespace polygon_coverage_planning; -TEST(BctTest, computeBCD) { +TEST(BcdTest, computeBCD) { // Diamond. PolygonWithHoles diamond(createDiamond()); FT expected_area = diamond.outer_boundary().area(); From 05a0bbd3b2f315e83314a72ae522027b192f21c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 21:25:43 +0200 Subject: [PATCH 05/11] Remove recursive test. --- .../polygon_coverage_solvers/graph_base.h | 20 ------------------- 1 file changed, 20 deletions(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h index e47d2f7..a27b946 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h @@ -131,26 +131,6 @@ class GraphBase { bool is_created_; }; -// Recursive function to search for largest x between lower and upper bound. -inline double searchLargestFeasibleScale(const double low, const double high, - const double condition, - const double eps = 0.1) { - // Early abort. - if (high < low) return low; - // Terminate recursion. - if ((high - low) < eps) return low; - - const double mid = (high + low) / 2.0; - - if (mid < condition) { - // mid satisfies condition. Find even higher scale. - return searchLargestFeasibleScale(mid, high, condition, eps); - } else { - // mid does not satisfy condition. Find smaller scale that does. - return searchLargestFeasibleScale(low, mid, condition, eps); - } -} - } // namespace polygon_coverage_planning #include "polygon_coverage_solvers/impl/graph_base_impl.h" From 906550137073747d49d1d6defc024fffbb1a3948 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 21:26:43 +0200 Subject: [PATCH 06/11] Apply min scale if necessary. Closes https://github.com/ethz-asl/polygon_coverage_planning/issues/75 --- .../impl/graph_base_impl.h | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index 9a88947..554ce57 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -344,34 +344,31 @@ GraphBase::getAdjacencyMatrix() const { auto min_diff = std::numeric_limits::max(); for (size_t i = 0; i < sorted_cost.size() - 2; i++) { auto diff = std::fabs(sorted_cost[i + 1] - sorted_cost[i]); - if (diff > std::numeric_limits::epsilon() && diff < min_diff) + const double equality = 0.000001; // Min. considered cost difference. + if (diff > equality && diff < min_diff) min_diff = diff; } // Only scale with min_diff if this does not blow up the cost. + // Maximum expected cost is number of clusters times max. cost per cluster. // TODO(rikba): Find a tighter upper bound. double total_cost_upper_bound = sorted_cost.back() * graph_.size(); + // Add 0.1 to make sure that min_diff * max_scale > 1.0 to avoid rounding + // errors. double max_scale = 1.1 / min_diff; double min_scale = std::numeric_limits::max() / (total_cost_upper_bound + 1.0); double scale = 1.0; if (max_scale * total_cost_upper_bound < std::numeric_limits::max()) { - ROS_DEBUG("Optimal scale applied."); + ROS_DEBUG("Optimal scale %.9f applied.", max_scale); scale = max_scale; - } else if (min_scale * total_cost_upper_bound + 1 > - std::numeric_limits::max()) { - // Recursive search for largest scale between min_scale and max_scale. - ROS_WARN( - "The adjacency matrix is ill conditioned. Adjacency matrix " - "coefficients are reduced to guarantee upper cost bound. Consider " - "removing small details in the polygon to have even decomposition " - "size."); - ROS_WARN("Recursively searching for best scale."); - scale = searchLargestFeasibleScale( - min_scale, max_scale, - std::numeric_limits::max() / total_cost_upper_bound + 1); - - ROS_WARN_COND(min_diff < 1.0, "Loss of optimality."); + } else { + ROS_DEBUG("Minimum scale %.9f applied.", min_scale); + scale = min_scale; + ROS_WARN_COND( + min_diff < 1.0, + "The adjacency matrix is ill conditioned. Consider removing small " + "details in the polygon to have even decomposition sizes."); } ROS_DEBUG("The minimum cost difference is: %.9f", min_diff); From dfc75e861f189e4f43d7343d5748c15535e5b2c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 21:27:44 +0200 Subject: [PATCH 07/11] Add minimum example of Qiangsun's polygon for tests. --- .../polygon_coverage_geometry/test_comm.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/polygon_coverage_geometry/include/polygon_coverage_geometry/test_comm.h b/polygon_coverage_geometry/include/polygon_coverage_geometry/test_comm.h index d5b3d57..0a92dfd 100644 --- a/polygon_coverage_geometry/include/polygon_coverage_geometry/test_comm.h +++ b/polygon_coverage_geometry/include/polygon_coverage_geometry/test_comm.h @@ -160,6 +160,22 @@ PolygonWithHoles createSophisticatedPolygon() { return poly_with_holes; } +template +Polygon createQiangsun89Polygon() { + Polygon poly; + poly.push_back(Point_2(3572.0, 115.0)); + poly.push_back(Point_2(2724.0, 460.0)); + poly.push_back(Point_2(2660.0, 95.0)); + poly.push_back(Point_2(855.0, 813.0)); + poly.push_back(Point_2(1182.0, 1497.0)); + poly.push_back(Point_2(2925.0, 832.0)); + poly.push_back(Point_2(2905.0, 480.0)); + poly.push_back(Point_2(3608.0, 208.0)); + + return poly; +} + + template bool checkVerticesIdentical(const typename Kernel::Point_2& a, const typename Kernel::Point_2& b) { From e86369bfb8ef3e1e00d821cc2afa1b8e74608180 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 22:09:27 +0200 Subject: [PATCH 08/11] Fix indexing error. --- .../polygon_coverage_solvers/impl/graph_base_impl.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index 554ce57..4c4134c 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -324,6 +324,7 @@ Solution GraphBase::reconstructSolution( template std::vector> GraphBase::getAdjacencyMatrix() const { + ROS_DEBUG("Create adjacency matrix."); // First scale the matrix such that when converting the cost to integers two // costs can always be differentiated (except for if they are the same). Find // the minimum absolute difference between any two values to normalize the @@ -341,10 +342,10 @@ GraphBase::getAdjacencyMatrix() const { } } sort(sorted_cost.begin(), sorted_cost.end()); - auto min_diff = std::numeric_limits::max(); - for (size_t i = 0; i < sorted_cost.size() - 2; i++) { + const double equality = 0.000001; // Min. considered cost difference. + auto min_diff = sorted_cost.back(); + for (size_t i = 0; i < sorted_cost.size() - 1; i++) { auto diff = std::fabs(sorted_cost[i + 1] - sorted_cost[i]); - const double equality = 0.000001; // Min. considered cost difference. if (diff > equality && diff < min_diff) min_diff = diff; } From 6af5dc85d0ba1b4f0e9190dbbc19d494dd1e952a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 22:13:33 +0200 Subject: [PATCH 09/11] Catch zero cost. --- .../polygon_coverage_solvers/impl/graph_base_impl.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index 4c4134c..cac713d 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -342,12 +342,15 @@ GraphBase::getAdjacencyMatrix() const { } } sort(sorted_cost.begin(), sorted_cost.end()); - const double equality = 0.000001; // Min. considered cost difference. + const double equality = 0.000001; // Min. considered cost difference. auto min_diff = sorted_cost.back(); + if (sorted_cost.back() == 0.0) { + ROS_ERROR("Adjacency matrix invalid. Greatest cost is 0.0."); + min_diff = std::numeric_limits::max(); + } for (size_t i = 0; i < sorted_cost.size() - 1; i++) { auto diff = std::fabs(sorted_cost[i + 1] - sorted_cost[i]); - if (diff > equality && diff < min_diff) - min_diff = diff; + if (diff > equality && diff < min_diff) min_diff = diff; } // Only scale with min_diff if this does not blow up the cost. From d8f09bc3a8ed745887c2082cb45d2b3070bb53cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 22:22:02 +0200 Subject: [PATCH 10/11] Loss of optimality note. --- .../include/polygon_coverage_solvers/impl/graph_base_impl.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index cac713d..e2a746f 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -372,7 +372,8 @@ GraphBase::getAdjacencyMatrix() const { ROS_WARN_COND( min_diff < 1.0, "The adjacency matrix is ill conditioned. Consider removing small " - "details in the polygon to have even decomposition sizes."); + "details in the polygon to have even decomposition sizes. Loss of " + "optimality!"); } ROS_DEBUG("The minimum cost difference is: %.9f", min_diff); From 4726c36768af4fbc4e0486a516d972a6a1884c64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Fri, 23 Sep 2022 22:49:39 +0200 Subject: [PATCH 11/11] Only scale up costs if 1.1 diff is not reached. --- .../polygon_coverage_solvers/impl/graph_base_impl.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index e2a746f..2ad91be 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -343,7 +343,8 @@ GraphBase::getAdjacencyMatrix() const { } sort(sorted_cost.begin(), sorted_cost.end()); const double equality = 0.000001; // Min. considered cost difference. - auto min_diff = sorted_cost.back(); + const double min_diff_desired = 1.1; // To distinguish integer value costs. + auto min_diff = min_diff_desired; if (sorted_cost.back() == 0.0) { ROS_ERROR("Adjacency matrix invalid. Greatest cost is 0.0."); min_diff = std::numeric_limits::max(); @@ -353,13 +354,11 @@ GraphBase::getAdjacencyMatrix() const { if (diff > equality && diff < min_diff) min_diff = diff; } - // Only scale with min_diff if this does not blow up the cost. + // Only scale with min_diff if this does not overflow the cost. // Maximum expected cost is number of clusters times max. cost per cluster. // TODO(rikba): Find a tighter upper bound. double total_cost_upper_bound = sorted_cost.back() * graph_.size(); - // Add 0.1 to make sure that min_diff * max_scale > 1.0 to avoid rounding - // errors. - double max_scale = 1.1 / min_diff; + double max_scale = min_diff_desired / min_diff; double min_scale = std::numeric_limits::max() / (total_cost_upper_bound + 1.0); double scale = 1.0;