From 636178f3bd8a8b850cde23fadebe12e3970f7b4a Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 6 Jul 2020 00:43:25 -0700 Subject: [PATCH] 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__