From e7214a7777e31509ff2ddfc7e393204a28be230a Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 23 Jul 2020 00:06:31 -0700 Subject: [PATCH] constructor API change after review1 --- gtsam/sfm/MFAS.cpp | 140 +++++++++++++++-------------------- gtsam/sfm/MFAS.h | 92 +++++++++-------------- gtsam/sfm/tests/testMFAS.cpp | 10 +-- 3 files changed, 101 insertions(+), 141 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 8c3e3596a..e3288387e 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -5,76 +5,53 @@ using std::pair; using std::vector; MFAS::MFAS(const std::shared_ptr> &nodes, - const std::shared_ptr &relativeTranslations, + const TranslationEdges& relativeTranslations, const Unit3 &projection_direction) - : nodes_(nodes), relativeTranslations_(relativeTranslations), - relativeTranslationsForWeights_(std::make_shared()) { - // 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; + : nodes_(nodes) { + // iterate over edges, obtain weights by projecting + // their relativeTranslations along the projection direction + for (auto it = relativeTranslations.begin(); + it != relativeTranslations.end(); it++) { + edgeWeights_[it->first] = it->second.dot(projection_direction); } } -MFAS::MFAS(const std::shared_ptr> &nodes, - const std::map &edgeWeights) : nodes_(nodes), - relativeTranslations_(std::make_shared()), - 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 MFAS::computeOrdering() { +std::vector MFAS::computeOrdering() const { FastMap in_weights; // sum on weights of incoming edges for a node FastMap out_weights; // sum on weights of outgoing edges for a node FastMap > in_neighbors; FastMap > out_neighbors; + vector ordered_nodes; // nodes in MFAS order (result) + FastMap ordered_positions; // map from node to its position in the output order + // 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; - 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); + const double weight = it->second; + Key edge_source = weight >= 0 ? edge.first : edge.second; + Key edge_dest = weight >= 0 ? edge.second : edge.first; + + 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 - while (orderedNodes_.size() < nodes_->size()) { + while (ordered_nodes.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 this node has not been chosen so far + if (ordered_positions.find(node) == ordered_positions.end()) { + // is this a root node 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; break; } else { @@ -86,53 +63,54 @@ std::vector MFAS::computeOrdering() { } } } - // find its inbrs, adjust their wout_deg + // find its in_neighbors, adjust their out_weights 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 + // the edge could be either (*it, choice) with a positive weight or (choice, *it) with a negative weight + 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(); 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(); - orderedNodes_.push_back(choice); + ordered_positions[choice] = ordered_nodes.size(); + ordered_nodes.push_back(choice); } - return orderedNodes_; + return ordered_nodes; } -std::map 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(); +std::map MFAS::computeOutlierWeights() const { + vector ordered_nodes = computeOrdering(); + FastMap ordered_positions; + std::map outlier_weights; + + // create a map becuase it is much faster to lookup the position of each node + // TODO(akshay-krishnan) this is already computed in computeOrdering. Would be nice if + // we could re-use. Either use an optional argument or change the output of + // computeOrdering + for(unsigned int i = 0; i < ordered_nodes.size(); i++) { + ordered_positions[ordered_nodes[i]] = i; } - for (auto it = start; it != end; it++) { - // relativeTranslations may have negative weight edges, we make sure all edges - // are along the positive direction by flipping them if they are not. - KeyPair edge = it->first; - if (positiveEdgeWeights_.find(edge) == positiveEdgeWeights_.end()) { - std::swap(edge.first, edge.second); + // iterate over all edges + for (auto it = edgeWeights_.begin(); it != edgeWeights_.end(); it++) { + Key edge_source, edge_dest; + if(it->second > 0) { + edge_source = it->first.first; + 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 // 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]); + if (ordered_positions.at(edge_dest) < ordered_positions.at(edge_source)) { + outlier_weights[it->first] = std::abs(edgeWeights_.at(it->first)); } else { - outlierWeights_[it->first] = 0; + outlier_weights[it->first] = 0; } } - return outlierWeights_; + return outlier_weights; } \ No newline at end of file diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 10052548b..c8ea4f45d 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -9,8 +9,7 @@ * -------------------------------------------------------------------------- */ -#ifndef __MFAS_H__ -#define __MFAS_H__ +#pragma once #include #include @@ -21,10 +20,6 @@ namespace gtsam { -// used to represent edges between two nodes in the graph -using KeyPair = std::pair; -using TranslationEdges = std::map; - /** The MFAS class to solve a Minimum feedback arc set (MFAS) problem. We implement the solution from: @@ -32,77 +27,64 @@ using TranslationEdges = std::map; 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 + problem is to obtain a directed acyclic graph by removing edges such that the total weight of removed edges is minimum. @addtogroup SFM */ class MFAS { + public: + // used to represent edges between two nodes in the graph + using KeyPair = std::pair; + using TranslationEdges = std::map; + private: + // pointer to nodes in the graph + const std::shared_ptr> nodes_; + + // edges with a direction such that all weights are positive + // i.e, edges that originally had negative weights are flipped + std::map edgeWeights_; + public: /** - * @brief Construct from the nodes in a graph (points in 3D), edges - * that are translation directions in 3D and the direction in - * which edges are to be projected. + * @brief Construct from the nodes in a graph and weighted directed edges + * between the graph. A shared pointer to the nodes is used as input parameter. + * 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> &nodes, + const std::map &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 relativeTranslations translation directions between nodes * @param projectionDirection direction in which edges are to be projected */ MFAS(const std::shared_ptr> &nodes, - const std::shared_ptr &relativeTranslations, + const TranslationEdges& relativeTranslations, 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> &nodes, - const std::map &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 + * of a edge to be zero if the edge is an inlier and the magnitude of its edgeWeight + * if it is an outlier. * @return outlierWeights: map from an edge to its outlier weight. */ - std::map computeOutlierWeights(); + std::map computeOutlierWeights() const; /** * @brief Computes the 1D MFAS ordering of nodes in the graph * @return orderedNodes: vector of nodes in the obtained order */ - std::vector computeOrdering(); - - private: - // pointer to nodes in the graph - const std::shared_ptr> nodes_; - // pointer to translation edges (translation directions between two node points) - const std::shared_ptr 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 relativeTranslationsForWeights_; - - // edges with a direction such that all weights are positive - // i.e, edges that originally had negative weights are flipped - std::map positiveEdgeWeights_; - - // map from edges to their outlier weight - std::map outlierWeights_; - - // nodes arranged in the MFAS order - std::vector orderedNodes_; - - // map from nodes to their position in the MFAS order - // used to speed up computation (lookup) when computing outlierWeights_ - FastMap orderedPositions_; + std::vector computeOrdering() const; }; } // namespace gtsam - -#endif // __MFAS_H__ diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 345b3a5d4..8819493be 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -15,7 +15,7 @@ using namespace gtsam; */ // edges in the graph - last edge from node 3 to 0 is an outlier -vector graph = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), +vector 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 vector nodes = {Key(0), Key(1), Key(2), Key(3)}; @@ -26,9 +26,9 @@ vector 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 -std::map getEdgeWeights(const vector &graph, +std::map getEdgeWeights(const vector &graph, const vector &weights) { - std::map edgeWeights; + std::map edgeWeights; for (size_t i = 0; i < graph.size(); i++) { edgeWeights[graph[i]] = weights[i]; } @@ -51,7 +51,7 @@ TEST(MFAS, OrderingWeights2) { EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); } - map outlier_weights = mfas_obj.computeOutlierWeights(); + map 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 @@ -81,7 +81,7 @@ TEST(MFAS, OrderingWeights1) { EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); } - map outlier_weights = mfas_obj.computeOutlierWeights(); + map 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