Merge pull request #77 from ethz-asl/feature/adjacency_matrix_conditi…

…oning

Feature/adjacency matrix conditioning

.gitignore

@@ -30,3 +30,6 @@
 *.exe
 *.out
 *.app
+
+# clion
+.idea/

polygon_coverage_geometry/include/polygon_coverage_geometry/test_comm.h

@@ -160,6 +160,22 @@ PolygonWithHoles createSophisticatedPolygon() {
   return poly_with_holes;
 }
 
+template <class Polygon>
+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 <class Kernel>
 bool checkVerticesIdentical(const typename Kernel::Point_2& a,
                             const typename Kernel::Point_2& b) {

polygon_coverage_geometry/src/decomposition.cc

@@ -106,6 +106,7 @@ bool computeBestBCDFromPolygonWithHoles(const PolygonWithHoles& pwh,
 
   // Get all possible decomposition directions.
   std::vector<Direction_2> directions = findPerpEdgeDirections(pwh);
+  ROS_DEBUG("Number of perpendicular edge directions: %d", directions.size());
 
   // For all possible rotations:
   for (const auto& dir : directions) {

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<Polygon_2>());
   FT expected_area = diamond.outer_boundary().area();

polygon_coverage_solvers/include/polygon_coverage_solvers/graph_base.h

@@ -108,14 +108,6 @@ class GraphBase {
   // transforming cost into milli int.
   std::vector<std::vector<int>> getAdjacencyMatrix() const;
 
-  // Preserving three decimal digits.
-  inline int doubleToMilliInt(double in) const {
-    return static_cast<int>(std::round(in * kToMilli));
-  }
-  inline double milliIntToDouble(int in) const {
-    return static_cast<double>(in) * kFromMilli;
-  }
-
  protected:
   // Called from addNode. Creates all edges to the node at the back of the
   // graph.
@@ -138,6 +130,7 @@ class GraphBase {
   size_t goal_idx_;
   bool is_created_;
 };
+
 }  // namespace polygon_coverage_planning
 
 #include "polygon_coverage_solvers/impl/graph_base_impl.h"

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 <algorithm>
-#include <set>
-
 #include <ros/assert.h>
 #include <ros/console.h>
 
+#include <algorithm>
+#include <set>
+
 namespace polygon_coverage_planning {
 
 template <class NodeProperty, class EdgeProperty>
@@ -324,15 +324,69 @@ Solution GraphBase<NodeProperty, EdgeProperty>::reconstructSolution(
 template <class NodeProperty, class EdgeProperty>
 std::vector<std::vector<int>>
 GraphBase<NodeProperty, EdgeProperty>::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
+  // adjacency matrix.
+  // https://afteracademy.com/blog/minimum-absolute-difference-in-an-array
+  std::vector<double> 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());
+  const double equality = 0.000001;  // Min. considered cost difference.
+  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<double>::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;
+  }
+
+  // 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();
+  double max_scale = min_diff_desired / min_diff;
+  double min_scale =
+      std::numeric_limits<int>::max() / (total_cost_upper_bound + 1.0);
+  double scale = 1.0;
+  if (max_scale * total_cost_upper_bound < std::numeric_limits<int>::max()) {
+    ROS_DEBUG("Optimal scale %.9f applied.", max_scale);
+    scale = max_scale;
+  } 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. 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<std::vector<int>> m(graph_.size(),
                                   std::vector<int>(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<int>(cost * scale);
       } else {
         m[i][j] = std::numeric_limits<int>::max();
       }