From ab047d6962cd93414d695344cb7fd3360a4febae Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 2 Jul 2020 23:35:34 -0700 Subject: [PATCH 01/13] forking code for mfas from 1dsfm --- gtsam/sfm/mfas.cc | 138 ++++++++++++++++++++++++++++++++++++++++++++++ gtsam/sfm/mfas.h | 25 +++++++++ 2 files changed, 163 insertions(+) create mode 100644 gtsam/sfm/mfas.cc create mode 100644 gtsam/sfm/mfas.h diff --git a/gtsam/sfm/mfas.cc b/gtsam/sfm/mfas.cc new file mode 100644 index 000000000..4bc8f21a6 --- /dev/null +++ b/gtsam/sfm/mfas.cc @@ -0,0 +1,138 @@ +#include "mfas.h" + +#include +#include +#include + +using std::map; +using std::pair; +using std::set; +using std::vector; + +void reindex_problem(vector &edges, map &reindexing_key) { + // get the unique set of notes + set nodes; + for (int i = 0; i < edges.size(); ++i) { + nodes.insert(edges[i].first); + nodes.insert(edges[i].second); + } + + // iterator through them and assign a new name to each vertex + std::set::const_iterator it; + reindexing_key.clear(); + int i = 0; + for (it = nodes.begin(); it != nodes.end(); ++it) { + reindexing_key[*it] = i; + ++i; + } + + // now renumber the edges + for (int i = 0; i < edges.size(); ++i) { + edges[i].first = reindexing_key[edges[i].first]; + edges[i].second = reindexing_key[edges[i].second]; + } +} + +void flip_neg_edges(vector &edges, vector &weights) { + // now renumber the edges + for (int i = 0; i < edges.size(); ++i) { + if (weights[i] < 0.0) { + double tmp = edges[i].second; + edges[i].second = edges[i].first; + edges[i].first = tmp; + weights[i] *= -1; + } + } +} + +void mfas_ratio(const std::vector &edges, + const std::vector &weights, std::vector &order) { + // find the number of nodes in this problem + int n = -1; + int m = edges.size(); + for (int i = 0; i < m; ++i) { + n = (edges[i].first > n) ? edges[i].first : n; + n = (edges[i].second > n) ? edges[i].second : n; + } + n += 1; // 0 indexed + + // initialize data structures + vector win_deg(n, 0.0); + vector wout_deg(n, 0.0); + vector unchosen(n, 1); + vector > > inbrs(n); + vector > > onbrs(n); + + // stuff data structures + for (int ii = 0; ii < m; ++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)); + } + + while (order.size() < n) { + // choose an unchosen node + int choice = -1; + double max_score = 0.0; + for (int i = 0; i < n; ++i) { + if (unchosen[i]) { + // is this a source + if (win_deg[i] < 1e-8) { + choice = i; + break; + } else { + double score = (wout_deg[i] + 1) / (win_deg[i] + 1); + if (score > max_score) { + max_score = score; + choice = i; + } + } + } + } + + // find its inbrs, adjust their wout_deg + vector >::iterator it; + for (it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it) + wout_deg[it->first] -= it->second; + // find its onbrs, adjust their win_deg + for (it = onbrs[choice].begin(); it != onbrs[choice].end(); ++it) + win_deg[it->first] -= it->second; + + order.push_back(choice); + unchosen[choice] = 0; + } +} + +void broken_weight(const std::vector &edges, + const std::vector &weight, + const std::vector &order, std::vector &broken) { + // clear the output vector + int m = edges.size(); + broken.resize(m); + broken.assign(broken.size(), 0.0); + + // find the number of nodes in this problem + int n = -1; + for (int i = 0; i < m; ++i) { + n = (edges[i].first > n) ? edges[i].first : n; + n = (edges[i].second > n) ? edges[i].second : n; + } + n += 1; // 0 indexed + + // invert the permutation + std::vector inv_perm(n, 0.0); + for (int i = 0; i < n; ++i) inv_perm[order[i]] = i; + + // find the broken edges + for (int i = 0; i < m; ++i) { + int x0 = inv_perm[edges[i].first]; + int x1 = inv_perm[edges[i].second]; + if ((x1 - x0) * weight[i] < 0) + broken[i] += weight[i] > 0 ? weight[i] : -weight[i]; + } +} diff --git a/gtsam/sfm/mfas.h b/gtsam/sfm/mfas.h new file mode 100644 index 000000000..57f69a756 --- /dev/null +++ b/gtsam/sfm/mfas.h @@ -0,0 +1,25 @@ +/* + This file contains the code to solve a Minimum feedback arc set (MFAS) problem + Copyright (c) 2014, Kyle Wilson + All rights reserved. +*/ +#ifndef __MFAS_H__ +#define __MFAS_H__ + +#include +#include +typedef std::pair Edge; + +void mfas_ratio(const std::vector &edges, + const std::vector &weight, std::vector &order); + +void reindex_problem(std::vector &edges, + std::map &reindexing_key); + +void flip_neg_edges(std::vector &edges, std::vector &weights); + +void broken_weight(const std::vector &edges, + const std::vector &weight, + const std::vector &order, std::vector &broken); + +#endif // __MFAS_H__ From 636178f3bd8a8b850cde23fadebe12e3970f7b4a Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 6 Jul 2020 00:43:25 -0700 Subject: [PATCH 02/13] changing mfas to use gtsam keys --- gtsam/sfm/mfas.cc | 110 ++++++++++++++-------------------------------- gtsam/sfm/mfas.h | 33 ++++++++++---- 2 files changed, 57 insertions(+), 86 deletions(-) diff --git a/gtsam/sfm/mfas.cc b/gtsam/sfm/mfas.cc index 4bc8f21a6..f2244bdfc 100644 --- a/gtsam/sfm/mfas.cc +++ b/gtsam/sfm/mfas.cc @@ -9,35 +9,14 @@ using std::pair; using std::set; using std::vector; -void reindex_problem(vector &edges, map &reindexing_key) { - // get the unique set of notes - set nodes; - for (int i = 0; i < edges.size(); ++i) { - nodes.insert(edges[i].first); - nodes.insert(edges[i].second); - } +namespace gtsam { +namespace mfas { - // iterator through them and assign a new name to each vertex - std::set::const_iterator it; - reindexing_key.clear(); - int i = 0; - for (it = nodes.begin(); it != nodes.end(); ++it) { - reindexing_key[*it] = i; - ++i; - } - - // now renumber the edges - for (int i = 0; i < edges.size(); ++i) { - edges[i].first = reindexing_key[edges[i].first]; - edges[i].second = reindexing_key[edges[i].second]; - } -} - -void flip_neg_edges(vector &edges, vector &weights) { +void flipNegEdges(vector &edges, vector &weights) { // now renumber the edges for (int i = 0; i < edges.size(); ++i) { if (weights[i] < 0.0) { - double tmp = edges[i].second; + Key tmp = edges[i].second; edges[i].second = edges[i].first; edges[i].first = tmp; weights[i] *= -1; @@ -45,26 +24,17 @@ void flip_neg_edges(vector &edges, vector &weights) { } } -void mfas_ratio(const std::vector &edges, - const std::vector &weights, std::vector &order) { - // find the number of nodes in this problem - int n = -1; - int m = edges.size(); - for (int i = 0; i < m; ++i) { - n = (edges[i].first > n) ? edges[i].first : n; - n = (edges[i].second > n) ? edges[i].second : n; - } - n += 1; // 0 indexed - +void mfasRatio(const std::vector &edges, + const std::vector &weights, const KeyVector &nodes, + FastMap &ordered_positions) { // initialize data structures - vector win_deg(n, 0.0); - vector wout_deg(n, 0.0); - vector unchosen(n, 1); - vector > > inbrs(n); - vector > > onbrs(n); + FastMap win_deg; + FastMap wout_deg; + FastMap > > inbrs; + FastMap > > onbrs; // stuff data structures - for (int ii = 0; ii < m; ++ii) { + for (int ii = 0; ii < edges.size(); ++ii) { int i = edges[ii].first; int j = edges[ii].second; double w = weights[ii]; @@ -75,64 +45,50 @@ void mfas_ratio(const std::vector &edges, onbrs[i].push_back(pair(j, w)); } - while (order.size() < n) { + int ordered_count = 0; + while (ordered_count < nodes.size()) { // choose an unchosen node - int choice = -1; + Key choice; double max_score = 0.0; - for (int i = 0; i < n; ++i) { - if (unchosen[i]) { + for (auto node : nodes) { + if (ordered_positions.find(node) != ordered_positions.end()) { // is this a source - if (win_deg[i] < 1e-8) { - choice = i; + if (win_deg[node] < 1e-8) { + choice = node; break; } else { - double score = (wout_deg[i] + 1) / (win_deg[i] + 1); + double score = (wout_deg[node] + 1) / (win_deg[node] + 1); if (score > max_score) { max_score = score; - choice = i; + choice = node; } } } } // find its inbrs, adjust their wout_deg - vector >::iterator it; - for (it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it) + 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 (it = onbrs[choice].begin(); it != onbrs[choice].end(); ++it) + for (auto it = onbrs[choice].begin(); it != onbrs[choice].end(); ++it) win_deg[it->first] -= it->second; - order.push_back(choice); - unchosen[choice] = 0; + ordered_positions[choice] = ordered_count++; } } -void broken_weight(const std::vector &edges, +void brokenWeights(const std::vector &edges, const std::vector &weight, - const std::vector &order, std::vector &broken) { - // clear the output vector - int m = edges.size(); - broken.resize(m); - broken.assign(broken.size(), 0.0); - - // find the number of nodes in this problem - int n = -1; - for (int i = 0; i < m; ++i) { - n = (edges[i].first > n) ? edges[i].first : n; - n = (edges[i].second > n) ? edges[i].second : n; - } - n += 1; // 0 indexed - - // invert the permutation - std::vector inv_perm(n, 0.0); - for (int i = 0; i < n; ++i) inv_perm[order[i]] = i; - + const FastMap &ordered_positions, + FastMap &broken) { // find the broken edges - for (int i = 0; i < m; ++i) { - int x0 = inv_perm[edges[i].first]; - int x1 = inv_perm[edges[i].second]; + for (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) broken[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 index 57f69a756..b76064ed6 100644 --- a/gtsam/sfm/mfas.h +++ b/gtsam/sfm/mfas.h @@ -1,25 +1,40 @@ /* - This file contains the code to solve a Minimum feedback arc set (MFAS) problem + 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 #include -typedef std::pair Edge; -void mfas_ratio(const std::vector &edges, - const std::vector &weight, std::vector &order); +namespace gtsam { -void reindex_problem(std::vector &edges, - std::map &reindexing_key); +using KeyPair = std::pair; -void flip_neg_edges(std::vector &edges, std::vector &weights); +namespace mfas { -void broken_weight(const std::vector &edges, +void flipNegEdges(std::vector &edges, std::vector &weights); + +void mfasRatio(const std::vector &edges, + const std::vector &weights, const KeyVector &nodes, + FastMap &ordered_positions); + +void brokenWeights(const std::vector &edges, const std::vector &weight, - const std::vector &order, std::vector &broken); + const FastMap &ordered_positions, + FastMap &broken); +} // namespace mfas +} // namespace gtsam #endif // __MFAS_H__ From 23ed11549e4a49be712cad70c545c7f2fc1a4164 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 7 Jul 2020 23:40:57 -0700 Subject: [PATCH 03/13] adding tests --- gtsam/sfm/mfas.h | 22 ++++++++- gtsam/sfm/tests/testMFAS.cpp | 90 ++++++++++++++++++++++++++++++++++++ 2 files changed, 111 insertions(+), 1 deletion(-) create mode 100644 gtsam/sfm/tests/testMFAS.cpp diff --git a/gtsam/sfm/mfas.h b/gtsam/sfm/mfas.h index b76064ed6..6aec492c4 100644 --- a/gtsam/sfm/mfas.h +++ b/gtsam/sfm/mfas.h @@ -13,7 +13,6 @@ #define __MFAS_H__ #include -#include #include #include @@ -24,12 +23,33 @@ 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 broken: reference to a map from edges to their "broken weights" + */ void brokenWeights(const std::vector &edges, const std::vector &weight, const FastMap &ordered_positions, diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp new file mode 100644 index 000000000..0c52928c1 --- /dev/null +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -0,0 +1,90 @@ +#include "gtsam/sfm/mfas.h" +#include + +using namespace std; +using namespace gtsam; + +// example from the paper +Key k0(0), k1(1), k2(2), k3(3), k4(4); +KeyPair e3_2(k3, k2), e0_1(k0, k1), e4_2(k4, k2), + e3_1(k3, k1), e4_0(k4, k0), e1_2(k1, k2), + e0_2(k0, k2), out_e3_0(k3, k0); + +vector graph = {make_pair(3, 2), make_pair(0, 1), make_pair(4, 2), + make_pair(3, 1), make_pair(4, 0), + make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; +KeyVector nodes = {0, 1, 2, 3, 4}; +vector weights1 = {2, 1.5, -2, -0.5, -0.5, 0.25, 1, 0.75}; + + +TEST(MFAS, FlipNegEdges) { + vector graph_copy = graph; + vector weights1_copy = weights1; + mfas::flipNegEdges(graph_copy, weights1_copy); + + EXPECT_LONGS_EQUAL(graph_copy.size(), graph.size()); + EXPECT_LONGS_EQUAL(weights1_copy.size(), weights1.size()); + + for (int i = 0; i < weights1.size(); i++) { + if (weights1[i] < 0) { + EXPECT_DOUBLES_EQUAL(weights1_copy[i], -weights1[i], 1e-6); + EXPECT(graph_copy[i].first == graph[i].second && + graph_copy[i].second == graph[i].first); + } else { + EXPECT_DOUBLES_EQUAL(weights1_copy[i], weights1[i], 1e-6); + EXPECT(graph_copy[i].first == graph[i].first && + graph_copy[i].second == graph[i].second); + } + } +} + +// TEST(MFAS, Ordering) { + +// } + +TEST(MFAS, OrderingWithoutRemoval) { + vector graph_copy = graph; + vector weights1_copy = weights1; + mfas::flipNegEdges(graph_copy, weights1_copy); + FastMap ordered_positions; + mfas::mfasRatio(graph_copy, weights1_copy, nodes, ordered_positions); + + FastMap gt_ordered_positions; + gt_ordered_positions[4] = 0; + gt_ordered_positions[3] = 1; + gt_ordered_positions[0] = 2; + gt_ordered_positions[1] = 3; + gt_ordered_positions[2] = 4; + + for(auto it = ordered_positions.begin(); it != ordered_positions.end(); ++it) + { + EXPECT_LONGS_EQUAL(gt_ordered_positions[it->first], it->second); + } +} + +TEST(MFAS, BrokenWeights) { + vector graph_copy = graph; + vector weights1_copy = weights1; + mfas::flipNegEdges(graph_copy, weights1_copy); + + FastMap gt_ordered_positions; + gt_ordered_positions[4] = 0; + gt_ordered_positions[3] = 1; + gt_ordered_positions[0] = 2; + gt_ordered_positions[1] = 3; + gt_ordered_positions[2] = 4; + + FastMap broken_weights; + mfas::brokenWeights(graph, weights1_copy, gt_ordered_positions, + broken_weights); + for (auto it = broken_weights.begin(); it != broken_weights.end(); it++) { + EXPECT_LONGS_EQUAL(it->second, 0); + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From b4e58795b7b6d8b5dcbdda46c8308fad4bbbaea8 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 8 Jul 2020 23:38:19 -0700 Subject: [PATCH 04/13] tests and documentation --- gtsam/sfm/{mfas.cc => mfas.cpp} | 43 +++++++--- gtsam/sfm/mfas.h | 25 +++--- gtsam/sfm/tests/testMFAS.cpp | 135 ++++++++++++++++++++------------ 3 files changed, 133 insertions(+), 70 deletions(-) rename gtsam/sfm/{mfas.cc => mfas.cpp} (59%) diff --git a/gtsam/sfm/mfas.cc b/gtsam/sfm/mfas.cpp similarity index 59% rename from gtsam/sfm/mfas.cc rename to gtsam/sfm/mfas.cpp index f2244bdfc..4ac1783e3 100644 --- a/gtsam/sfm/mfas.cc +++ b/gtsam/sfm/mfas.cpp @@ -1,5 +1,14 @@ +/* + 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 "mfas.h" +#include #include #include #include @@ -14,7 +23,7 @@ namespace mfas { void flipNegEdges(vector &edges, vector &weights) { // now renumber the edges - for (int i = 0; i < edges.size(); ++i) { + 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; @@ -34,7 +43,7 @@ void mfasRatio(const std::vector &edges, FastMap > > onbrs; // stuff data structures - for (int ii = 0; ii < edges.size(); ++ii) { + for (unsigned int ii = 0; ii < edges.size(); ++ii) { int i = edges[ii].first; int j = edges[ii].second; double w = weights[ii]; @@ -45,13 +54,23 @@ void mfasRatio(const std::vector &edges, onbrs[i].push_back(pair(j, w)); } - int ordered_count = 0; + for (auto &node : nodes) { + std::cout << node << " " << win_deg[node] << " " << wout_deg[node] + << std::endl; + for (auto it = inbrs[node].begin(); it != inbrs[node].end(); it++) + std::cout << it->first << "," << it->second << " "; + std::cout << std::endl; + for (auto it = onbrs[node].begin(); it != onbrs[node].end(); it++) + std::cout << it->first << "," << it->second << " "; + std::cout << std::endl; + } + 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()) { + if (ordered_positions.find(node) == ordered_positions.end()) { // is this a source if (win_deg[node] < 1e-8) { choice = node; @@ -65,7 +84,7 @@ void mfasRatio(const std::vector &edges, } } } - + std::cout << "choice is " << choice << std::endl; // find its inbrs, adjust their wout_deg for (auto it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it) wout_deg[it->first] -= it->second; @@ -77,16 +96,16 @@ void mfasRatio(const std::vector &edges, } } -void brokenWeights(const std::vector &edges, - const std::vector &weight, - const FastMap &ordered_positions, - FastMap &broken) { - // find the broken edges - for (int i = 0; i < edges.size(); ++i) { +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) - broken[i] += weight[i] > 0 ? weight[i] : -weight[i]; + outlier_weights[edges[i]] += weight[i] > 0 ? weight[i] : -weight[i]; } } diff --git a/gtsam/sfm/mfas.h b/gtsam/sfm/mfas.h index 6aec492c4..c3d852b92 100644 --- a/gtsam/sfm/mfas.h +++ b/gtsam/sfm/mfas.h @@ -24,20 +24,24 @@ 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. + * 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. + * 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) + * @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, @@ -48,12 +52,13 @@ void mfasRatio(const std::vector &edges, * @param edges in the graph * @param weights of the edges in the graph * @param ordered_positions: ordering (obtained from MFAS solution) - * @param broken: reference to a map from edges to their "broken weights" + * @param outlier_weights: reference to a map from edges to their "outlier + * weights" */ -void brokenWeights(const std::vector &edges, - const std::vector &weight, - const FastMap &ordered_positions, - FastMap &broken); +void outlierWeights(const std::vector &edges, + const std::vector &weight, + const FastMap &ordered_positions, + FastMap &outlier_weights); } // namespace mfas } // namespace gtsam diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 0c52928c1..83c03925b 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -1,84 +1,123 @@ -#include "gtsam/sfm/mfas.h" #include +#include + +#include "gtsam/sfm/mfas.h" + using namespace std; using namespace gtsam; -// example from the paper -Key k0(0), k1(1), k2(2), k3(3), k4(4); -KeyPair e3_2(k3, k2), e0_1(k0, k1), e4_2(k4, k2), - e3_1(k3, k1), e4_0(k4, k0), e1_2(k1, k2), - e0_2(k0, k2), out_e3_0(k3, k0); +/* 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 + * avoids an ambiguity in the ground truth ordering that arises due to + * insufficient edges in the geaph. */ -vector graph = {make_pair(3, 2), make_pair(0, 1), make_pair(4, 2), - make_pair(3, 1), make_pair(4, 0), +// 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)}; -KeyVector nodes = {0, 1, 2, 3, 4}; -vector weights1 = {2, 1.5, -2, -0.5, -0.5, 0.25, 1, 0.75}; - +// nodes in the graph +KeyVector 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_copy = weights1; - mfas::flipNegEdges(graph_copy, weights1_copy); + 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_copy.size(), weights1.size()); + EXPECT_LONGS_EQUAL(weights1_positive.size(), weights1.size()); - for (int i = 0; i < weights1.size(); i++) { + for (unsigned int i = 0; i < weights1.size(); i++) { if (weights1[i] < 0) { - EXPECT_DOUBLES_EQUAL(weights1_copy[i], -weights1[i], 1e-6); + // 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); + graph_copy[i].second == graph[i].first); } else { - EXPECT_DOUBLES_EQUAL(weights1_copy[i], weights1[i], 1e-6); + // 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); + graph_copy[i].second == graph[i].second); } } } -// TEST(MFAS, Ordering) { - -// } - -TEST(MFAS, OrderingWithoutRemoval) { +// 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 weights1_copy = weights1; - mfas::flipNegEdges(graph_copy, weights1_copy); + vector weights2_positive = weights2; + mfas::flipNegEdges(graph_copy, weights2_positive); FastMap ordered_positions; - mfas::mfasRatio(graph_copy, weights1_copy, nodes, ordered_positions); + // compute ordering from positive edge weights + mfas::mfasRatio(graph_copy, weights2_positive, nodes, ordered_positions); + // expected ordering in this example FastMap gt_ordered_positions; - gt_ordered_positions[4] = 0; - gt_ordered_positions[3] = 1; - gt_ordered_positions[0] = 2; - gt_ordered_positions[1] = 3; - gt_ordered_positions[2] = 4; + gt_ordered_positions[0] = 0; + gt_ordered_positions[1] = 1; + gt_ordered_positions[3] = 2; + gt_ordered_positions[2] = 3; - for(auto it = ordered_positions.begin(); it != ordered_positions.end(); ++it) - { - EXPECT_LONGS_EQUAL(gt_ordered_positions[it->first], it->second); + // check if the expected ordering is obtained + for (auto node : nodes) { + EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]); + } + + // testing the outlierWeights method + FastMap outlier_weights; + mfas::outlierWeights(graph_copy, weights2_positive, gt_ordered_positions, + outlier_weights); + // 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) { + 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(MFAS, BrokenWeights) { +// test the ordering function and the outlierWeights method using +// weights2 (outlier edge is accepted when projected in a direction that +// produces weights2) +TEST(MFAS, OrderingWeights1) { vector graph_copy = graph; - vector weights1_copy = weights1; - mfas::flipNegEdges(graph_copy, weights1_copy); + 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); + // expected "ground truth" ordering in this example FastMap gt_ordered_positions; - gt_ordered_positions[4] = 0; - gt_ordered_positions[3] = 1; - gt_ordered_positions[0] = 2; - gt_ordered_positions[1] = 3; - gt_ordered_positions[2] = 4; + gt_ordered_positions[3] = 0; + gt_ordered_positions[0] = 1; + gt_ordered_positions[1] = 2; + gt_ordered_positions[2] = 3; - FastMap broken_weights; - mfas::brokenWeights(graph, weights1_copy, gt_ordered_positions, - broken_weights); - for (auto it = broken_weights.begin(); it != broken_weights.end(); it++) { - EXPECT_LONGS_EQUAL(it->second, 0); + // check if the expected ordering is obtained + for (auto node : nodes) { + EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]); + } + + // 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); + for (auto &edge : graph) { + EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6); } } From 90e7fb8229bd6693cac494627ced996b8462d1ec Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 8 Jul 2020 23:52:36 -0700 Subject: [PATCH 05/13] formatting headers and removing debugging code --- gtsam/sfm/mfas.cpp | 14 +------------- gtsam/sfm/tests/testMFAS.cpp | 6 ++---- 2 files changed, 3 insertions(+), 17 deletions(-) diff --git a/gtsam/sfm/mfas.cpp b/gtsam/sfm/mfas.cpp index 4ac1783e3..4764f2703 100644 --- a/gtsam/sfm/mfas.cpp +++ b/gtsam/sfm/mfas.cpp @@ -6,9 +6,8 @@ All rights reserved. */ -#include "mfas.h" +#include -#include #include #include #include @@ -54,16 +53,6 @@ void mfasRatio(const std::vector &edges, onbrs[i].push_back(pair(j, w)); } - for (auto &node : nodes) { - std::cout << node << " " << win_deg[node] << " " << wout_deg[node] - << std::endl; - for (auto it = inbrs[node].begin(); it != inbrs[node].end(); it++) - std::cout << it->first << "," << it->second << " "; - std::cout << std::endl; - for (auto it = onbrs[node].begin(); it != onbrs[node].end(); it++) - std::cout << it->first << "," << it->second << " "; - std::cout << std::endl; - } unsigned int ordered_count = 0; while (ordered_count < nodes.size()) { // choose an unchosen node @@ -84,7 +73,6 @@ void mfasRatio(const std::vector &edges, } } } - std::cout << "choice is " << choice << std::endl; // find its inbrs, adjust their wout_deg for (auto it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it) wout_deg[it->first] -= it->second; diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 83c03925b..d5daee1a6 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -1,9 +1,7 @@ +#include + #include -#include - -#include "gtsam/sfm/mfas.h" - using namespace std; using namespace gtsam; From c1c8b0a1a327892692e2545d8adfda9b3f9b99b8 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 20 Jul 2020 23:32:28 -0700 Subject: [PATCH 06/13] 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); } From 0eec6fbeec7e1568b564dbfcd2cffa6c0a001a7d Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 20 Jul 2020 23:45:45 -0700 Subject: [PATCH 07/13] minor comments change --- gtsam/sfm/MFAS.cpp | 2 +- gtsam/sfm/MFAS.h | 17 +++++++++-------- gtsam/sfm/tests/testMFAS.cpp | 6 ++++-- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 76b7f532a..8c3e3596a 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -120,7 +120,7 @@ std::map MFAS::computeOutlierWeights() { 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. + // 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); diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 20213a168..10052548b 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -25,7 +25,7 @@ namespace gtsam { 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", @@ -34,12 +34,13 @@ using TranslationEdges = std::map; 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 transation directions in 3D and the direction in + * 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 @@ -49,8 +50,8 @@ class MFAS { const std::shared_ptr &relativeTranslations, const Unit3 &projectionDirection); - /* - * Construct from the nodes in a graph and weighted directed edges + /** + * @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. @@ -62,7 +63,7 @@ class MFAS { 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 @@ -70,8 +71,8 @@ class MFAS { */ std::map computeOutlierWeights(); - /* - * Computes the 1D MFAS ordering of nodes in the graph + /** + * @brief Computes the 1D MFAS ordering of nodes in the graph * @return orderedNodes: vector of nodes in the obtained order */ std::vector computeOrdering(); diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 8b0192ade..345b3a5d4 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -5,12 +5,14 @@ using namespace std; using namespace gtsam; -/* We (partially) use the example from the paper on 1dsfm +/** + * 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. */ + * 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), From e7214a7777e31509ff2ddfc7e393204a28be230a Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 23 Jul 2020 00:06:31 -0700 Subject: [PATCH 08/13] constructor API change after review1 --- gtsam/sfm/MFAS.cpp | 140 +++++++++++++++-------------------- gtsam/sfm/MFAS.h | 92 +++++++++-------------- gtsam/sfm/tests/testMFAS.cpp | 10 +-- 3 files changed, 101 insertions(+), 141 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 8c3e3596a..e3288387e 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -5,76 +5,53 @@ using std::pair; using std::vector; MFAS::MFAS(const std::shared_ptr> &nodes, - const std::shared_ptr &relativeTranslations, + const TranslationEdges& 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; + : nodes_(nodes) { + // 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); } } -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() { +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 ordered_nodes; // nodes in MFAS order (result) + FastMap ordered_positions; // map from node to its position in the output order + // populate neighbors and weights - for (auto it = positiveEdgeWeights_.begin(); it != positiveEdgeWeights_.end(); it++) { + for (auto it = edgeWeights_.begin(); it != edgeWeights_.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); + 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] += weight; + out_weights[edge_source] += weight; + in_neighbors[edge_dest].push_back(edge_source); + out_neighbors[edge_source].push_back(edge_dest); } // in each iteration, one node is appended to the ordered list - while (orderedNodes_.size() < nodes_->size()) { + 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 (orderedPositions_.find(node) == orderedPositions_.end()) { - // is this a source + // 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 { @@ -86,53 +63,54 @@ std::vector MFAS::computeOrdering() { } } } - // find its inbrs, adjust their wout_deg + // find its in_neighbors, adjust their out_weights 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 + // 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] -= positiveEdgeWeights_[KeyPair(choice, *it)]; + in_weights[*it] -= edgeWeights_.find(KeyPair(choice, *it)) == edgeWeights_.end() ? -edgeWeights_.at(KeyPair(*it, choice)) : edgeWeights_.at(KeyPair(choice, *it)); - orderedPositions_[choice] = orderedNodes_.size(); - orderedNodes_.push_back(choice); + ordered_positions[choice] = ordered_nodes.size(); + ordered_nodes.push_back(choice); } - return orderedNodes_; + return ordered_nodes; } -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(); +std::map MFAS::computeOutlierWeights() const { + vector ordered_nodes = computeOrdering(); + FastMap ordered_positions; + std::map outlier_weights; + + // 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; } - 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); + // 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; } // 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]); + if (ordered_positions.at(edge_dest) < ordered_positions.at(edge_source)) { + outlier_weights[it->first] = std::abs(edgeWeights_.at(it->first)); } else { - outlierWeights_[it->first] = 0; + outlier_weights[it->first] = 0; } } - return outlierWeights_; + return outlier_weights; } \ No newline at end of file diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 10052548b..c8ea4f45d 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -9,8 +9,7 @@ * -------------------------------------------------------------------------- */ -#ifndef __MFAS_H__ -#define __MFAS_H__ +#pragma once #include #include @@ -21,10 +20,6 @@ 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: @@ -32,77 +27,64 @@ using TranslationEdges = std::map; 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 + problem is to obtain a directed acyclic graph by removing edges such that the total weight of removed edges is minimum. @addtogroup SFM */ class MFAS { + public: + // used to represent edges between two nodes in the graph + using KeyPair = std::pair; + using TranslationEdges = std::map; + 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 (points in 3D), edges - * that are translation directions in 3D and the direction in - * which edges are to be projected. + * @brief Construct from the nodes in a graph and weighted directed edges + * between the graph. A shared pointer to the nodes is used as input parameter. + * This is because, MFAS ordering is usually used to compute the ordering of a + * large graph that is already stored in memory. It is unnecessary to copy the + * set of nodes in this class. + * @param nodes: Nodes in the graph + * @param edgeWeights: weights of edges in the graph (map from pair of keys + * to signed double) + */ + MFAS(const std::shared_ptr> &nodes, + const std::map &edgeWeights) : + nodes_(nodes), edgeWeights_(edgeWeights) {} + + /** + * @brief Constructor for using 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 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 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> &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 + * of a edge to be zero if the edge is an inlier and the magnitude of its edgeWeight + * if it is an outlier. * @return outlierWeights: map from an edge to its outlier weight. */ - std::map computeOutlierWeights(); + std::map computeOutlierWeights() const; /** * @brief 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_; + std::vector computeOrdering() const; }; } // namespace gtsam - -#endif // __MFAS_H__ diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 345b3a5d4..8819493be 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -15,7 +15,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 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 nodes = {Key(0), Key(1), Key(2), Key(3)}; @@ -26,9 +26,9 @@ 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, +std::map getEdgeWeights(const vector &graph, const vector &weights) { - std::map edgeWeights; + std::map edgeWeights; for (size_t i = 0; i < graph.size(); i++) { edgeWeights[graph[i]] = weights[i]; } @@ -51,7 +51,7 @@ TEST(MFAS, OrderingWeights2) { EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); } - map outlier_weights = mfas_obj.computeOutlierWeights(); + 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 @@ -81,7 +81,7 @@ TEST(MFAS, OrderingWeights1) { EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); } - map outlier_weights = mfas_obj.computeOutlierWeights(); + 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 From b25809d5a3d373a4f43d40174e988d3d0e51363a Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 25 Jul 2020 13:39:58 -0700 Subject: [PATCH 09/13] changes after review - removing positiveEdgeWeights --- gtsam/sfm/MFAS.cpp | 17 +++++++++-- gtsam/sfm/MFAS.h | 55 ++++++++++++++++++++++++------------ gtsam/sfm/tests/testMFAS.cpp | 7 +++++ 3 files changed, 59 insertions(+), 20 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index e3288387e..b784e7efe 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -1,3 +1,10 @@ +/** + * @file MFAS.cpp + * @brief Source file for the MFAS class + * @author Akshay Krishnan + * @date July 2020 + */ + #include using namespace gtsam; @@ -26,14 +33,20 @@ std::vector MFAS::computeOrdering() const { FastMap ordered_positions; // map from node to its position in the output order // 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] += weight; - out_weights[edge_source] += weight; + 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); } diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index c8ea4f45d..c910e5621 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -11,6 +11,13 @@ #pragma once +/** + * @file MFAS.h + * @brief MFAS class to solve Minimum Feedback Arc Set graph problem + * @author Akshay Krishnan + * @date July 2020 + */ + #include #include @@ -29,13 +36,24 @@ namespace gtsam { 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 + // 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; + private: // pointer to nodes in the graph const std::shared_ptr> nodes_; @@ -47,44 +65,45 @@ class MFAS { public: /** * @brief Construct from the nodes in a graph and weighted directed edges - * between the graph. A shared pointer to the nodes is used as input parameter. - * This is because, MFAS ordering is usually used to compute the ordering of a - * large graph that is already stored in memory. It is unnecessary to copy the - * set of nodes in this class. + * 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. * @param nodes: Nodes in the graph - * @param edgeWeights: weights of edges in the graph (map from pair of keys - * to signed double) + * @param edgeWeights: weights of edges in the graph */ MFAS(const std::shared_ptr> &nodes, const std::map &edgeWeights) : nodes_(nodes), edgeWeights_(edgeWeights) {} /** - * @brief Constructor for using in the context of translation averaging. Here, + * @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 Nodes in the graph - * @param relativeTranslations translation directions between nodes + * @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 "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. - * @return outlierWeights: map from an edge to its outlier weight. - */ - std::map computeOutlierWeights() const; - /** * @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 index 8819493be..923d75a45 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -1,3 +1,10 @@ +/** + * @file testMFAS.cpp + * @brief Unit tests for the MFAS class + * @author Akshay Krishnan + * @date July 2020 + */ + #include #include #include From 43af7c4ae471d6f75b4c551e617c4f823c5ff322 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 12 Sep 2020 13:31:45 -0700 Subject: [PATCH 10/13] code changes to increase modularity --- gtsam/sfm/MFAS.cpp | 231 ++++++++++++++++++++--------------- gtsam/sfm/MFAS.h | 60 ++++----- gtsam/sfm/tests/testMFAS.cpp | 18 +-- 3 files changed, 175 insertions(+), 134 deletions(-) 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); } } From 0d41941cda67c3907e6dd7dca31feada4f6af8a3 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 12 Sep 2020 14:22:00 -0700 Subject: [PATCH 11/13] fixes1 --- gtsam/sfm/MFAS.cpp | 59 +++++++++++++++++++++++++--------------------- 1 file changed, 32 insertions(+), 27 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index dff8b6fa5..a48482768 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -11,20 +11,21 @@ #include #include #include +#include using namespace gtsam; -using MFAS::KeyPair; 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. - vector inNeighbors; // Nodes from which there is an incoming edge. - vector outNeighbors; // Nodes to which there is an outgoing edge. + 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() { return (outWeightSum + 1) / (inWeightSum + 1); } @@ -33,24 +34,27 @@ struct GraphNode { // 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) { + const map& edgeWeights) { unordered_map graph; - for (const auto& [edge, weight] : edgeWeights) { + 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. + MFAS::KeyPair& edge = edgeWeight.first; + 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.push_back(edgeSource); + graph[edgeDest].inNeighbors.insert(edgeSource); // Update the out weight and neighbors for the source. graph[edgeSource].outWeightSum += std::abs(weight); - graph[edgeSource].outNeighbors.push_back(edgeDest); + graph[edgeSource].outNeighbors.insert(edgeDest); } return graph; } @@ -58,36 +62,37 @@ unordered_map graphFromEdges( // 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) { + for (const auto& keyNode : graph) { // It is a root node if the inWeightSum is close to zero. - if (node.inWeightSum < 1e-8) { + 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 key; + 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& node1, - const std::pair& node2) { - return node1.second.heuristic() < - node2.second.heuristic(); + [](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 node1, const Key node2, - const map& edgeWeights) { + 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))); + return edgeWeights_.find(MFAS::KeyPair(node1, node2)) != edgeWeights_.end() + ? std::abs(edgeWeights_.at(MFAS::KeyPair(node1, node2))) + : std::abs(edgeWeights_.at(MFAS::KeyPair(node2, node1))); } // Removes a node from the graph and updates edge weights of its neighbors. -void removeNodeFromGraph(const Key node, const map edgeWeights, +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) { @@ -95,12 +100,12 @@ void removeNodeFromGraph(const Key node, const map edgeWeights, // (choice, *it) with a negative weight graph[neighbor].outWeightSum -= absWeightOfEdge(node, neighbor, edgeWeights); - graph[neighbor].outNeighbors.erase(graph[neighbor].outNeighbors.find(node)); + 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(graph[neighbor].inNeighbors.find(node)); + graph[neighbor].inNeighbors.erase(node); } // Erase node. graph.erase(node); @@ -148,10 +153,10 @@ std::map MFAS::computeOutlierWeights() const { map outlierWeights; // Check if the direction of each edge is consistent with the ordering. - for (const auto& [edge, weight] : edgeWeights_) { + for (const auto& edgeWeight : edgeWeights_) { // Find edge source and destination. - Key source = edge.first; - Key dest = edge.second; + Key source = edgeWeight.first.first; + Key dest = edgeWeight.first.second; if (weight < 0) { std::swap(source, dest); } @@ -159,7 +164,7 @@ std::map MFAS::computeOutlierWeights() const { // 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); + outlierWeights[edge] = std::abs(edgeWeight.second); } else { outlierWeights[edge] = 0; } From 539ebb032a0888596d7a0048d817651e7ecd5570 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 12 Sep 2020 14:28:47 -0700 Subject: [PATCH 12/13] fixes2 --- gtsam/sfm/MFAS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index a48482768..d4b7ad0a7 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -28,7 +28,7 @@ struct GraphNode { unordered_set 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); } + double heuristic() const { return (outWeightSum + 1) / (inWeightSum + 1); } }; // A graph is a map from key to GraphNode. This function returns the graph from @@ -42,8 +42,8 @@ unordered_map graphFromEdges( // 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. - MFAS::KeyPair& edge = edgeWeight.first; - double& weight = edgeWeight.second; + 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; @@ -140,7 +140,7 @@ vector MFAS::computeOrdering() const { return ordering; } -std::map MFAS::computeOutlierWeights() const { +map MFAS::computeOutlierWeights() const { // Find the ordering. vector ordering = computeOrdering(); From 0ab7f4c7c881f41e9e906ddcf7d544978ccf506b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 12 Sep 2020 21:40:11 +0000 Subject: [PATCH 13/13] fixes3 --- gtsam/sfm/MFAS.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index d4b7ad0a7..6dccc2dee 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -82,12 +82,12 @@ Key selectNextNodeInOrdering(const unordered_map& graph) { } // Returns the absolute weight of the edge between node1 and node2. -double absWeightOfEdge(const Key node1, const Key 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(node1, node2)) != edgeWeights_.end() - ? std::abs(edgeWeights_.at(MFAS::KeyPair(node1, node2))) - : std::abs(edgeWeights_.at(MFAS::KeyPair(node2, node1))); + 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. @@ -157,17 +157,17 @@ map MFAS::computeOutlierWeights() const { // Find edge source and destination. Key source = edgeWeight.first.first; Key dest = edgeWeight.first.second; - if (weight < 0) { + 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[edge] = std::abs(edgeWeight.second); + outlierWeights[edgeWeight.first] = std::abs(edgeWeight.second); } else { - outlierWeights[edge] = 0; + outlierWeights[edgeWeight.first] = 0; } } return outlierWeights; -} \ No newline at end of file +}