changing MFAS to oops and refactoring

release/4.3a0
akrishnan86 2020-07-20 23:32:28 -07:00
parent 90e7fb8229
commit c1c8b0a1a3
5 changed files with 282 additions and 234 deletions

138
gtsam/sfm/MFAS.cpp Normal file
View File

@ -0,0 +1,138 @@
#include <gtsam/sfm/MFAS.h>
using namespace gtsam;
using std::pair;
using std::vector;
MFAS::MFAS(const std::shared_ptr<vector<Key>> &nodes,
const std::shared_ptr<TranslationEdges> &relativeTranslations,
const Unit3 &projection_direction)
: nodes_(nodes), relativeTranslations_(relativeTranslations),
relativeTranslationsForWeights_(std::make_shared<TranslationEdges>()) {
// 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<std::vector<Key>> &nodes,
const std::map<KeyPair, double> &edgeWeights) : nodes_(nodes),
relativeTranslations_(std::make_shared<TranslationEdges>()),
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<Key> MFAS::computeOrdering() {
FastMap<Key, double> in_weights; // sum on weights of incoming edges for a node
FastMap<Key, double> out_weights; // sum on weights of outgoing edges for a node
FastMap<Key, vector<Key> > in_neighbors;
FastMap<Key, vector<Key> > 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<KeyPair, double> 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_;
}

107
gtsam/sfm/MFAS.h Normal file
View File

@ -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 <gtsam/geometry/Unit3.h>
#include <gtsam/inference/Key.h>
#include <map>
#include <memory>
#include <vector>
namespace gtsam {
// used to represent edges between two nodes in the graph
using KeyPair = std::pair<Key, Key>;
using TranslationEdges = std::map<KeyPair, Unit3>;
/*
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<std::vector<Key>> &nodes,
const std::shared_ptr<TranslationEdges> &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<std::vector<Key>> &nodes,
const std::map<KeyPair, double> &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<KeyPair, double> computeOutlierWeights();
/*
* Computes the 1D MFAS ordering of nodes in the graph
* @return orderedNodes: vector of nodes in the obtained order
*/
std::vector<Key> computeOrdering();
private:
// pointer to nodes in the graph
const std::shared_ptr<std::vector<Key>> nodes_;
// pointer to translation edges (translation directions between two node points)
const std::shared_ptr<TranslationEdges> 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<TranslationEdges> relativeTranslationsForWeights_;
// edges with a direction such that all weights are positive
// i.e, edges that originally had negative weights are flipped
std::map<KeyPair, double> positiveEdgeWeights_;
// map from edges to their outlier weight
std::map<KeyPair, double> outlierWeights_;
// nodes arranged in the MFAS order
std::vector<Key> orderedNodes_;
// map from nodes to their position in the MFAS order
// used to speed up computation (lookup) when computing outlierWeights_
FastMap<Key, int> orderedPositions_;
};
} // namespace gtsam
#endif // __MFAS_H__

View File

@ -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 <gtsam/sfm/mfas.h>
#include <map>
#include <set>
#include <vector>
using std::map;
using std::pair;
using std::set;
using std::vector;
namespace gtsam {
namespace mfas {
void flipNegEdges(vector<KeyPair> &edges, vector<double> &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<KeyPair> &edges,
const std::vector<double> &weights, const KeyVector &nodes,
FastMap<Key, int> &ordered_positions) {
// initialize data structures
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 (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<int, double>(i, w));
onbrs[i].push_back(pair<int, double>(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<KeyPair> &edges,
const std::vector<double> &weight,
const FastMap<Key, int> &ordered_positions,
FastMap<KeyPair, double> &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

View File

@ -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 <gtsam/inference/Key.h>
#include <map>
#include <vector>
namespace gtsam {
using KeyPair = std::pair<Key, Key>;
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<KeyPair> &edges, std::vector<double> &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<KeyPair> &edges,
const std::vector<double> &weights, const KeyVector &nodes,
FastMap<Key, int> &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<KeyPair> &edges,
const std::vector<double> &weight,
const FastMap<Key, int> &ordered_positions,
FastMap<KeyPair, double> &outlier_weights);
} // namespace mfas
} // namespace gtsam
#endif // __MFAS_H__

View File

@ -1,5 +1,5 @@
#include <gtsam/sfm/mfas.h>
#include <gtsam/sfm/MFAS.h>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
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<KeyPair> 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<Key> nodes = {Key(0), Key(1), Key(2), Key(3)};
// weights from projecting in direction-1 (bad direction, outlier accepted)
vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75};
// weights from projecting in direction-2 (good direction, outlier rejected)
vector<double> weights2 = {0.5, 0.75, -0.25, 0.75, 1, 0.5};
// Testing the flipNegEdges function for weights1
TEST(MFAS, FlipNegEdges) {
vector<KeyPair> graph_copy = graph;
vector<double> 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<KeyPair, double> getEdgeWeights(const vector<KeyPair> &graph,
const vector<double> &weights) {
std::map<KeyPair, double> 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<KeyPair> graph_copy = graph;
vector<double> weights2_positive = weights2;
mfas::flipNegEdges(graph_copy, weights2_positive);
FastMap<Key, int> ordered_positions;
// compute ordering from positive edge weights
mfas::mfasRatio(graph_copy, weights2_positive, nodes, ordered_positions);
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(graph, weights2));
// expected ordering in this example
FastMap<Key, int> gt_ordered_positions;
gt_ordered_positions[0] = 0;
gt_ordered_positions[1] = 1;
gt_ordered_positions[3] = 2;
gt_ordered_positions[2] = 3;
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
// ground truth (expected) ordering in this example
vector<Key> 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<KeyPair, double> outlier_weights;
mfas::outlierWeights(graph_copy, weights2_positive, gt_ordered_positions,
outlier_weights);
map<KeyPair, double> 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<KeyPair> graph_copy = graph;
vector<double> weights1_positive = weights1;
mfas::flipNegEdges(graph_copy, weights1_positive);
FastMap<Key, int> ordered_positions;
// compute ordering from positive edge weights
mfas::mfasRatio(graph_copy, weights1_positive, nodes, ordered_positions);
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(graph, weights1));
// expected "ground truth" ordering in this example
FastMap<Key, int> gt_ordered_positions;
gt_ordered_positions[3] = 0;
gt_ordered_positions[0] = 1;
gt_ordered_positions[1] = 2;
gt_ordered_positions[2] = 3;
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
// "ground truth" expected ordering in this example
vector<Key> 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<KeyPair, double> outlier_weights;
mfas::outlierWeights(graph, weights1_positive, gt_ordered_positions,
outlier_weights);
map<KeyPair, double> 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);
}