12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

changing mfas to use gtsam keys

release/4.3a0
akrishnan86 2020-07-06 00:43:25 -07:00
parent ab047d6962
commit 636178f3bd
2 changed files with 57 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
gtsam/sfm/mfas.cc
View File

@ -9,35 +9,14 @@ using std::pair;
using std::set;
using std::vector;
void reindex_problem(vector<Edge> &edges, map<int, int> &reindexing_key) {
// get the unique set of notes
set<int> 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<int>::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<Edge> &edges, vector<double> &weights) {
void flipNegEdges(vector<KeyPair> &edges, vector<double> &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<Edge> &edges, vector<double> &weights) {
}
}
void mfas_ratio(const std::vector<Edge> &edges,
const std::vector<double> &weights, std::vector<int> &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<KeyPair> &edges,
const std::vector<double> &weights, const KeyVector &nodes,
FastMap<Key, int> &ordered_positions) {
// initialize data structures
vector<double> win_deg(n, 0.0);
vector<double> wout_deg(n, 0.0);
vector<bool> unchosen(n, 1);
vector<vector<pair<int, double> > > inbrs(n);
vector<vector<pair<int, double> > > onbrs(n);
FastMap<Key, double> win_deg;
FastMap<Key, double> wout_deg;
FastMap<Key, vector<pair<int, double> > > inbrs;
FastMap<Key, vector<pair<int, double> > > 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<Edge> &edges,
onbrs[i].push_back(pair<int, double>(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<pair<int, double> >::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<Edge> &edges,
void brokenWeights(const std::vector<KeyPair> &edges,
const std::vector<double> &weight,
const std::vector<int> &order, std::vector<double> &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<int> inv_perm(n, 0.0);
for (int i = 0; i < n; ++i) inv_perm[order[i]] = i;
const FastMap<Key, int> &ordered_positions,
FastMap<Key, double> &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

33
gtsam/sfm/mfas.h
View File

@ -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 <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <map>
#include <vector>
typedef std::pair<int, int> Edge;
void mfas_ratio(const std::vector<Edge> &edges,
const std::vector<double> &weight, std::vector<int> &order);
namespace gtsam {
void reindex_problem(std::vector<Edge> &edges,
std::map<int, int> &reindexing_key);
using KeyPair = std::pair<Key, Key>;
void flip_neg_edges(std::vector<Edge> &edges, std::vector<double> &weights);
namespace mfas {
void broken_weight(const std::vector<Edge> &edges,
void flipNegEdges(std::vector<KeyPair> &edges, std::vector<double> &weights);
void mfasRatio(const std::vector<KeyPair> &edges,
const std::vector<double> &weights, const KeyVector &nodes,
FastMap<Key, int> &ordered_positions);
void brokenWeights(const std::vector<KeyPair> &edges,
const std::vector<double> &weight,
const std::vector<int> &order, std::vector<double> &broken);
const FastMap<Key, int> &ordered_positions,
FastMap<Key, double> &broken);
} // namespace mfas
} // namespace gtsam
#endif // __MFAS_H__