Method to condition adjacency matrix.

Author: rikba

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,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"

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,68 @@ Solution GraphBase<NodeProperty, EdgeProperty>::reconstructSolution(
 template <class NodeProperty, class EdgeProperty>
 std::vector<std::vector<int>>
 GraphBase<NodeProperty, EdgeProperty>::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<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());
+  auto min_diff = std::numeric_limits<double>::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<double>::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<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 applied.");
+    scale = max_scale;
+  } else if (min_scale * total_cost_upper_bound + 1 >
+             std::numeric_limits<int>::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<int>::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<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();
       }