changing MFAS to oops and refactoring

gtsam/sfm/MFAS.cpp

@@ -0,0 +1,138 @@
+#include <gtsam/sfm/MFAS.h>
+
+using namespace gtsam;
+using std::pair;
+using std::vector;
+
+MFAS::MFAS(const std::shared_ptr<vector<Key>> &nodes,
+           const std::shared_ptr<TranslationEdges> &relativeTranslations,
+           const Unit3 &projection_direction)
+    : nodes_(nodes), relativeTranslations_(relativeTranslations),
+      relativeTranslationsForWeights_(std::make_shared<TranslationEdges>()) {
+  // iterate over edges and flip all edges that have negative weights,
+  // while storing the magnitude of the weights.
+  for (auto it = relativeTranslations->begin();
+       it != relativeTranslations->end(); it++) {
+    KeyPair edge = it->first;
+    double weight = it->second.dot(projection_direction);
+    if (weight < 0.0) {
+      std::swap(edge.first, edge.second);
+      weight *= -1;
+    }
+    positiveEdgeWeights_[edge] = weight;
+  }
+}
+
+MFAS::MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
+           const std::map<KeyPair, double> &edgeWeights) : nodes_(nodes),
+                                                           relativeTranslations_(std::make_shared<TranslationEdges>()),
+                                                           relativeTranslationsForWeights_(std::make_shared<
+                                                               TranslationEdges>()) {
+  // similar to the above direction constructor, but here weights are
+  // provided as input.
+  for (auto it = edgeWeights.begin(); it != edgeWeights.end(); it++) {
+    KeyPair edge = it->first;
+
+    // When constructed like this, we do not have access to the relative translations. 
+    // So, we store the unswapped edge in the relativeTranslationsForWeights_ map with a default 
+    // Unit3 value. This helps retain the original direction of the edge in the returned result
+    // of computeOutlierWeights
+    relativeTranslationsForWeights_->insert({edge, Unit3()});
+
+    double weight = it->second;
+    if (weight < 0.0) {
+      // change the direction of the edge to make weight positive
+      std::swap(edge.first, edge.second);
+      weight *= -1;
+    }
+    positiveEdgeWeights_[edge] = weight;
+  }
+}
+
+std::vector<Key> MFAS::computeOrdering() {
+  FastMap<Key, double> in_weights;    // sum on weights of incoming edges for a node
+  FastMap<Key, double> out_weights;   // sum on weights of outgoing edges for a node
+  FastMap<Key, vector<Key> > in_neighbors;
+  FastMap<Key, vector<Key> > out_neighbors;
+
+  // populate neighbors and weights
+  for (auto it = positiveEdgeWeights_.begin(); it != positiveEdgeWeights_.end(); it++) {
+    const KeyPair &edge = it->first;
+    in_weights[edge.second] += it->second;
+    out_weights[edge.first] += it->second;
+    in_neighbors[edge.second].push_back(edge.first);
+    out_neighbors[edge.first].push_back(edge.second);
+  }
+
+  // in each iteration, one node is appended to the ordered list
+  while (orderedNodes_.size() < nodes_->size()) {
+
+    // finding the node with the max heuristic score
+    Key choice;
+    double max_score = 0.0;
+
+    for (const Key &node : *nodes_) {
+      if (orderedPositions_.find(node) == orderedPositions_.end()) {
+        // is this a source
+        if (in_weights[node] < 1e-8) {
+          choice = node;
+          break;
+        } else {
+          double score = (out_weights[node] + 1) / (in_weights[node] + 1);
+          if (score > max_score) {
+            max_score = score;
+            choice = node;
+          }
+        }
+      }
+    }
+    // find its inbrs, adjust their wout_deg
+    for (auto it = in_neighbors[choice].begin();
+         it != in_neighbors[choice].end(); ++it)
+      out_weights[*it] -= positiveEdgeWeights_[KeyPair(*it, choice)];
+    // find its onbrs, adjust their win_deg
+    for (auto it = out_neighbors[choice].begin();
+         it != out_neighbors[choice].end(); ++it)
+      in_weights[*it] -= positiveEdgeWeights_[KeyPair(choice, *it)];
+
+    orderedPositions_[choice] = orderedNodes_.size();
+    orderedNodes_.push_back(choice);
+  }
+  return orderedNodes_;
+}
+
+std::map<KeyPair, double> MFAS::computeOutlierWeights() {
+  // if ordering has not been computed yet
+  if (orderedNodes_.size() != nodes_->size()) {
+    computeOrdering();
+  }
+  // iterate over all edges
+  // start and end iterators depend on whether we are using relativeTranslations_ or
+  // relativeTranslationsForWeights_ to store the original edge directions
+  TranslationEdges::iterator start, end;
+  if (relativeTranslationsForWeights_->size() == 0) {
+    start = relativeTranslations_->begin();
+    end = relativeTranslations_->end();
+  } else {
+    start = relativeTranslationsForWeights_->begin();
+    end = relativeTranslationsForWeights_->end();
+  }
+
+  for (auto it = start; it != end; it++) {
+    // relativeTranslations may have negative weight edges, we make sure all edges
+    // are along the postive direction by flipping them if they are not.
+    KeyPair edge = it->first;
+    if (positiveEdgeWeights_.find(edge) == positiveEdgeWeights_.end()) {
+      std::swap(edge.first, edge.second);
+    }
+
+    // if the ordered position of nodes is not consistent with the edge
+    // direction for consistency second should be greater than first
+    if (orderedPositions_.at(edge.second) < orderedPositions_.at(edge.first)) {
+      outlierWeights_[it->first] = std::abs(positiveEdgeWeights_[edge]);
+    } else {
+      outlierWeights_[it->first] = 0;
+    }
+  }
+  return outlierWeights_;
+}

gtsam/sfm/MFAS.h

@@ -0,0 +1,107 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+#ifndef __MFAS_H__
+#define __MFAS_H__
+
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/inference/Key.h>
+
+#include <map>
+#include <memory>
+#include <vector>
+
+namespace gtsam {
+
+// used to represent edges between two nodes in the graph
+using KeyPair = std::pair<Key, Key>;
+using TranslationEdges = std::map<KeyPair, Unit3>;
+
+/*
+  The MFAS class to solve a Minimum feedback arc set (MFAS)
+  problem. We implement the solution from:
+  Kyle Wilson and Noah Snavely, "Robust Global Translations with 1DSfM", 
+  Proceedings of the European Conference on Computer Vision, ECCV 2014
+
+  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.
+*/
+class MFAS {
+ public:
+  /*
+   * @brief Construct from the nodes in a graph (points in 3D), edges
+   * that are transation directions in 3D and the direction in
+   * which edges are to be projected.
+   * @param nodes Nodes in the graph
+   * @param relativeTranslations translation directions between nodes
+   * @param projectionDirection direction in which edges are to be projected
+   */
+  MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
+       const std::shared_ptr<TranslationEdges> &relativeTranslations,
+       const Unit3 &projectionDirection);
+
+  /*
+   * Construct from the nodes in a graph and weighted directed edges
+   * between the graph. Not recommended for any purpose other than unit testing. 
+   * The computeOutlierWeights method will return an empty output if this constructor 
+   * is used. 
+   * When used in a translation averaging context, these weights are obtained
+   * by projecting edges in a particular direction.
+   * @param nodes: Nodes in the graph
+   * @param edgeWeights: weights of edges in the graph (map from edge to signed double)
+   */
+  MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
+       const std::map<KeyPair, double> &edgeWeights);
+
+  /*
+   * @brief Computes the "outlier weights" of the graph. We define the outlier weight
+   * of a edge to be zero if the edge in an inlier and the magnitude of its edgeWeight
+   * if it is an outlier. This function can only be used when constructing the 
+   * @return outlierWeights: map from an edge to its outlier weight.
+   */
+  std::map<KeyPair, double> computeOutlierWeights();
+
+  /*
+   * Computes the 1D MFAS ordering of nodes in the graph
+   * @return orderedNodes: vector of nodes in the obtained order
+   */
+  std::vector<Key> computeOrdering();
+
+ private:
+  // pointer to nodes in the graph
+  const std::shared_ptr<std::vector<Key>> nodes_;
+  // pointer to translation edges (translation directions between two node points)
+  const std::shared_ptr<TranslationEdges> relativeTranslations_;
+
+  // relative translations when the object is initialized without using the actual
+  // relative translations, but with the weights from projecting in a certain 
+  // direction. This is used for unit testing, but not in practice. 
+  std::shared_ptr<TranslationEdges> relativeTranslationsForWeights_;
+
+  // edges with a direction such that all weights are positive
+  // i.e, edges that originally had negative weights are flipped
+  std::map<KeyPair, double> positiveEdgeWeights_;
+
+  // map from edges to their outlier weight
+  std::map<KeyPair, double> outlierWeights_;
+
+  // nodes arranged in the MFAS order
+  std::vector<Key> orderedNodes_;
+
+  // map from nodes to their position in the MFAS order
+  // used to speed up computation (lookup) when computing outlierWeights_
+  FastMap<Key, int> orderedPositions_;
+};
+
+}  // namespace gtsam
+
+#endif  // __MFAS_H__

gtsam/sfm/mfas.cpp

@@ -1,101 +0,0 @@
-/*
-  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.
-*/
-
-#include <gtsam/sfm/mfas.h>
-
-#include <map>
-#include <set>
-#include <vector>
-
-using std::map;
-using std::pair;
-using std::set;
-using std::vector;
-
-namespace gtsam {
-namespace mfas {
-
-void flipNegEdges(vector<KeyPair> &edges, vector<double> &weights) {
-  // now renumber the edges
-  for (unsigned int i = 0; i < edges.size(); ++i) {
-    if (weights[i] < 0.0) {
-      Key tmp = edges[i].second;
-      edges[i].second = edges[i].first;
-      edges[i].first = tmp;
-      weights[i] *= -1;
-    }
-  }
-}
-
-void mfasRatio(const std::vector<KeyPair> &edges,
-               const std::vector<double> &weights, const KeyVector &nodes,
-               FastMap<Key, int> &ordered_positions) {
-  // initialize data structures
-  FastMap<Key, double> win_deg;
-  FastMap<Key, double> wout_deg;
-  FastMap<Key, vector<pair<int, double> > > inbrs;
-  FastMap<Key, vector<pair<int, double> > > onbrs;
-
-  // stuff data structures
-  for (unsigned int ii = 0; ii < edges.size(); ++ii) {
-    int i = edges[ii].first;
-    int j = edges[ii].second;
-    double w = weights[ii];
-
-    win_deg[j] += w;
-    wout_deg[i] += w;
-    inbrs[j].push_back(pair<int, double>(i, w));
-    onbrs[i].push_back(pair<int, double>(j, w));
-  }
-
-  unsigned int ordered_count = 0;
-  while (ordered_count < nodes.size()) {
-    // choose an unchosen node
-    Key choice;
-    double max_score = 0.0;
-    for (auto node : nodes) {
-      if (ordered_positions.find(node) == ordered_positions.end()) {
-        // is this a source
-        if (win_deg[node] < 1e-8) {
-          choice = node;
-          break;
-        } else {
-          double score = (wout_deg[node] + 1) / (win_deg[node] + 1);
-          if (score > max_score) {
-            max_score = score;
-            choice = node;
-          }
-        }
-      }
-    }
-    // find its inbrs, adjust their wout_deg
-    for (auto it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it)
-      wout_deg[it->first] -= it->second;
-    // find its onbrs, adjust their win_deg
-    for (auto it = onbrs[choice].begin(); it != onbrs[choice].end(); ++it)
-      win_deg[it->first] -= it->second;
-
-    ordered_positions[choice] = ordered_count++;
-  }
-}
-
-void outlierWeights(const std::vector<KeyPair> &edges,
-                    const std::vector<double> &weight,
-                    const FastMap<Key, int> &ordered_positions,
-                    FastMap<KeyPair, double> &outlier_weights) {
-  // find the outlier edges
-  for (unsigned int i = 0; i < edges.size(); ++i) {
-    int x0 = ordered_positions.at(edges[i].first);
-    int x1 = ordered_positions.at(edges[i].second);
-    if ((x1 - x0) * weight[i] < 0)
-      outlier_weights[edges[i]] += weight[i] > 0 ? weight[i] : -weight[i];
-  }
-}
-
-}  // namespace mfas
-}  // namespace gtsam

gtsam/sfm/mfas.h

@@ -1,65 +0,0 @@
-/*
-  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 <map>
-#include <vector>
-
-namespace gtsam {
-
-using KeyPair = std::pair<Key, Key>;
-
-namespace mfas {
-
-/*
- * Given a vector of KeyPairs that constitutes edges in a graph and the weights
- * corresponding to these edges, this function changes all the weights to
- * positive numbers by flipping the direction of the edges that have a negative
- * weight. The changes are made in place.
- * @param edges reference to vector of KeyPairs
- * @param weights weights corresponding to edges
- */
-void flipNegEdges(std::vector<KeyPair> &edges, std::vector<double> &weights);
-
-/*
- * Computes the MFAS ordering, ie an ordering of the nodes in the graph such
- * that the source of any edge appears before its destination in the ordering.
- * The weight of edges that are removed to obtain this ordering is minimized.
- * @param edges: edges in the graph
- * @param weights: weights corresponding to the edges (have to be positive)
- * @param nodes: nodes in the graph
- * @param ordered_positions: map from node to position in the ordering (0
- * indexed)
- */
-void mfasRatio(const std::vector<KeyPair> &edges,
-               const std::vector<double> &weights, const KeyVector &nodes,
-               FastMap<Key, int> &ordered_positions);
-
-/*
- * Returns the weights of edges that are not consistent with the input ordering.
- * @param edges in the graph
- * @param weights of the edges in the graph
- * @param ordered_positions: ordering (obtained from MFAS solution)
- * @param outlier_weights: reference to a map from edges to their "outlier
- * weights"
- */
-void outlierWeights(const std::vector<KeyPair> &edges,
-                    const std::vector<double> &weight,
-                    const FastMap<Key, int> &ordered_positions,
-                    FastMap<KeyPair, double> &outlier_weights);
-
-}  // namespace mfas
-}  // namespace gtsam
-#endif  // __MFAS_H__

gtsam/sfm/tests/testMFAS.cpp

@@ -1,5 +1,5 @@
-#include <gtsam/sfm/mfas.h>
+#include <gtsam/sfm/MFAS.h>
-
+#include <iostream>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -8,75 +8,52 @@ using namespace gtsam;
 /* We (partially) use the example from the paper on 1dsfm
  * (https://research.cs.cornell.edu/1dsfm/docs/1DSfM_ECCV14.pdf, Fig 1, Page 5)
  * for the unit tests here. The only change is that we leave out node 4 and use
- * only nodes 0-3. This not only makes the test easier to understand but also
+ * only nodes 0-3. This makes the test easier to understand and also
  * avoids an ambiguity in the ground truth ordering that arises due to
- * insufficient edges in the geaph. */
+ * insufficient edges in the geaph when using the 4th node. */
 
 // edges in the graph - last edge from node 3 to 0 is an outlier
 vector<KeyPair> graph = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1),
                          make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)};
 // nodes in the graph
-KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)};
+vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)};
 // weights from projecting in direction-1 (bad direction, outlier accepted)
 vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75};
 // weights from projecting in direction-2 (good direction, outlier rejected)
 vector<double> weights2 = {0.5, 0.75, -0.25, 0.75, 1, 0.5};
 
-// Testing the flipNegEdges function for weights1
+// helper function to obtain map from keypairs to weights from the 
-TEST(MFAS, FlipNegEdges) {
+// vector representations
-  vector<KeyPair> graph_copy = graph;
+std::map<KeyPair, double> getEdgeWeights(const vector<KeyPair> &graph,
-  vector<double> weights1_positive = weights1;
+                                         const vector<double> &weights) {
-  mfas::flipNegEdges(graph_copy, weights1_positive);
+  std::map<KeyPair, double> edgeWeights;
-
+  for (size_t i = 0; i < graph.size(); i++) {
-  // resulting graph and edges must be of same size
+    edgeWeights[graph[i]] = weights[i];
-  EXPECT_LONGS_EQUAL(graph_copy.size(), graph.size());
-  EXPECT_LONGS_EQUAL(weights1_positive.size(), weights1.size());
-
-  for (unsigned int i = 0; i < weights1.size(); i++) {
-    if (weights1[i] < 0) {
-      // if original weight was negative, edges must be flipped and new weight
-      // must be positive
-      EXPECT_DOUBLES_EQUAL(weights1_positive[i], -weights1[i], 1e-6);
-      EXPECT(graph_copy[i].first == graph[i].second &&
-             graph_copy[i].second == graph[i].first);
-    } else {
-      // unchanged if original weight was positive
-      EXPECT_DOUBLES_EQUAL(weights1_positive[i], weights1[i], 1e-6);
-      EXPECT(graph_copy[i].first == graph[i].first &&
-             graph_copy[i].second == graph[i].second);
-    }
   }
+  cout << "returning edge weights " << edgeWeights.size() << endl;
+  return edgeWeights;
 }
 
 // test the ordering and the outlierWeights function using weights2 - outlier
 // edge is rejected when projected in a direction that gives weights2
 TEST(MFAS, OrderingWeights2) {
-  vector<KeyPair> graph_copy = graph;
+  MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(graph, weights2));
-  vector<double> weights2_positive = weights2;
-  mfas::flipNegEdges(graph_copy, weights2_positive);
-  FastMap<Key, int> ordered_positions;
-  // compute ordering from positive edge weights
-  mfas::mfasRatio(graph_copy, weights2_positive, nodes, ordered_positions);
 
-  // expected ordering in this example
+  vector<Key> ordered_nodes = mfas_obj.computeOrdering();
-  FastMap<Key, int> gt_ordered_positions;
+
-  gt_ordered_positions[0] = 0;
+  // ground truth (expected) ordering in this example
-  gt_ordered_positions[1] = 1;
+  vector<Key> gt_ordered_nodes = {0, 1, 3, 2};
-  gt_ordered_positions[3] = 2;
-  gt_ordered_positions[2] = 3;
 
   // check if the expected ordering is obtained
-  for (auto node : nodes) {
+  for (size_t i = 0; i < ordered_nodes.size(); i++) {
-    EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]);
+    EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
   }
 
-  // testing the outlierWeights method
+  map<KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights();
-  FastMap<KeyPair, double> outlier_weights;
+
-  mfas::outlierWeights(graph_copy, weights2_positive, gt_ordered_positions,
-                       outlier_weights);
   // since edge between 3 and 0 is inconsistent with the ordering, it must have
   // positive outlier weight, other outlier weights must be zero
-  for (auto &edge : graph_copy) {
+  for (auto &edge : graph) {
     if (edge == make_pair(Key(3), Key(0)) ||
         edge == make_pair(Key(0), Key(3))) {
       EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0.5, 1e-6);
@@ -87,33 +64,25 @@ TEST(MFAS, OrderingWeights2) {
 }
 
 // test the ordering function and the outlierWeights method using
-// weights2 (outlier edge is accepted when projected in a direction that
+// weights1 (outlier edge is accepted when projected in a direction that
-// produces weights2)
+// produces weights1)
 TEST(MFAS, OrderingWeights1) {
-  vector<KeyPair> graph_copy = graph;
+  MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(graph, weights1));
-  vector<double> weights1_positive = weights1;
-  mfas::flipNegEdges(graph_copy, weights1_positive);
-  FastMap<Key, int> ordered_positions;
-  // compute ordering from positive edge weights
-  mfas::mfasRatio(graph_copy, weights1_positive, nodes, ordered_positions);
 
-  // expected "ground truth" ordering in this example
+  vector<Key> ordered_nodes = mfas_obj.computeOrdering();
-  FastMap<Key, int> gt_ordered_positions;
+
-  gt_ordered_positions[3] = 0;
+  // "ground truth" expected ordering in this example
-  gt_ordered_positions[0] = 1;
+  vector<Key> gt_ordered_nodes = {3, 0, 1, 2};
-  gt_ordered_positions[1] = 2;
-  gt_ordered_positions[2] = 3;
 
   // check if the expected ordering is obtained
-  for (auto node : nodes) {
+  for (size_t i = 0; i < ordered_nodes.size(); i++) {
-    EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]);
+    EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
   }
 
-  // since all edges (including the outlier) are consistent with this ordering,
+  map<KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights();
-  // all outlier_weights must be zero
+
-  FastMap<KeyPair, double> outlier_weights;
+  // since edge between 3 and 0 is inconsistent with the ordering, it must have
-  mfas::outlierWeights(graph, weights1_positive, gt_ordered_positions,
+  // positive outlier weight, other outlier weights must be zero
-                       outlier_weights);
   for (auto &edge : graph) {
     EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6);
   }