From c1c8b0a1a327892692e2545d8adfda9b3f9b99b8 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 20 Jul 2020 23:32:28 -0700 Subject: [PATCH] changing MFAS to oops and refactoring --- gtsam/sfm/MFAS.cpp | 138 +++++++++++++++++++++++++++++++++++ gtsam/sfm/MFAS.h | 107 +++++++++++++++++++++++++++ gtsam/sfm/mfas.cpp | 101 ------------------------- gtsam/sfm/mfas.h | 65 ----------------- gtsam/sfm/tests/testMFAS.cpp | 105 ++++++++++---------------- 5 files changed, 282 insertions(+), 234 deletions(-) create mode 100644 gtsam/sfm/MFAS.cpp create mode 100644 gtsam/sfm/MFAS.h delete mode 100644 gtsam/sfm/mfas.cpp delete mode 100644 gtsam/sfm/mfas.h diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp new file mode 100644 index 000000000..76b7f532a --- /dev/null +++ b/gtsam/sfm/MFAS.cpp @@ -0,0 +1,138 @@ +#include + +using namespace gtsam; +using std::pair; +using std::vector; + +MFAS::MFAS(const std::shared_ptr> &nodes, + const std::shared_ptr &relativeTranslations, + const Unit3 &projection_direction) + : nodes_(nodes), relativeTranslations_(relativeTranslations), + relativeTranslationsForWeights_(std::make_shared()) { + // 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> &nodes, + const std::map &edgeWeights) : nodes_(nodes), + relativeTranslations_(std::make_shared()), + 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 MFAS::computeOrdering() { + 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; + + // 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 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_; +} \ No newline at end of file diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h new file mode 100644 index 000000000..20213a168 --- /dev/null +++ b/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 +#include + +#include +#include +#include + +namespace gtsam { + +// used to represent edges between two nodes in the graph +using KeyPair = std::pair; +using TranslationEdges = std::map; + +/* + 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> &nodes, + const std::shared_ptr &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> &nodes, + const std::map &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 computeOutlierWeights(); + + /* + * Computes the 1D MFAS ordering of nodes in the graph + * @return orderedNodes: vector of nodes in the obtained order + */ + std::vector computeOrdering(); + + private: + // pointer to nodes in the graph + const std::shared_ptr> nodes_; + // pointer to translation edges (translation directions between two node points) + const std::shared_ptr 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 relativeTranslationsForWeights_; + + // edges with a direction such that all weights are positive + // i.e, edges that originally had negative weights are flipped + std::map positiveEdgeWeights_; + + // map from edges to their outlier weight + std::map outlierWeights_; + + // nodes arranged in the MFAS order + std::vector orderedNodes_; + + // map from nodes to their position in the MFAS order + // used to speed up computation (lookup) when computing outlierWeights_ + FastMap orderedPositions_; +}; + +} // namespace gtsam + +#endif // __MFAS_H__ diff --git a/gtsam/sfm/mfas.cpp b/gtsam/sfm/mfas.cpp deleted file mode 100644 index 4764f2703..000000000 --- a/gtsam/sfm/mfas.cpp +++ /dev/null @@ -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 - -#include -#include -#include - -using std::map; -using std::pair; -using std::set; -using std::vector; - -namespace gtsam { -namespace mfas { - -void flipNegEdges(vector &edges, vector &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 &edges, - const std::vector &weights, const KeyVector &nodes, - FastMap &ordered_positions) { - // initialize data structures - FastMap win_deg; - FastMap wout_deg; - FastMap > > inbrs; - FastMap > > 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(i, w)); - onbrs[i].push_back(pair(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 &edges, - const std::vector &weight, - const FastMap &ordered_positions, - FastMap &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 \ No newline at end of file diff --git a/gtsam/sfm/mfas.h b/gtsam/sfm/mfas.h deleted file mode 100644 index c3d852b92..000000000 --- a/gtsam/sfm/mfas.h +++ /dev/null @@ -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 - -#include -#include - -namespace gtsam { - -using KeyPair = std::pair; - -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 &edges, std::vector &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 &edges, - const std::vector &weights, const KeyVector &nodes, - FastMap &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 &edges, - const std::vector &weight, - const FastMap &ordered_positions, - FastMap &outlier_weights); - -} // namespace mfas -} // namespace gtsam -#endif // __MFAS_H__ diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index d5daee1a6..8b0192ade 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -1,5 +1,5 @@ -#include - +#include +#include #include 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 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 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}; -// Testing the flipNegEdges function for weights1 -TEST(MFAS, FlipNegEdges) { - vector graph_copy = graph; - vector weights1_positive = weights1; - mfas::flipNegEdges(graph_copy, weights1_positive); - - // resulting graph and edges must be of same size - 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); - } +// helper function to obtain map from keypairs to weights from the +// vector representations +std::map getEdgeWeights(const vector &graph, + const vector &weights) { + std::map 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) { - vector graph_copy = graph; - vector weights2_positive = weights2; - mfas::flipNegEdges(graph_copy, weights2_positive); - FastMap ordered_positions; - // compute ordering from positive edge weights - mfas::mfasRatio(graph_copy, weights2_positive, nodes, ordered_positions); + MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(graph, weights2)); - // expected ordering in this example - FastMap gt_ordered_positions; - gt_ordered_positions[0] = 0; - gt_ordered_positions[1] = 1; - gt_ordered_positions[3] = 2; - gt_ordered_positions[2] = 3; + 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 (auto node : nodes) { - EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]); + for (size_t i = 0; i < ordered_nodes.size(); i++) { + EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); } - // testing the outlierWeights method - FastMap outlier_weights; - mfas::outlierWeights(graph_copy, weights2_positive, gt_ordered_positions, - outlier_weights); + 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 : 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 -// produces weights2) +// weights1 (outlier edge is accepted when projected in a direction that +// produces weights1) TEST(MFAS, OrderingWeights1) { - vector graph_copy = graph; - vector weights1_positive = weights1; - mfas::flipNegEdges(graph_copy, weights1_positive); - FastMap ordered_positions; - // compute ordering from positive edge weights - mfas::mfasRatio(graph_copy, weights1_positive, nodes, ordered_positions); + MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(graph, weights1)); - // expected "ground truth" ordering in this example - FastMap gt_ordered_positions; - gt_ordered_positions[3] = 0; - gt_ordered_positions[0] = 1; - gt_ordered_positions[1] = 2; - gt_ordered_positions[2] = 3; + 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 (auto node : nodes) { - EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]); + for (size_t i = 0; i < ordered_nodes.size(); i++) { + EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); } - // since all edges (including the outlier) are consistent with this ordering, - // all outlier_weights must be zero - FastMap outlier_weights; - mfas::outlierWeights(graph, weights1_positive, gt_ordered_positions, - outlier_weights); + 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 : graph) { EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6); }