changing mfas to use gtsam keys

gtsam/sfm/mfas.cc

@@ -9,35 +9,14 @@ using std::pair;
 using std::set;
 using std::vector;
 
-void reindex_problem(vector<Edge> &edges, map<int, int> &reindexing_key) {
+namespace gtsam {
-  // get the unique set of notes
+namespace mfas {
-  set<int> nodes;
-  for (int i = 0; i < edges.size(); ++i) {
-    nodes.insert(edges[i].first);
-    nodes.insert(edges[i].second);
-  }
 
-  // iterator through them and assign a new name to each vertex
+void flipNegEdges(vector<KeyPair> &edges, vector<double> &weights) {
-  std::set<int>::const_iterator it;
-  reindexing_key.clear();
-  int i = 0;
-  for (it = nodes.begin(); it != nodes.end(); ++it) {
-    reindexing_key[*it] = i;
-    ++i;
-  }
-
-  // now renumber the edges
-  for (int i = 0; i < edges.size(); ++i) {
-    edges[i].first = reindexing_key[edges[i].first];
-    edges[i].second = reindexing_key[edges[i].second];
-  }
-}
-
-void flip_neg_edges(vector<Edge> &edges, vector<double> &weights) {
   // now renumber the edges
   for (int i = 0; i < edges.size(); ++i) {
     if (weights[i] < 0.0) {
-      double tmp = edges[i].second;
+      Key tmp = edges[i].second;
       edges[i].second = edges[i].first;
       edges[i].first = tmp;
       weights[i] *= -1;
@@ -45,26 +24,17 @@ void flip_neg_edges(vector<Edge> &edges, vector<double> &weights) {
   }
 }
 
-void mfas_ratio(const std::vector<Edge> &edges,
+void mfasRatio(const std::vector<KeyPair> &edges,
-                const std::vector<double> &weights, std::vector<int> &order) {
+               const std::vector<double> &weights, const KeyVector &nodes,
-  // find the number of nodes in this problem
+               FastMap<Key, int> &ordered_positions) {
-  int n = -1;
-  int m = edges.size();
-  for (int i = 0; i < m; ++i) {
-    n = (edges[i].first > n) ? edges[i].first : n;
-    n = (edges[i].second > n) ? edges[i].second : n;
-  }
-  n += 1;  // 0 indexed
-
   // initialize data structures
-  vector<double> win_deg(n, 0.0);
+  FastMap<Key, double> win_deg;
-  vector<double> wout_deg(n, 0.0);
+  FastMap<Key, double> wout_deg;
-  vector<bool> unchosen(n, 1);
+  FastMap<Key, vector<pair<int, double> > > inbrs;
-  vector<vector<pair<int, double> > > inbrs(n);
+  FastMap<Key, vector<pair<int, double> > > onbrs;
-  vector<vector<pair<int, double> > > onbrs(n);
 
   // stuff data structures
-  for (int ii = 0; ii < m; ++ii) {
+  for (int ii = 0; ii < edges.size(); ++ii) {
     int i = edges[ii].first;
     int j = edges[ii].second;
     double w = weights[ii];
@@ -75,64 +45,50 @@ void mfas_ratio(const std::vector<Edge> &edges,
     onbrs[i].push_back(pair<int, double>(j, w));
   }
 
-  while (order.size() < n) {
+  int ordered_count = 0;
+  while (ordered_count < nodes.size()) {
     // choose an unchosen node
-    int choice = -1;
+    Key choice;
     double max_score = 0.0;
-    for (int i = 0; i < n; ++i) {
+    for (auto node : nodes) {
-      if (unchosen[i]) {
+      if (ordered_positions.find(node) != ordered_positions.end()) {
         // is this a source
-        if (win_deg[i] < 1e-8) {
+        if (win_deg[node] < 1e-8) {
-          choice = i;
+          choice = node;
           break;
         } else {
-          double score = (wout_deg[i] + 1) / (win_deg[i] + 1);
+          double score = (wout_deg[node] + 1) / (win_deg[node] + 1);
           if (score > max_score) {
             max_score = score;
-            choice = i;
+            choice = node;
           }
         }
       }
     }
 
     // find its inbrs, adjust their wout_deg
-    vector<pair<int, double> >::iterator it;
+    for (auto it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it)
-    for (it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it)
       wout_deg[it->first] -= it->second;
     // find its onbrs, adjust their win_deg
-    for (it = onbrs[choice].begin(); it != onbrs[choice].end(); ++it)
+    for (auto it = onbrs[choice].begin(); it != onbrs[choice].end(); ++it)
       win_deg[it->first] -= it->second;
 
-    order.push_back(choice);
+    ordered_positions[choice] = ordered_count++;
-    unchosen[choice] = 0;
   }
 }
 
-void broken_weight(const std::vector<Edge> &edges,
+void brokenWeights(const std::vector<KeyPair> &edges,
                    const std::vector<double> &weight,
-                   const std::vector<int> &order, std::vector<double> &broken) {
+                   const FastMap<Key, int> &ordered_positions,
-  // clear the output vector
+                   FastMap<Key, double> &broken) {
-  int m = edges.size();
-  broken.resize(m);
-  broken.assign(broken.size(), 0.0);
-
-  // find the number of nodes in this problem
-  int n = -1;
-  for (int i = 0; i < m; ++i) {
-    n = (edges[i].first > n) ? edges[i].first : n;
-    n = (edges[i].second > n) ? edges[i].second : n;
-  }
-  n += 1;  // 0 indexed
-
-  // invert the permutation
-  std::vector<int> inv_perm(n, 0.0);
-  for (int i = 0; i < n; ++i) inv_perm[order[i]] = i;
-
   // find the broken edges
-  for (int i = 0; i < m; ++i) {
+  for (int i = 0; i < edges.size(); ++i) {
-    int x0 = inv_perm[edges[i].first];
+    int x0 = ordered_positions.at(edges[i].first);
-    int x1 = inv_perm[edges[i].second];
+    int x1 = ordered_positions.at(edges[i].second);
     if ((x1 - x0) * weight[i] < 0)
       broken[i] += weight[i] > 0 ? weight[i] : -weight[i];
   }
 }
+
+}  // namespace mfas
+}  // namespace gtsam

gtsam/sfm/mfas.h

@@ -1,25 +1,40 @@
 /*
-  This file contains the code to solve a Minimum feedback arc set (MFAS) problem
+  This file defines functions used to solve a Minimum feedback arc set (MFAS)
+  problem. This code was forked and modified from Kyle Wilson's repository at
+  https://github.com/wilsonkl/SfM_Init.
   Copyright (c) 2014, Kyle Wilson
   All rights reserved.
+
+  Given a weighted directed graph, the objective in a Minimum feedback arc set
+  problem is to obtain a graph that does not contain any cycles by removing
+  edges such that the total weight of removed edges is minimum.
 */
 #ifndef __MFAS_H__
 #define __MFAS_H__
 
+#include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/Values.h>
+
 #include <map>
 #include <vector>
-typedef std::pair<int, int> Edge;
 
-void mfas_ratio(const std::vector<Edge> &edges,
+namespace gtsam {
-                const std::vector<double> &weight, std::vector<int> &order);
 
-void reindex_problem(std::vector<Edge> &edges,
+using KeyPair = std::pair<Key, Key>;
-                     std::map<int, int> &reindexing_key);
 
-void flip_neg_edges(std::vector<Edge> &edges, std::vector<double> &weights);
+namespace mfas {
 
-void broken_weight(const std::vector<Edge> &edges,
+void flipNegEdges(std::vector<KeyPair> &edges, std::vector<double> &weights);
+
+void mfasRatio(const std::vector<KeyPair> &edges,
+               const std::vector<double> &weights, const KeyVector &nodes,
+               FastMap<Key, int> &ordered_positions);
+
+void brokenWeights(const std::vector<KeyPair> &edges,
                    const std::vector<double> &weight,
-                   const std::vector<int> &order, std::vector<double> &broken);
+                   const FastMap<Key, int> &ordered_positions,
+                   FastMap<Key, double> &broken);
 
+}  // namespace mfas
+}  // namespace gtsam
 #endif  // __MFAS_H__