Minimum Feedback Arc Set (MFAS) for 1dsfm

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 positive 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,108 @@
+/* ----------------------------------------------------------------------------
+
+ * 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.
+  @addtogroup SFM
+*/
+class MFAS {
+ public:
+  /**
+   * @brief Construct from the nodes in a graph (points in 3D), edges
+   * that are translation 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);
+
+  /**
+   * @brief 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();
+
+  /**
+   * @brief 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/tests/testMFAS.cpp

@@ -0,0 +1,98 @@
+#include <gtsam/sfm/MFAS.h>
+#include <iostream>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+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 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 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
+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};
+
+// helper function to obtain map from keypairs to weights from the 
+// vector representations
+std::map<KeyPair, double> getEdgeWeights(const vector<KeyPair> &graph,
+                                         const vector<double> &weights) {
+  std::map<KeyPair, double> edgeWeights;
+  for (size_t i = 0; i < graph.size(); i++) {
+    edgeWeights[graph[i]] = weights[i];
+  }
+  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) {
+  MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(graph, weights2));
+
+  vector<Key> ordered_nodes = mfas_obj.computeOrdering();
+
+  // ground truth (expected) ordering in this example
+  vector<Key> gt_ordered_nodes = {0, 1, 3, 2};
+
+  // check if the expected ordering is obtained
+  for (size_t i = 0; i < ordered_nodes.size(); i++) {
+    EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
+  }
+
+  map<KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights();
+
+  // 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) {
+    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);
+    } else {
+      EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6);
+    }
+  }
+}
+
+// test the ordering function and the outlierWeights method using
+// weights1 (outlier edge is accepted when projected in a direction that
+// produces weights1)
+TEST(MFAS, OrderingWeights1) {
+  MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(graph, weights1));
+
+  vector<Key> ordered_nodes = mfas_obj.computeOrdering();
+
+  // "ground truth" expected ordering in this example
+  vector<Key> gt_ordered_nodes = {3, 0, 1, 2};
+
+  // check if the expected ordering is obtained
+  for (size_t i = 0; i < ordered_nodes.size(); i++) {
+    EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
+  }
+
+  map<KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights();
+
+  // 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) {
+    EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6);
+  }
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */