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

constructor API change after review1

release/4.3a0
akrishnan86 2020-07-23 00:06:31 -07:00
parent 62ba551830
commit e7214a7777
3 changed files with 101 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

140
gtsam/sfm/MFAS.cpp
View File

@ -5,76 +5,53 @@ using std::pair;
using std::vector; using std::vector;
MFAS::MFAS(const std::shared_ptr<vector<Key>> &nodes, MFAS::MFAS(const std::shared_ptr<vector<Key>> &nodes,
const std::shared_ptr<TranslationEdges> &relativeTranslations, const TranslationEdges& relativeTranslations,
const Unit3 &projection_direction) const Unit3 &projection_direction)
: nodes_(nodes), relativeTranslations_(relativeTranslations), : nodes_(nodes) {
relativeTranslationsForWeights_(std::make_shared<TranslationEdges>()) { // iterate over edges, obtain weights by projecting
// iterate over edges and flip all edges that have negative weights, // their relativeTranslations along the projection direction
// while storing the magnitude of the weights. for (auto it = relativeTranslations.begin();
for (auto it = relativeTranslations->begin(); it != relativeTranslations.end(); it++) {
it != relativeTranslations->end(); it++) { edgeWeights_[it->first] = it->second.dot(projection_direction);
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, std::vector<Key> MFAS::computeOrdering() const {
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> 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, double> out_weights; // sum on weights of outgoing edges for a node
FastMap<Key, vector<Key> > in_neighbors; FastMap<Key, vector<Key> > in_neighbors;
FastMap<Key, vector<Key> > out_neighbors; FastMap<Key, vector<Key> > out_neighbors;
vector<Key> ordered_nodes; // nodes in MFAS order (result)
FastMap<Key, int> ordered_positions; // map from node to its position in the output order
// populate neighbors and weights // 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; const KeyPair &edge = it->first;
in_weights[edge.second] += it->second; const double weight = it->second;
out_weights[edge.first] += it->second; Key edge_source = weight >= 0 ? edge.first : edge.second;
in_neighbors[edge.second].push_back(edge.first); Key edge_dest = weight >= 0 ? edge.second : edge.first;
out_neighbors[edge.first].push_back(edge.second);
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 // 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 // finding the node with the max heuristic score
Key choice; Key choice;
double max_score = 0.0; double max_score = 0.0;
for (const Key &node : *nodes_) { for (const Key &node : *nodes_) {
if (orderedPositions_.find(node) == orderedPositions_.end()) { // if this node has not been chosen so far
// is this a source if (ordered_positions.find(node) == ordered_positions.end()) {
// is this a root node
if (in_weights[node] < 1e-8) { 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; choice = node;
break; break;
} else { } else {
@ -86,53 +63,54 @@ std::vector<Key> MFAS::computeOrdering() {
} }
} }
} }
// find its inbrs, adjust their wout_deg // find its in_neighbors, adjust their out_weights
for (auto it = in_neighbors[choice].begin(); for (auto it = in_neighbors[choice].begin();
it != in_neighbors[choice].end(); ++it) it != in_neighbors[choice].end(); ++it)
out_weights[*it] -= positiveEdgeWeights_[KeyPair(*it, choice)]; // the edge could be either (*it, choice) with a positive weight or (choice, *it) with a negative weight
// find its onbrs, adjust their win_deg 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(); for (auto it = out_neighbors[choice].begin();
it != out_neighbors[choice].end(); ++it) 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(); ordered_positions[choice] = ordered_nodes.size();
orderedNodes_.push_back(choice); ordered_nodes.push_back(choice);
} }
return orderedNodes_; return ordered_nodes;
} }
std::map<KeyPair, double> MFAS::computeOutlierWeights() { std::map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
// if ordering has not been computed yet vector<Key> ordered_nodes = computeOrdering();
if (orderedNodes_.size() != nodes_->size()) { FastMap<Key, int> ordered_positions;
computeOrdering(); std::map<KeyPair, double> outlier_weights;
}
// iterate over all edges // create a map becuase it is much faster to lookup the position of each node
// start and end iterators depend on whether we are using relativeTranslations_ or // TODO(akshay-krishnan) this is already computed in computeOrdering. Would be nice if
// relativeTranslationsForWeights_ to store the original edge directions // we could re-use. Either use an optional argument or change the output of
TranslationEdges::iterator start, end; // computeOrdering
if (relativeTranslationsForWeights_->size() == 0) { for(unsigned int i = 0; i < ordered_nodes.size(); i++) {
start = relativeTranslations_->begin(); ordered_positions[ordered_nodes[i]] = i;
end = relativeTranslations_->end();
} else {
start = relativeTranslationsForWeights_->begin();
end = relativeTranslationsForWeights_->end();
} }
for (auto it = start; it != end; it++) { // iterate over all edges
// relativeTranslations may have negative weight edges, we make sure all edges for (auto it = edgeWeights_.begin(); it != edgeWeights_.end(); it++) {
// are along the positive direction by flipping them if they are not. Key edge_source, edge_dest;
KeyPair edge = it->first; if(it->second > 0) {
if (positiveEdgeWeights_.find(edge) == positiveEdgeWeights_.end()) { edge_source = it->first.first;
std::swap(edge.first, edge.second); 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 // if the ordered position of nodes is not consistent with the edge
// direction for consistency second should be greater than first // direction for consistency second should be greater than first
if (orderedPositions_.at(edge.second) < orderedPositions_.at(edge.first)) { if (ordered_positions.at(edge_dest) < ordered_positions.at(edge_source)) {
outlierWeights_[it->first] = std::abs(positiveEdgeWeights_[edge]); outlier_weights[it->first] = std::abs(edgeWeights_.at(it->first));
} else { } else {
outlierWeights_[it->first] = 0; outlier_weights[it->first] = 0;
} }
} }
return outlierWeights_; return outlier_weights;
} }

92
gtsam/sfm/MFAS.h
View File

@ -9,8 +9,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
#ifndef __MFAS_H__ #pragma once
#define __MFAS_H__
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -21,10 +20,6 @@
namespace gtsam { 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) The MFAS class to solve a Minimum feedback arc set (MFAS)
problem. We implement the solution from: problem. We implement the solution from:
@ -32,77 +27,64 @@ using TranslationEdges = std::map<KeyPair, Unit3>;
Proceedings of the European Conference on Computer Vision, ECCV 2014 Proceedings of the European Conference on Computer Vision, ECCV 2014
Given a weighted directed graph, the objective in a Minimum feedback arc set 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. edges such that the total weight of removed edges is minimum.
@addtogroup SFM @addtogroup SFM
*/ */
class MFAS { class MFAS {
public:
// used to represent edges between two nodes in the graph
using KeyPair = std::pair<Key, Key>;
using TranslationEdges = std::map<KeyPair, 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: public:
/** /**
* @brief Construct from the nodes in a graph (points in 3D), edges * @brief Construct from the nodes in a graph and weighted directed edges
* that are translation directions in 3D and the direction in * between the graph. A shared pointer to the nodes is used as input parameter.
* which edges are to be projected. * 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<std::vector<Key>> &nodes,
const std::map<KeyPair, double> &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 nodes Nodes in the graph
* @param relativeTranslations translation directions between nodes * @param relativeTranslations translation directions between nodes
* @param projectionDirection direction in which edges are to be projected * @param projectionDirection direction in which edges are to be projected
*/ */
MFAS(const std::shared_ptr<std::vector<Key>> &nodes, MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
const std::shared_ptr<TranslationEdges> &relativeTranslations, const TranslationEdges& relativeTranslations,
const Unit3 &projectionDirection); 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<std::vector<Key>> &nodes,
const std::map<KeyPair, double> &edgeWeights);
/** /**
* @brief Computes the "outlier weights" of the graph. We define the outlier weight * @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 * 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 can only be used when constructing the * if it is an outlier.
* @return outlierWeights: map from an edge to its outlier weight. * @return outlierWeights: map from an edge to its outlier weight.
*/ */
std::map<KeyPair, double> computeOutlierWeights(); std::map<KeyPair, double> computeOutlierWeights() const;
/** /**
* @brief 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 * @return orderedNodes: vector of nodes in the obtained order
*/ */
std::vector<Key> computeOrdering(); std::vector<Key> computeOrdering() const;
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 } // namespace gtsam
#endif // __MFAS_H__

10
gtsam/sfm/tests/testMFAS.cpp
View File

@ -15,7 +15,7 @@ using namespace gtsam;
*/ */
// edges in the graph - last edge from node 3 to 0 is an outlier // 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), vector<MFAS::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)}; make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)};
// nodes in the graph // nodes in the graph
vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)}; vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)};
@ -26,9 +26,9 @@ 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 // helper function to obtain map from keypairs to weights from the
// vector representations // vector representations
std::map<KeyPair, double> getEdgeWeights(const vector<KeyPair> &graph, std::map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &graph,
const vector<double> &weights) { const vector<double> &weights) {
std::map<KeyPair, double> edgeWeights; std::map<MFAS::KeyPair, double> edgeWeights;
for (size_t i = 0; i < graph.size(); i++) { for (size_t i = 0; i < graph.size(); i++) {
edgeWeights[graph[i]] = weights[i]; edgeWeights[graph[i]] = weights[i];
} }
@ -51,7 +51,7 @@ TEST(MFAS, OrderingWeights2) {
EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
} }
map<KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights(); map<MFAS::KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights();
// since edge between 3 and 0 is inconsistent with the ordering, it must have // since edge between 3 and 0 is inconsistent with the ordering, it must have
// positive outlier weight, other outlier weights must be zero // 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]); EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]);
} }
map<KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights(); map<MFAS::KeyPair, double> outlier_weights = mfas_obj.computeOutlierWeights();
// since edge between 3 and 0 is inconsistent with the ordering, it must have // since edge between 3 and 0 is inconsistent with the ordering, it must have
// positive outlier weight, other outlier weights must be zero // positive outlier weight, other outlier weights must be zero