Merge pull request #387 from borglab/feature/1dsfm_mfas
Minimum Feedback Arc Set (MFAS) for 1dsfmrelease/4.3a0
Varun Agrawal
GitHub
commit
9350dd71ae
|
|
@ -0,0 +1,173 @@
|
|||
/**
|
||||
* @file MFAS.cpp
|
||||
* @brief Source file for the MFAS class
|
||||
* @author Akshay Krishnan
|
||||
* @date July 2020
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/MFAS.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include <unordered_set>
|
||||
|
||||
using namespace gtsam;
|
||||
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
|
||||
unordered_set<Key> inNeighbors; // Nodes from which there is an incoming edge
|
||||
unordered_set<Key> outNeighbors; // Nodes to which there is an outgoing edge
|
||||
|
||||
// Heuristic for the node that is to select nodes in MFAS.
|
||||
double heuristic() const { 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<Key, GraphNode> graphFromEdges(
|
||||
const map<MFAS::KeyPair, double>& edgeWeights) {
|
||||
unordered_map<Key, GraphNode> graph;
|
||||
|
||||
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.
|
||||
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;
|
||||
|
||||
// Update the in weight and neighbors for the destination.
|
||||
graph[edgeDest].inWeightSum += std::abs(weight);
|
||||
graph[edgeDest].inNeighbors.insert(edgeSource);
|
||||
|
||||
// Update the out weight and neighbors for the source.
|
||||
graph[edgeSource].outWeightSum += std::abs(weight);
|
||||
graph[edgeSource].outNeighbors.insert(edgeDest);
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
|
||||
// Selects the next node in the ordering from the graph.
|
||||
Key selectNextNodeInOrdering(const unordered_map<Key, GraphNode>& graph) {
|
||||
// Find the root nodes in the graph.
|
||||
for (const auto& keyNode : graph) {
|
||||
// It is a root node if the inWeightSum is close to zero.
|
||||
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 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<Key, GraphNode>& keyNode1,
|
||||
const std::pair<Key, GraphNode>& keyNode2) {
|
||||
return keyNode1.second.heuristic() <
|
||||
keyNode2.second.heuristic();
|
||||
})
|
||||
->first;
|
||||
}
|
||||
|
||||
// Returns the absolute weight of the edge between node1 and node2.
|
||||
double absWeightOfEdge(const Key key1, const Key key2,
|
||||
const map<MFAS::KeyPair, double>& edgeWeights) {
|
||||
// Check the direction of the edge before returning.
|
||||
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.
|
||||
void removeNodeFromGraph(const Key node,
|
||||
const map<MFAS::KeyPair, double> edgeWeights,
|
||||
unordered_map<Key, GraphNode>& 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(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(node);
|
||||
}
|
||||
// Erase node.
|
||||
graph.erase(node);
|
||||
}
|
||||
|
||||
MFAS::MFAS(const std::shared_ptr<vector<Key>>& nodes,
|
||||
const TranslationEdges& relativeTranslations,
|
||||
const Unit3& projectionDirection)
|
||||
: nodes_(nodes) {
|
||||
// Iterate over edges, obtain weights by projecting
|
||||
// their relativeTranslations along the projection direction
|
||||
for (const auto& measurement : relativeTranslations) {
|
||||
edgeWeights_[std::make_pair(measurement.key1(), measurement.key2())] =
|
||||
measurement.measured().dot(projectionDirection);
|
||||
}
|
||||
}
|
||||
|
||||
vector<Key> MFAS::computeOrdering() const {
|
||||
vector<Key> ordering; // Nodes in MFAS order (result).
|
||||
|
||||
// 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<Key, GraphNode> graph = graphFromEdges(edgeWeights_);
|
||||
|
||||
// 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);
|
||||
}
|
||||
return ordering;
|
||||
}
|
||||
|
||||
map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
|
||||
// Find the ordering.
|
||||
vector<Key> ordering = computeOrdering();
|
||||
|
||||
// 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<Key, int> orderingPositions;
|
||||
for (size_t i = 0; i < ordering.size(); i++) {
|
||||
orderingPositions[ordering[i]] = i;
|
||||
}
|
||||
|
||||
map<KeyPair, double> outlierWeights;
|
||||
// Check if the direction of each edge is consistent with the ordering.
|
||||
for (const auto& edgeWeight : edgeWeights_) {
|
||||
// Find edge source and destination.
|
||||
Key source = edgeWeight.first.first;
|
||||
Key dest = edgeWeight.first.second;
|
||||
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[edgeWeight.first] = std::abs(edgeWeight.second);
|
||||
} else {
|
||||
outlierWeights[edgeWeight.first] = 0;
|
||||
}
|
||||
}
|
||||
return outlierWeights;
|
||||
}
|
||||
|
|
@ -0,0 +1,111 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file MFAS.h
|
||||
* @brief MFAS class to solve Minimum Feedback Arc Set graph problem
|
||||
* @author Akshay Krishnan
|
||||
* @date September 2020
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/sfm/BinaryMeasurement.h>
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
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",
|
||||
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.
|
||||
@addtogroup SFM
|
||||
*/
|
||||
class MFAS {
|
||||
public:
|
||||
// used to represent edges between two nodes in the graph. When used in
|
||||
// translation averaging for global SfM
|
||||
using KeyPair = std::pair<Key, Key>;
|
||||
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
|
||||
|
||||
private:
|
||||
// pointer to nodes in the graph
|
||||
const std::shared_ptr<std::vector<Key>> nodes_;
|
||||
|
||||
// edges with a direction such that all weights are positive
|
||||
// i.e, edges that originally had negative weights are flipped
|
||||
std::map<KeyPair, double> edgeWeights_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Construct from the nodes in a graph and weighted directed edges
|
||||
* 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 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<std::vector<Key>> &nodes,
|
||||
const std::map<KeyPair, double> &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)
|
||||
* @param relativeTranslations translation directions between the cameras
|
||||
* @param projectionDirection direction in which edges are to be projected
|
||||
*/
|
||||
MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
|
||||
const TranslationEdges &relativeTranslations,
|
||||
const Unit3 &projectionDirection);
|
||||
|
||||
/**
|
||||
* @brief Computes the 1D MFAS ordering of nodes in the graph
|
||||
* @return orderedNodes: vector of nodes in the obtained order
|
||||
*/
|
||||
std::vector<Key> 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<KeyPair, double> computeOutlierWeights() const;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
/**
|
||||
* @file testMFAS.cpp
|
||||
* @brief Unit tests for the MFAS class
|
||||
* @author Akshay Krishnan
|
||||
* @date July 2020
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/MFAS.h>
|
||||
#include <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
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 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.
|
||||
*/
|
||||
|
||||
// edges in the graph - last edge from node 3 to 0 is an outlier
|
||||
vector<MFAS::KeyPair> 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<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};
|
||||
|
||||
// helper function to obtain map from keypairs to weights from the
|
||||
// vector representations
|
||||
map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
|
||||
const vector<double> &weights) {
|
||||
map<MFAS::KeyPair, double> edgeWeights;
|
||||
for (size_t i = 0; i < edges.size(); i++) {
|
||||
edgeWeights[edges[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) {
|
||||
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights2));
|
||||
|
||||
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 (size_t i = 0; i < ordered_nodes.size(); i++) {
|
||||
EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
|
||||
}
|
||||
|
||||
map<MFAS::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 : 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);
|
||||
} else {
|
||||
EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// test the ordering function and the outlierWeights method using
|
||||
// weights1 (outlier edge is accepted when projected in a direction that
|
||||
// produces weights1)
|
||||
TEST(MFAS, OrderingWeights1) {
|
||||
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights1));
|
||||
|
||||
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 (size_t i = 0; i < ordered_nodes.size(); i++) {
|
||||
EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
|
||||
}
|
||||
|
||||
map<MFAS::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 : edges) {
|
||||
EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue