From b4e58795b7b6d8b5dcbdda46c8308fad4bbbaea8 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 8 Jul 2020 23:38:19 -0700 Subject: [PATCH] tests and documentation --- gtsam/sfm/{mfas.cc => mfas.cpp} | 43 +++++++--- gtsam/sfm/mfas.h | 25 +++--- gtsam/sfm/tests/testMFAS.cpp | 135 ++++++++++++++++++++------------ 3 files changed, 133 insertions(+), 70 deletions(-) rename gtsam/sfm/{mfas.cc => mfas.cpp} (59%) diff --git a/gtsam/sfm/mfas.cc b/gtsam/sfm/mfas.cpp similarity index 59% rename from gtsam/sfm/mfas.cc rename to gtsam/sfm/mfas.cpp index f2244bdfc..4ac1783e3 100644 --- a/gtsam/sfm/mfas.cc +++ b/gtsam/sfm/mfas.cpp @@ -1,5 +1,14 @@ +/* + 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 "mfas.h" +#include #include #include #include @@ -14,7 +23,7 @@ namespace mfas { void flipNegEdges(vector &edges, vector &weights) { // now renumber the edges - for (int i = 0; i < edges.size(); ++i) { + 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; @@ -34,7 +43,7 @@ void mfasRatio(const std::vector &edges, FastMap > > onbrs; // stuff data structures - for (int ii = 0; ii < edges.size(); ++ii) { + for (unsigned int ii = 0; ii < edges.size(); ++ii) { int i = edges[ii].first; int j = edges[ii].second; double w = weights[ii]; @@ -45,13 +54,23 @@ void mfasRatio(const std::vector &edges, onbrs[i].push_back(pair(j, w)); } - int ordered_count = 0; + for (auto &node : nodes) { + std::cout << node << " " << win_deg[node] << " " << wout_deg[node] + << std::endl; + for (auto it = inbrs[node].begin(); it != inbrs[node].end(); it++) + std::cout << it->first << "," << it->second << " "; + std::cout << std::endl; + for (auto it = onbrs[node].begin(); it != onbrs[node].end(); it++) + std::cout << it->first << "," << it->second << " "; + std::cout << std::endl; + } + 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()) { + if (ordered_positions.find(node) == ordered_positions.end()) { // is this a source if (win_deg[node] < 1e-8) { choice = node; @@ -65,7 +84,7 @@ void mfasRatio(const std::vector &edges, } } } - + std::cout << "choice is " << choice << std::endl; // find its inbrs, adjust their wout_deg for (auto it = inbrs[choice].begin(); it != inbrs[choice].end(); ++it) wout_deg[it->first] -= it->second; @@ -77,16 +96,16 @@ void mfasRatio(const std::vector &edges, } } -void brokenWeights(const std::vector &edges, - const std::vector &weight, - const FastMap &ordered_positions, - FastMap &broken) { - // find the broken edges - for (int i = 0; i < edges.size(); ++i) { +void outlierWeights(const std::vector &edges, + const std::vector &weight, + const FastMap &ordered_positions, + FastMap &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) - broken[i] += weight[i] > 0 ? weight[i] : -weight[i]; + outlier_weights[edges[i]] += weight[i] > 0 ? weight[i] : -weight[i]; } } diff --git a/gtsam/sfm/mfas.h b/gtsam/sfm/mfas.h index 6aec492c4..c3d852b92 100644 --- a/gtsam/sfm/mfas.h +++ b/gtsam/sfm/mfas.h @@ -24,20 +24,24 @@ using KeyPair = std::pair; 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. + * 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 &edges, std::vector &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. + * 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) + * @param ordered_positions: map from node to position in the ordering (0 + * indexed) */ void mfasRatio(const std::vector &edges, const std::vector &weights, const KeyVector &nodes, @@ -48,12 +52,13 @@ void mfasRatio(const std::vector &edges, * @param edges in the graph * @param weights of the edges in the graph * @param ordered_positions: ordering (obtained from MFAS solution) - * @param broken: reference to a map from edges to their "broken weights" + * @param outlier_weights: reference to a map from edges to their "outlier + * weights" */ -void brokenWeights(const std::vector &edges, - const std::vector &weight, - const FastMap &ordered_positions, - FastMap &broken); +void outlierWeights(const std::vector &edges, + const std::vector &weight, + const FastMap &ordered_positions, + FastMap &outlier_weights); } // namespace mfas } // namespace gtsam diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 0c52928c1..83c03925b 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -1,84 +1,123 @@ -#include "gtsam/sfm/mfas.h" #include +#include + +#include "gtsam/sfm/mfas.h" + using namespace std; using namespace gtsam; -// example from the paper -Key k0(0), k1(1), k2(2), k3(3), k4(4); -KeyPair e3_2(k3, k2), e0_1(k0, k1), e4_2(k4, k2), - e3_1(k3, k1), e4_0(k4, k0), e1_2(k1, k2), - e0_2(k0, k2), out_e3_0(k3, k0); +/* 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 + * avoids an ambiguity in the ground truth ordering that arises due to + * insufficient edges in the geaph. */ -vector graph = {make_pair(3, 2), make_pair(0, 1), make_pair(4, 2), - make_pair(3, 1), make_pair(4, 0), +// 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), make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; -KeyVector nodes = {0, 1, 2, 3, 4}; -vector weights1 = {2, 1.5, -2, -0.5, -0.5, 0.25, 1, 0.75}; - +// nodes in the graph +KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)}; +// weights from projecting in direction-1 (bad direction, outlier accepted) +vector weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; +// weights from projecting in direction-2 (good direction, outlier rejected) +vector weights2 = {0.5, 0.75, -0.25, 0.75, 1, 0.5}; +// Testing the flipNegEdges function for weights1 TEST(MFAS, FlipNegEdges) { vector graph_copy = graph; - vector weights1_copy = weights1; - mfas::flipNegEdges(graph_copy, weights1_copy); + vector 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_copy.size(), weights1.size()); + EXPECT_LONGS_EQUAL(weights1_positive.size(), weights1.size()); - for (int i = 0; i < weights1.size(); i++) { + for (unsigned int i = 0; i < weights1.size(); i++) { if (weights1[i] < 0) { - EXPECT_DOUBLES_EQUAL(weights1_copy[i], -weights1[i], 1e-6); + // 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); + graph_copy[i].second == graph[i].first); } else { - EXPECT_DOUBLES_EQUAL(weights1_copy[i], weights1[i], 1e-6); + // 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); + graph_copy[i].second == graph[i].second); } } } -// TEST(MFAS, Ordering) { - -// } - -TEST(MFAS, OrderingWithoutRemoval) { +// 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 graph_copy = graph; - vector weights1_copy = weights1; - mfas::flipNegEdges(graph_copy, weights1_copy); + vector weights2_positive = weights2; + mfas::flipNegEdges(graph_copy, weights2_positive); FastMap ordered_positions; - mfas::mfasRatio(graph_copy, weights1_copy, nodes, ordered_positions); + // compute ordering from positive edge weights + mfas::mfasRatio(graph_copy, weights2_positive, nodes, ordered_positions); + // expected ordering in this example FastMap gt_ordered_positions; - gt_ordered_positions[4] = 0; - gt_ordered_positions[3] = 1; - gt_ordered_positions[0] = 2; - gt_ordered_positions[1] = 3; - gt_ordered_positions[2] = 4; + gt_ordered_positions[0] = 0; + gt_ordered_positions[1] = 1; + gt_ordered_positions[3] = 2; + gt_ordered_positions[2] = 3; - for(auto it = ordered_positions.begin(); it != ordered_positions.end(); ++it) - { - EXPECT_LONGS_EQUAL(gt_ordered_positions[it->first], it->second); + // check if the expected ordering is obtained + for (auto node : nodes) { + EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]); + } + + // testing the outlierWeights method + FastMap outlier_weights; + mfas::outlierWeights(graph_copy, weights2_positive, gt_ordered_positions, + outlier_weights); + // 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) { + 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(MFAS, BrokenWeights) { +// test the ordering function and the outlierWeights method using +// weights2 (outlier edge is accepted when projected in a direction that +// produces weights2) +TEST(MFAS, OrderingWeights1) { vector graph_copy = graph; - vector weights1_copy = weights1; - mfas::flipNegEdges(graph_copy, weights1_copy); + vector weights1_positive = weights1; + mfas::flipNegEdges(graph_copy, weights1_positive); + FastMap ordered_positions; + // compute ordering from positive edge weights + mfas::mfasRatio(graph_copy, weights1_positive, nodes, ordered_positions); + // expected "ground truth" ordering in this example FastMap gt_ordered_positions; - gt_ordered_positions[4] = 0; - gt_ordered_positions[3] = 1; - gt_ordered_positions[0] = 2; - gt_ordered_positions[1] = 3; - gt_ordered_positions[2] = 4; + gt_ordered_positions[3] = 0; + gt_ordered_positions[0] = 1; + gt_ordered_positions[1] = 2; + gt_ordered_positions[2] = 3; - FastMap broken_weights; - mfas::brokenWeights(graph, weights1_copy, gt_ordered_positions, - broken_weights); - for (auto it = broken_weights.begin(); it != broken_weights.end(); it++) { - EXPECT_LONGS_EQUAL(it->second, 0); + // check if the expected ordering is obtained + for (auto node : nodes) { + EXPECT_LONGS_EQUAL(gt_ordered_positions[node], ordered_positions[node]); + } + + // since all edges (including the outlier) are consistent with this ordering, + // all outlier_weights must be zero + FastMap outlier_weights; + mfas::outlierWeights(graph, weights1_positive, gt_ordered_positions, + outlier_weights); + for (auto &edge : graph) { + EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6); } }