Merge pull request #387 from borglab/feature/1dsfm_mfas

Minimum Feedback Arc Set (MFAS) for 1dsfm

gtsam/sfm/MFAS.cpp

@@ -0,0 +1,173 @@
+/**
+ *  @file  MFAS.cpp
+ *  @brief Source file for the MFAS class
+ *  @author Akshay Krishnan
+ *  @date July 2020
+ */
+
+#include <gtsam/sfm/MFAS.h>
+
+#include <algorithm>
+#include <map>
+#include <unordered_map>
+#include <vector>
+#include <unordered_set>
+
+using namespace gtsam;
+using std::map;
+using std::pair;
+using std::unordered_map;
+using std::vector;
+using std::unordered_set;
+
+// A node in the graph.
+struct GraphNode {
+  double inWeightSum;               // Sum of absolute weights of incoming edges
+  double outWeightSum;              // Sum of absolute weights of outgoing edges
+  unordered_set<Key> inNeighbors;   // Nodes from which there is an incoming edge
+  unordered_set<Key> outNeighbors;  // Nodes to which there is an outgoing edge
+
+  // Heuristic for the node that is to select nodes in MFAS.
+  double heuristic() const { return (outWeightSum + 1) / (inWeightSum + 1); }
+};
+
+// A graph is a map from key to GraphNode. This function returns the graph from
+// the edgeWeights between keys.
+unordered_map<Key, GraphNode> graphFromEdges(
+    const map<MFAS::KeyPair, double>& edgeWeights) {
+  unordered_map<Key, GraphNode> graph;
+
+  for (const auto& edgeWeight : edgeWeights) {
+    // The weights can be either negative or positive. The direction of the edge
+    // is the direction of positive weight. This means that the edges is from
+    // edge.first -> edge.second if weight is positive and edge.second ->
+    // edge.first if weight is negative.
+    const MFAS::KeyPair& edge = edgeWeight.first;
+    const double& weight = edgeWeight.second;
+
+    Key edgeSource = weight >= 0 ? edge.first : edge.second;
+    Key edgeDest = weight >= 0 ? edge.second : edge.first;
+
+    // Update the in weight and neighbors for the destination.
+    graph[edgeDest].inWeightSum += std::abs(weight);
+    graph[edgeDest].inNeighbors.insert(edgeSource);
+
+    // Update the out weight and neighbors for the source.
+    graph[edgeSource].outWeightSum += std::abs(weight);
+    graph[edgeSource].outNeighbors.insert(edgeDest);
+  }
+  return graph;
+}
+
+// Selects the next node in the ordering from the graph.
+Key selectNextNodeInOrdering(const unordered_map<Key, GraphNode>& graph) {
+  // Find the root nodes in the graph.
+  for (const auto& keyNode : graph) {
+    // It is a root node if the inWeightSum is close to zero.
+    if (keyNode.second.inWeightSum < 1e-8) {
+      // TODO(akshay-krishnan) if there are multiple roots, it is better to
+      // choose the one with highest heuristic. This is missing in the 1dsfm
+      // solution.
+      return keyNode.first;
+    }
+  }
+  // If there are no root nodes, return the node with the highest heuristic.
+  return std::max_element(graph.begin(), graph.end(),
+                          [](const std::pair<Key, GraphNode>& keyNode1,
+                             const std::pair<Key, GraphNode>& keyNode2) {
+                            return keyNode1.second.heuristic() <
+                                   keyNode2.second.heuristic();
+                          })
+      ->first;
+}
+
+// Returns the absolute weight of the edge between node1 and node2.
+double absWeightOfEdge(const Key key1, const Key key2,
+                       const map<MFAS::KeyPair, double>& edgeWeights) {
+  // Check the direction of the edge before returning.
+  return edgeWeights.find(MFAS::KeyPair(key1, key2)) != edgeWeights.end()
+             ? std::abs(edgeWeights.at(MFAS::KeyPair(key1, key2)))
+             : std::abs(edgeWeights.at(MFAS::KeyPair(key2, key1)));
+}
+
+// Removes a node from the graph and updates edge weights of its neighbors.
+void removeNodeFromGraph(const Key node,
+                         const map<MFAS::KeyPair, double> edgeWeights,
+                         unordered_map<Key, GraphNode>& graph) {
+  // Update the outweights and outNeighbors of node's inNeighbors
+  for (const Key neighbor : graph[node].inNeighbors) {
+    // the edge could be either (*it, choice) with a positive weight or
+    // (choice, *it) with a negative weight
+    graph[neighbor].outWeightSum -=
+        absWeightOfEdge(node, neighbor, edgeWeights);
+    graph[neighbor].outNeighbors.erase(node);
+  }
+  // Update the inWeights and inNeighbors of node's outNeighbors
+  for (const Key neighbor : graph[node].outNeighbors) {
+    graph[neighbor].inWeightSum -= absWeightOfEdge(node, neighbor, edgeWeights);
+    graph[neighbor].inNeighbors.erase(node);
+  }
+  // Erase node.
+  graph.erase(node);
+}
+
+MFAS::MFAS(const std::shared_ptr<vector<Key>>& nodes,
+           const TranslationEdges& relativeTranslations,
+           const Unit3& projectionDirection)
+    : nodes_(nodes) {
+  // Iterate over edges, obtain weights by projecting
+  // their relativeTranslations along the projection direction
+  for (const auto& measurement : relativeTranslations) {
+    edgeWeights_[std::make_pair(measurement.key1(), measurement.key2())] =
+        measurement.measured().dot(projectionDirection);
+  }
+}
+
+vector<Key> MFAS::computeOrdering() const {
+  vector<Key> ordering;  // Nodes in MFAS order (result).
+
+  // A graph is an unordered map from keys to nodes. Each node contains a list
+  // of its adjacent nodes. Create the graph from the edgeWeights.
+  unordered_map<Key, GraphNode> graph = graphFromEdges(edgeWeights_);
+
+  // In each iteration, one node is removed from the graph and appended to the
+  // ordering.
+  while (!graph.empty()) {
+    Key selection = selectNextNodeInOrdering(graph);
+    removeNodeFromGraph(selection, edgeWeights_, graph);
+    ordering.push_back(selection);
+  }
+  return ordering;
+}
+
+map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
+  // Find the ordering.
+  vector<Key> ordering = computeOrdering();
+
+  // Create a map from the node key to its position in the ordering. This makes
+  // it easier to lookup positions of different nodes.
+  unordered_map<Key, int> orderingPositions;
+  for (size_t i = 0; i < ordering.size(); i++) {
+    orderingPositions[ordering[i]] = i;
+  }
+
+  map<KeyPair, double> outlierWeights;
+  // Check if the direction of each edge is consistent with the ordering.
+  for (const auto& edgeWeight : edgeWeights_) {
+    // Find edge source and destination.
+    Key source = edgeWeight.first.first;
+    Key dest = edgeWeight.first.second;
+    if (edgeWeight.second < 0) {
+      std::swap(source, dest);
+    }
+
+    // If the direction is not consistent with the ordering (i.e dest occurs
+    // before src), it is an outlier edge, and has non-zero outlier weight.
+    if (orderingPositions.at(dest) < orderingPositions.at(source)) {
+      outlierWeights[edgeWeight.first] = std::abs(edgeWeight.second);
+    } else {
+      outlierWeights[edgeWeight.first] = 0;
+    }
+  }
+  return outlierWeights;
+}

gtsam/sfm/MFAS.h

@@ -0,0 +1,111 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ *  @file  MFAS.h
+ *  @brief MFAS class to solve Minimum Feedback Arc Set graph problem
+ *  @author Akshay Krishnan
+ *  @date September 2020
+ */
+
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/sfm/BinaryMeasurement.h>
+
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+namespace gtsam {
+
+/**
+  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 directed acyclic graph by removing
+  edges such that the total weight of removed edges is minimum.
+
+  Although MFAS is a general graph problem and can be applied in many areas,
+  this classed was designed for the purpose of outlier rejection in a
+  translation averaging for SfM setting. For more details, refer to the above
+  paper. The nodes of the graph in this context represents cameras in 3D and the
+  edges between them represent unit translations in the world coordinate frame,
+  i.e w_aZb is the unit translation from a to b expressed in the world
+  coordinate frame. The weights for the edges are obtained by projecting the
+  unit translations in a projection direction.
+  @addtogroup SFM
+*/
+class MFAS {
+ public:
+  // used to represent edges between two nodes in the graph. When used in
+  // translation averaging for global SfM
+  using KeyPair = std::pair<Key, Key>;
+  using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
+
+ private:
+  // pointer to nodes in the graph
+  const std::shared_ptr<std::vector<Key>> nodes_;
+
+  // edges with a direction such that all weights are positive
+  // i.e, edges that originally had negative weights are flipped
+  std::map<KeyPair, double> edgeWeights_;
+
+ public:
+  /**
+   * @brief Construct from the nodes in a graph and weighted directed edges
+   * between the nodes. Each node is identified by a Key.
+   * A shared pointer to the nodes is used as input parameter
+   * because, MFAS ordering is usually used to compute the ordering of a
+   * large graph that is already stored in memory. It is unnecessary to make a
+   * copy of the nodes in this class.
+   * @param nodes: Nodes in the graph
+   * @param edgeWeights: weights of edges in the graph
+   */
+  MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
+       const std::map<KeyPair, double> &edgeWeights)
+      : nodes_(nodes), edgeWeights_(edgeWeights) {}
+
+  /**
+   * @brief Constructor to be used in the context of translation averaging.
+   * Here, the nodes of the graph are cameras in 3D and the edges have a unit
+   * translation direction between them. The weights of the edges is computed by
+   * projecting them along a projection direction.
+   * @param nodes cameras in the epipolar graph (each camera is identified by a
+   * Key)
+   * @param relativeTranslations translation directions between the cameras
+   * @param projectionDirection direction in which edges are to be projected
+   */
+  MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
+       const TranslationEdges &relativeTranslations,
+       const Unit3 &projectionDirection);
+
+  /**
+   * @brief Computes the 1D MFAS ordering of nodes in the graph
+   * @return orderedNodes: vector of nodes in the obtained order
+   */
+  std::vector<Key> computeOrdering() const;
+
+  /**
+   * @brief Computes the "outlier weights" of the graph. We define the outlier
+   * weight of a edge to be zero if the edge is an inlier and the magnitude of
+   * its edgeWeight if it is an outlier. This function internally calls
+   * computeOrdering and uses the obtained ordering to identify outlier edges.
+   * @return outlierWeights: map from an edge to its outlier weight.
+   */
+  std::map<KeyPair, double> computeOutlierWeights() const;
+};
+
+}  // namespace gtsam

gtsam/sfm/tests/testMFAS.cpp

@@ -0,0 +1,105 @@
+/**
+ *  @file  testMFAS.cpp
+ *  @brief Unit tests for the MFAS class
+ *  @author Akshay Krishnan
+ *  @date July 2020
+ */
+
+#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<MFAS::KeyPair> edges = {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
+map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
+                                         const vector<double> &weights) {
+  map<MFAS::KeyPair, double> edgeWeights;
+  for (size_t i = 0; i < edges.size(); i++) {
+    edgeWeights[edges[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(edges, 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<MFAS::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 : edges) {
+    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(edges, 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<MFAS::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 : edges) {
+    EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6);
+  }
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */