diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp new file mode 100644 index 000000000..6dccc2dee --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include + +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 inNeighbors; // Nodes from which there is an incoming edge + unordered_set 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 graphFromEdges( + const map& edgeWeights) { + unordered_map 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& 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& keyNode1, + const std::pair& 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& 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 edgeWeights, + unordered_map& 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>& 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 MFAS::computeOrdering() const { + vector 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 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::computeOutlierWeights() const { + // Find the ordering. + vector 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 orderingPositions; + for (size_t i = 0; i < ordering.size(); i++) { + orderingPositions[ordering[i]] = i; + } + + map 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; +} diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h new file mode 100644 index 000000000..929aa5ff0 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +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; + using TranslationEdges = std::vector>; + + private: + // pointer to nodes in the graph + const std::shared_ptr> nodes_; + + // edges with a direction such that all weights are positive + // i.e, edges that originally had negative weights are flipped + std::map 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> &nodes, + const std::map &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> &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 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 computeOutlierWeights() const; +}; + +} // namespace gtsam diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp new file mode 100644 index 000000000..58ea4cc84 --- /dev/null +++ b/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 +#include +#include + +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 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 nodes = {Key(0), Key(1), Key(2), Key(3)}; +// weights from projecting in direction-1 (bad direction, outlier accepted) +vector weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; +// weights from projecting in direction-2 (good direction, outlier rejected) +vector 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 getEdgeWeights(const vector &edges, + const vector &weights) { + map 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>(nodes), getEdgeWeights(edges, weights2)); + + vector ordered_nodes = mfas_obj.computeOrdering(); + + // ground truth (expected) ordering in this example + vector 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 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>(nodes), getEdgeWeights(edges, weights1)); + + vector ordered_nodes = mfas_obj.computeOrdering(); + + // "ground truth" expected ordering in this example + vector 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 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); +} +/* ************************************************************************* */