diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index b784e7efe..dff8b6fa5 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -7,123 +7,162 @@ #include +#include +#include +#include +#include + using namespace gtsam; +using MFAS::KeyPair; +using std::map; using std::pair; +using std::unordered_map; using std::vector; -MFAS::MFAS(const std::shared_ptr> &nodes, +// 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. + vector inNeighbors; // Nodes from which there is an incoming edge. + vector outNeighbors; // Nodes to which there is an outgoing edge. + + // Heuristic for the node that is to select nodes in MFAS. + double heuristic() { 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& [edge, weight] : 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. + 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.push_back(edgeSource); + + // Update the out weight and neighbors for the source. + graph[edgeSource].outWeightSum += std::abs(weight); + graph[edgeSource].outNeighbors.push_back(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& [key, node] : graph) { + // It is a root node if the inWeightSum is close to zero. + if (node.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 key; + } + } + // If there are no root nodes, return the node with the highest heuristic. + return std::max_element(graph.begin(), graph.end(), + [](const std::pair& node1, + const std::pair& node2) { + return node1.second.heuristic() < + node2.second.heuristic(); + }) + ->first; +} + +// Returns the absolute weight of the edge between node1 and node2. +double absWeightOfEdge(const Key node1, const Key node2, + const map& edgeWeights) { + // Check the direction of the edge before returning. + return edgeWeights_.find(KeyPair(node1, node2)) != edgeWeights_.end() + ? std::abs(edgeWeights_.at(KeyPair(node1, node2))) + : std::abs(edgeWeights_.at(KeyPair(node2, node1))); +} + +// 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(graph[neighbor].outNeighbors.find(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(graph[neighbor].inNeighbors.find(node)); + } + // Erase node. + graph.erase(node); +} + +MFAS::MFAS(const std::shared_ptr>& nodes, const TranslationEdges& relativeTranslations, - const Unit3 &projection_direction) + const Unit3& projectionDirection) : nodes_(nodes) { - // iterate over edges, obtain weights by projecting + // Iterate over edges, obtain weights by projecting // their relativeTranslations along the projection direction - for (auto it = relativeTranslations.begin(); - it != relativeTranslations.end(); it++) { - edgeWeights_[it->first] = it->second.dot(projection_direction); + for (const auto& measurement : relativeTranslations) { + edgeWeights_[std::make_pair(measurement.key1(), measurement.key2())] = + measurement.measured().dot(projectionDirection); } } -std::vector MFAS::computeOrdering() const { - FastMap in_weights; // sum on weights of incoming edges for a node - FastMap out_weights; // sum on weights of outgoing edges for a node - FastMap > in_neighbors; - FastMap > out_neighbors; +vector MFAS::computeOrdering() const { + vector ordering; // Nodes in MFAS order (result). - vector ordered_nodes; // nodes in MFAS order (result) - FastMap ordered_positions; // map from node to its position in the output order + // 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_); - // populate neighbors and weights - // Since the weights could be obtained by projection, they can be either - // negative or positive. Ideally, the weights should be positive in the - // direction of the edge. So, we define the direction of the edge as - // edge.first -> edge.second if weight is positive and - // edge.second -> edge.first if weight is negative. Once we know the - // direction, we only use the magnitude of the weights. - for (auto it = edgeWeights_.begin(); it != edgeWeights_.end(); it++) { - const KeyPair &edge = it->first; - const double weight = it->second; - Key edge_source = weight >= 0 ? edge.first : edge.second; - Key edge_dest = weight >= 0 ? edge.second : edge.first; - - in_weights[edge_dest] += std::abs(weight); - out_weights[edge_source] += std::abs(weight); - in_neighbors[edge_dest].push_back(edge_source); - out_neighbors[edge_source].push_back(edge_dest); + // 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); } - - // in each iteration, one node is appended to the ordered list - while (ordered_nodes.size() < nodes_->size()) { - - // finding the node with the max heuristic score - Key choice; - double max_score = 0.0; - - for (const Key &node : *nodes_) { - // if this node has not been chosen so far - if (ordered_positions.find(node) == ordered_positions.end()) { - // is this a root node - if (in_weights[node] < 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. - 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 in_neighbors, adjust their out_weights - for (auto it = in_neighbors[choice].begin(); - it != in_neighbors[choice].end(); ++it) - // the edge could be either (*it, choice) with a positive weight or (choice, *it) with a negative weight - out_weights[*it] -= edgeWeights_.find(KeyPair(*it, choice)) == edgeWeights_.end() ? -edgeWeights_.at(KeyPair(choice, *it)) : edgeWeights_.at(KeyPair(*it, choice)); - - // find its out_neighbors, adjust their in_weights - for (auto it = out_neighbors[choice].begin(); - it != out_neighbors[choice].end(); ++it) - in_weights[*it] -= edgeWeights_.find(KeyPair(choice, *it)) == edgeWeights_.end() ? -edgeWeights_.at(KeyPair(*it, choice)) : edgeWeights_.at(KeyPair(choice, *it)); - - ordered_positions[choice] = ordered_nodes.size(); - ordered_nodes.push_back(choice); - } - return ordered_nodes; + return ordering; } -std::map MFAS::computeOutlierWeights() const { - vector ordered_nodes = computeOrdering(); - FastMap ordered_positions; - std::map outlier_weights; +std::map MFAS::computeOutlierWeights() const { + // Find the ordering. + vector ordering = computeOrdering(); - // create a map becuase it is much faster to lookup the position of each node - // TODO(akshay-krishnan) this is already computed in computeOrdering. Would be nice if - // we could re-use. Either use an optional argument or change the output of - // computeOrdering - for(unsigned int i = 0; i < ordered_nodes.size(); i++) { - ordered_positions[ordered_nodes[i]] = i; + // 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; } - // iterate over all edges - for (auto it = edgeWeights_.begin(); it != edgeWeights_.end(); it++) { - Key edge_source, edge_dest; - if(it->second > 0) { - edge_source = it->first.first; - edge_dest = it->first.second; - } else { - edge_source = it->first.second; - edge_dest = it->first.first; + map outlierWeights; + // Check if the direction of each edge is consistent with the ordering. + for (const auto& [edge, weight] : edgeWeights_) { + // Find edge source and destination. + Key source = edge.first; + Key dest = edge.second; + if (weight < 0) { + std::swap(source, dest); } - // if the ordered position of nodes is not consistent with the edge - // direction for consistency second should be greater than first - if (ordered_positions.at(edge_dest) < ordered_positions.at(edge_source)) { - outlier_weights[it->first] = std::abs(edgeWeights_.at(it->first)); + // 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[edge] = std::abs(weight); } else { - outlier_weights[it->first] = 0; + outlierWeights[edge] = 0; } } - return outlier_weights; + return outlierWeights; } \ No newline at end of file diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index c910e5621..929aa5ff0 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -15,14 +15,15 @@ * @file MFAS.h * @brief MFAS class to solve Minimum Feedback Arc Set graph problem * @author Akshay Krishnan - * @date July 2020 + * @date September 2020 */ #include #include +#include -#include #include +#include #include namespace gtsam { @@ -30,29 +31,29 @@ 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", + 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. + 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 + // 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::map; + using TranslationEdges = std::vector>; private: // pointer to nodes in the graph @@ -65,29 +66,30 @@ class MFAS { public: /** * @brief Construct from the nodes in a graph and weighted directed edges - * between the nodes. Each node is identified by a Key. + * 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 make a - * copy of the nodes in this class. + * 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) {} + 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) + * @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 TranslationEdges &relativeTranslations, const Unit3 &projectionDirection); /** @@ -97,10 +99,10 @@ class MFAS { 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. + * @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; diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 923d75a45..58ea4cc84 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -22,7 +22,7 @@ using namespace gtsam; */ // edges in the graph - last edge from node 3 to 0 is an outlier -vector graph = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), +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)}; @@ -33,11 +33,11 @@ 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 -std::map getEdgeWeights(const vector &graph, +map getEdgeWeights(const vector &edges, const vector &weights) { - std::map edgeWeights; - for (size_t i = 0; i < graph.size(); i++) { - edgeWeights[graph[i]] = weights[i]; + 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; @@ -46,7 +46,7 @@ std::map getEdgeWeights(const vector &grap // 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(graph, weights2)); + MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights2)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -62,7 +62,7 @@ TEST(MFAS, OrderingWeights2) { // 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) { + 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); @@ -76,7 +76,7 @@ TEST(MFAS, OrderingWeights2) { // weights1 (outlier edge is accepted when projected in a direction that // produces weights1) TEST(MFAS, OrderingWeights1) { - MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(graph, weights1)); + MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights1)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -92,7 +92,7 @@ TEST(MFAS, OrderingWeights1) { // 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) { + for (auto &edge : edges) { EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6); } }