From 1ed651b1a2bc537d5c7f249f8d30bdd1e42ff8e5 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 18 Sep 2020 23:14:07 -0700 Subject: [PATCH 01/12] wrap MFAS --- gtsam/gtsam.i | 21 +++++++++++++++++++++ gtsam/sfm/MFAS.h | 2 ++ python/CMakeLists.txt | 7 +++++-- python/gtsam/specializations.h | 1 + 4 files changed, 29 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..649d80ae3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3105,6 +3105,27 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include + +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(pair) const; +}; + +class MFAS { + MFAS(const KeyVector*& nodes, + const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); + + KeyPairDoubleMap computeOutlierWeights() const; + KeyVector computeOrdering() const; +}; + #include class TranslationRecovery { TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 929aa5ff0..67a7df219 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -108,4 +108,6 @@ class MFAS { std::map computeOutlierWeights() const; }; +typedef std::map, double> KeyPairDoubleMap; + } // namespace gtsam diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bec02fb64..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -37,7 +37,8 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector - gtsam::BinaryMeasurementsUnit3) + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_py # target ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header @@ -80,7 +81,9 @@ set(ignore gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue - gtsam::BinaryMeasurementsUnit3) + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) + pybind_wrap(gtsam_unstable_py # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3f6b8fa38..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -12,3 +12,4 @@ py::bind_vector py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); +py::bind_map(m_, "KeyPairDoubleMap"); From 6c14605ed0860f648ef73321a434033d98ed0796 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 19 Sep 2020 08:36:49 +0000 Subject: [PATCH 02/12] changing to boost shared_ptr --- gtsam/gtsam.i | 8 ++++---- gtsam/sfm/MFAS.cpp | 4 +++- gtsam/sfm/MFAS.h | 8 +++++--- gtsam/sfm/tests/testMFAS.cpp | 10 +++++++--- 4 files changed, 19 insertions(+), 11 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 649d80ae3..df486e19e 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3114,16 +3114,16 @@ class KeyPairDoubleMap { size_t size() const; bool empty() const; void clear(); - size_t at(pair) const; + size_t at(const pair& keypair) const; }; class MFAS { - MFAS(const KeyVector*& nodes, + MFAS(const gtsam::KeyVector* nodes, const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::Unit3& projectionDirection); - KeyPairDoubleMap computeOutlierWeights() const; - KeyVector computeOrdering() const; + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; }; #include diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 6dccc2dee..0a407785b 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -7,6 +7,8 @@ #include +#include + #include #include #include @@ -111,7 +113,7 @@ void removeNodeFromGraph(const Key node, graph.erase(node); } -MFAS::MFAS(const std::shared_ptr>& nodes, +MFAS::MFAS(const boost::shared_ptr> nodes, const TranslationEdges& relativeTranslations, const Unit3& projectionDirection) : nodes_(nodes) { diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 67a7df219..ca85d3248 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -22,6 +22,8 @@ #include #include +#include + #include #include #include @@ -57,7 +59,7 @@ class MFAS { private: // pointer to nodes in the graph - const std::shared_ptr> nodes_; + const boost::shared_ptr> nodes_; // edges with a direction such that all weights are positive // i.e, edges that originally had negative weights are flipped @@ -74,7 +76,7 @@ class MFAS { * @param nodes: Nodes in the graph * @param edgeWeights: weights of edges in the graph */ - MFAS(const std::shared_ptr> &nodes, + MFAS(const boost::shared_ptr> nodes, const std::map &edgeWeights) : nodes_(nodes), edgeWeights_(edgeWeights) {} @@ -88,7 +90,7 @@ class MFAS { * @param relativeTranslations translation directions between the cameras * @param projectionDirection direction in which edges are to be projected */ - MFAS(const std::shared_ptr> &nodes, + MFAS(const boost::shared_ptr> nodes, const TranslationEdges &relativeTranslations, const Unit3 &projectionDirection); diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 58ea4cc84..53526cce1 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -6,9 +6,13 @@ */ #include -#include + +#include +#include #include +#include + using namespace std; using namespace gtsam; @@ -46,7 +50,7 @@ map getEdgeWeights(const vector &edges, // 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>(nodes), getEdgeWeights(edges, weights2)); + MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights2)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -76,7 +80,7 @@ TEST(MFAS, OrderingWeights2) { // weights1 (outlier edge is accepted when projected in a direction that // produces weights1) TEST(MFAS, OrderingWeights1) { - MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights1)); + MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights1)); vector ordered_nodes = mfas_obj.computeOrdering(); From 1f5c6b8b4b712250e5e7261ae7ba68273d734741 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 20 Sep 2020 20:33:37 +0000 Subject: [PATCH 03/12] remove unusede ptr member in MFAS --- gtsam/gtsam.i | 4 ++-- gtsam/sfm/MFAS.cpp | 6 ++---- gtsam/sfm/MFAS.h | 24 +++++------------------- gtsam/sfm/tests/testMFAS.cpp | 7 ++----- 4 files changed, 11 insertions(+), 30 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index df486e19e..3a7bebaba 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2972,6 +2972,7 @@ class BinaryMeasurement { size_t key1() const; size_t key2() const; T measured() const; + gtsam::noiseModel::Base* noiseModel() const; }; typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; @@ -3118,8 +3119,7 @@ class KeyPairDoubleMap { }; class MFAS { - MFAS(const gtsam::KeyVector* nodes, - const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::Unit3& projectionDirection); gtsam::KeyPairDoubleMap computeOutlierWeights() const; diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 0a407785b..bc66d0711 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -113,10 +113,8 @@ void removeNodeFromGraph(const Key node, graph.erase(node); } -MFAS::MFAS(const boost::shared_ptr> nodes, - const TranslationEdges& relativeTranslations, - const Unit3& projectionDirection) - : nodes_(nodes) { +MFAS::MFAS(const TranslationEdges& relativeTranslations, + const Unit3& projectionDirection) { // Iterate over edges, obtain weights by projecting // their relativeTranslations along the projection direction for (const auto& measurement : relativeTranslations) { diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index ca85d3248..3b01122a9 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -22,8 +22,6 @@ #include #include -#include - #include #include #include @@ -58,40 +56,28 @@ class MFAS { using TranslationEdges = std::vector>; private: - // pointer to nodes in the graph - const boost::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 and weighted directed edges + * @brief Construct from the 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 boost::shared_ptr> nodes, - const std::map &edgeWeights) - : nodes_(nodes), edgeWeights_(edgeWeights) {} + MFAS(const std::map &edgeWeights) + : 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 boost::shared_ptr> nodes, - const TranslationEdges &relativeTranslations, + MFAS(const TranslationEdges &relativeTranslations, const Unit3 &projectionDirection); /** @@ -101,7 +87,7 @@ class MFAS { std::vector computeOrdering() const; /** - * @brief Computes the "outlier weights" of the graph. We define the outlier + * @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. diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 53526cce1..b2daf0d2e 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -7,8 +7,6 @@ #include -#include -#include #include #include @@ -43,14 +41,13 @@ map getEdgeWeights(const vector &edges, 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(boost::make_shared>(nodes), getEdgeWeights(edges, weights2)); + MFAS mfas_obj(getEdgeWeights(edges, weights2)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -80,7 +77,7 @@ TEST(MFAS, OrderingWeights2) { // weights1 (outlier edge is accepted when projected in a direction that // produces weights1) TEST(MFAS, OrderingWeights1) { - MFAS mfas_obj(boost::make_shared>(nodes), getEdgeWeights(edges, weights1)); + MFAS mfas_obj(getEdgeWeights(edges, weights1)); vector ordered_nodes = mfas_obj.computeOrdering(); From 565467f2ff48dcdd9a53f4e93c664ac7fa51b4b3 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 20 Sep 2020 20:34:10 +0000 Subject: [PATCH 04/12] translation averaging example --- .../examples/TranslationAveragingExample.py | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 python/gtsam/examples/TranslationAveragingExample.py diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py new file mode 100644 index 000000000..09d261d97 --- /dev/null +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -0,0 +1,93 @@ +from collections import Counter +import functools +import operator + +import numpy as np + +import gtsam +from gtsam.examples import SFMdata + +max_1dsfm_projection_directions = 50 +outlier_weight_threshold = 0.1 + +def get_data(): + """"Returns data from SfMData.createPoses(). This contains the global rotations and the unit translations directions.""" + # Using toy dataset in SfMdata for example. + poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + rotations = gtsam.Values() + translation_directions = [] + for i in range(0, len(poses) - 2): + # Add the rotation + rotations.insert(i, poses[i].rotation()) + # Create unit translation measurements with next two poses + for j in range(i+1, i+3): + i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(poses[j].translation() - poses[i].translation())) + translation_directions.append(gtsam.BinaryMeasurementUnit3( + i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + # Add the last two rotations. + rotations.insert(len(poses) - 1, poses[-1].rotation()) + rotations.insert(len(poses) - 2, poses[-2].rotation()) + return (rotations, translation_directions) + + +def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, + rotations: gtsam.Values): + """Estimate poses given normalized translation directions and rotations between nodes. + + Arguments: + measurements - List of translation direction from the first node to the second node in the coordinate frame of the first node. + rotations {Values} -- Estimated rotations + + Returns: + Values -- Estimated poses. + """ + + # Convert the translation directions to global frame using the rotations. + w_measurements = gtsam.BinaryMeasurementsUnit3() + for measurement in measurements: + w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2( + ), gtsam.Unit3(rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) + + # Indices of measurements that are to be used as projection directions. These are randomly chosen. + indices = np.random.choice(len(w_measurements), min( + max_1dsfm_projection_directions, len(w_measurements)), replace=False) + # Sample projection directions from the measurements. + projection_directions = [w_measurements[idx].measured() for idx in indices] + + outlier_weights = [] + # Find the outlier weights for each direction using MFAS. + for direction in projection_directions: + algorithm = gtsam.MFAS(w_measurements, direction) + outlier_weights.append(algorithm.computeOutlierWeights()) + + # Compute average of outlier weights. + avg_outlier_weights = {} + for outlier_weight_dict in outlier_weights: + for k, v in outlier_weight_dict.items(): + if k in avg_outlier_weights: + avg_outlier_weights[k] += v/len(outlier_weights) + else: + avg_outlier_weights[k] = v/len(outlier_weights) + + # Remove measurements that have weight greater than threshold. + inlier_measurements = gtsam.BinaryMeasurementsUnit3() + [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[(m.key1(), m.key2())] < outlier_weight_threshold] + + # Run the optimizer to obtain translations for normalized directions. + translations = gtsam.TranslationRecovery(inlier_measurements).run() + + poses = gtsam.Values() + for key in rotations.keys(): + poses.insert(key, gtsam.Pose3( + rotations.atRot3(key), translations.atPoint3(key))) + return poses + +def main(): + rotations, translation_directions = get_data() + poses = estimate_poses_given_rot(translation_directions, rotations) + print("**** Translation averaging output ****") + print(poses) + print("**************************************") + +if __name__ == '__main__': + main() From 4b06616dfedcd2c2b850cf865c4c8118e81681c3 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 21 Sep 2020 20:40:43 -0700 Subject: [PATCH 05/12] adding documentation for example --- .../examples/TranslationAveragingExample.py | 49 +++++++++++++------ 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 09d261d97..4e3c7467a 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -1,17 +1,27 @@ -from collections import Counter -import functools -import operator +""" +GTSAM Copyright 2010-2018, 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 + +This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery) +together for estimating global translations from relative translation directions and global rotations. +The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset. + +Author: Akshay Krishnan +Date: September 2020 +""" import numpy as np import gtsam from gtsam.examples import SFMdata -max_1dsfm_projection_directions = 50 -outlier_weight_threshold = 0.1 def get_data(): - """"Returns data from SfMData.createPoses(). This contains the global rotations and the unit translations directions.""" + """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" # Using toy dataset in SfMdata for example. poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) rotations = gtsam.Values() @@ -20,8 +30,9 @@ def get_data(): # Add the rotation rotations.insert(i, poses[i].rotation()) # Create unit translation measurements with next two poses - for j in range(i+1, i+3): - i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(poses[j].translation() - poses[i].translation())) + for j in range(i + 1, i + 3): + i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate( + poses[j].translation() - poses[i].translation())) translation_directions.append(gtsam.BinaryMeasurementUnit3( i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. @@ -35,25 +46,30 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, """Estimate poses given normalized translation directions and rotations between nodes. Arguments: - measurements - List of translation direction from the first node to the second node in the coordinate frame of the first node. + measurements {BinaryMeasurementsUnit3}- List of translation direction from the first node to + the second node in the coordinate frame of the first node. rotations {Values} -- Estimated rotations Returns: Values -- Estimated poses. """ + # Some hyperparameters. + max_1dsfm_projection_directions = 50 + outlier_weight_threshold = 0.1 + # Convert the translation directions to global frame using the rotations. w_measurements = gtsam.BinaryMeasurementsUnit3() for measurement in measurements: - w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2( - ), gtsam.Unit3(rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) + w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2(), gtsam.Unit3( + rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) # Indices of measurements that are to be used as projection directions. These are randomly chosen. indices = np.random.choice(len(w_measurements), min( max_1dsfm_projection_directions, len(w_measurements)), replace=False) # Sample projection directions from the measurements. projection_directions = [w_measurements[idx].measured() for idx in indices] - + outlier_weights = [] # Find the outlier weights for each direction using MFAS. for direction in projection_directions: @@ -65,13 +81,14 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, for outlier_weight_dict in outlier_weights: for k, v in outlier_weight_dict.items(): if k in avg_outlier_weights: - avg_outlier_weights[k] += v/len(outlier_weights) + avg_outlier_weights[k] += v / len(outlier_weights) else: - avg_outlier_weights[k] = v/len(outlier_weights) + avg_outlier_weights[k] = v / len(outlier_weights) # Remove measurements that have weight greater than threshold. inlier_measurements = gtsam.BinaryMeasurementsUnit3() - [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[(m.key1(), m.key2())] < outlier_weight_threshold] + [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[( + m.key1(), m.key2())] < outlier_weight_threshold] # Run the optimizer to obtain translations for normalized directions. translations = gtsam.TranslationRecovery(inlier_measurements).run() @@ -82,6 +99,7 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, rotations.atRot3(key), translations.atPoint3(key))) return poses + def main(): rotations, translation_directions = get_data() poses = estimate_poses_given_rot(translation_directions, rotations) @@ -89,5 +107,6 @@ def main(): print(poses) print("**************************************") + if __name__ == '__main__': main() From fbb26eea07ccc05c5496ea3ef019c6b024f4e626 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 24 Sep 2020 22:32:04 -0700 Subject: [PATCH 06/12] naming and other changes - review1 --- .../examples/TranslationAveragingExample.py | 78 +++++++++++-------- 1 file changed, 44 insertions(+), 34 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 4e3c7467a..c3dcf2ad2 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -14,22 +14,28 @@ Author: Akshay Krishnan Date: September 2020 """ +from collections import defaultdict +from typing import Tuple, List + import numpy as np import gtsam from gtsam.examples import SFMdata -def get_data(): +def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" # Using toy dataset in SfMdata for example. poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + # Rotations of the cameras in the world frame - wRc. rotations = gtsam.Values() + # Normalized translation directions for pairs of cameras - from first camera to second, + # in the coordinate frame of the first camera. translation_directions = [] for i in range(0, len(poses) - 2): - # Add the rotation + # Add the rotation. rotations.insert(i, poses[i].rotation()) - # Create unit translation measurements with next two poses + # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate( poses[j].translation() - poses[i].translation())) @@ -41,68 +47,72 @@ def get_data(): return (rotations, translation_directions) -def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3, - rotations: gtsam.Values): - """Estimate poses given normalized translation directions and rotations between nodes. +def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, + rotations: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations normalized translation directions between cameras. - Arguments: - measurements {BinaryMeasurementsUnit3}- List of translation direction from the first node to - the second node in the coordinate frame of the first node. - rotations {Values} -- Estimated rotations + Args: + relative_translations -- List of normalized translation directions between camera pairs, each direction + is from the first camera to the second, in the frame of the first camera. + rotations -- Rotations of the cameras in the world frame. Returns: Values -- Estimated poses. """ - # Some hyperparameters. - max_1dsfm_projection_directions = 50 + # Some hyperparameters, values used from 1dsfm. + max_1dsfm_projection_directions = 48 outlier_weight_threshold = 0.1 # Convert the translation directions to global frame using the rotations. - w_measurements = gtsam.BinaryMeasurementsUnit3() - for measurement in measurements: - w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2(), gtsam.Unit3( - rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) + w_relative_translations = gtsam.BinaryMeasurementsUnit3() + for relative_translation in relative_translations: + w_relative_translation = gtsam.Unit3(rotations.atRot3(relative_translation.key1()) + .rotate(relative_translation.measured().point3())) + w_relative_translations.append(gtsam.BinaryMeasurementUnit3(relative_translation.key1(), + relative_translation.key2(), + w_relative_translation, + relative_translation.noiseModel())) # Indices of measurements that are to be used as projection directions. These are randomly chosen. - indices = np.random.choice(len(w_measurements), min( - max_1dsfm_projection_directions, len(w_measurements)), replace=False) + sampled_indices = np.random.choice(len(w_relative_translations), min( + max_1dsfm_projection_directions, len(w_relative_translations)), replace=False) # Sample projection directions from the measurements. - projection_directions = [w_measurements[idx].measured() for idx in indices] + projection_directions = [ + w_relative_translations[idx].measured() for idx in sampled_indices] outlier_weights = [] # Find the outlier weights for each direction using MFAS. for direction in projection_directions: - algorithm = gtsam.MFAS(w_measurements, direction) + algorithm = gtsam.MFAS(w_relative_translations, direction) outlier_weights.append(algorithm.computeOutlierWeights()) - # Compute average of outlier weights. - avg_outlier_weights = {} + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, + # where weights are proportional to the probability of the edge being an outlier. + avg_outlier_weights = defaultdict(lambda: 0.0) for outlier_weight_dict in outlier_weights: - for k, v in outlier_weight_dict.items(): - if k in avg_outlier_weights: - avg_outlier_weights[k] += v / len(outlier_weights) - else: - avg_outlier_weights[k] = v / len(outlier_weights) + for keypair, weight in outlier_weight_dict.items(): + avg_outlier_weights[keypair] += weight / len(outlier_weights) - # Remove measurements that have weight greater than threshold. - inlier_measurements = gtsam.BinaryMeasurementsUnit3() - [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[( - m.key1(), m.key2())] < outlier_weight_threshold] + # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. + inlier_w_relative_translations = gtsam.BinaryMeasurementsUnit3() + [inlier_w_relative_translations.append(Z) for Z in w_relative_translations + if avg_outlier_weights[(Z.key1(), Z.key2())] < outlier_weight_threshold] # Run the optimizer to obtain translations for normalized directions. - translations = gtsam.TranslationRecovery(inlier_measurements).run() + w_translations = gtsam.TranslationRecovery( + inlier_w_relative_translations).run() poses = gtsam.Values() for key in rotations.keys(): poses.insert(key, gtsam.Pose3( - rotations.atRot3(key), translations.atPoint3(key))) + rotations.atRot3(key), w_translations.atPoint3(key))) return poses def main(): rotations, translation_directions = get_data() - poses = estimate_poses_given_rot(translation_directions, rotations) + poses = estimate_poses(translation_directions, rotations) print("**** Translation averaging output ****") print(poses) print("**************************************") From 98404ad27e583ef72fca03aa2f46f9f89fc31254 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 27 Sep 2020 18:55:14 -0700 Subject: [PATCH 07/12] updating defaultdict init --- python/gtsam/examples/TranslationAveragingExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index c3dcf2ad2..0f1314645 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -89,7 +89,7 @@ def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, # where weights are proportional to the probability of the edge being an outlier. - avg_outlier_weights = defaultdict(lambda: 0.0) + avg_outlier_weights = defaultdict(float) for outlier_weight_dict in outlier_weights: for keypair, weight in outlier_weight_dict.items(): avg_outlier_weights[keypair] += weight / len(outlier_weights) From 634682738e085d1a3c7d4ab2efdf9716971e1327 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 30 Sep 2020 23:25:20 -0700 Subject: [PATCH 08/12] renaming variables --- .../examples/TranslationAveragingExample.py | 116 +++++++++--------- 1 file changed, 60 insertions(+), 56 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 0f1314645..a374dc630 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -22,99 +22,103 @@ import numpy as np import gtsam from gtsam.examples import SFMdata +# Hyperparameters for 1dsfm, values used from Kyle Wilson's code. +MAX_1DSFM_PROJECTION_DIRECTIONS = 48 +OUTLIER_WEIGHT_THRESHOLD = 0.1 + def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: - """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" + """"Returns global rotations and unit translation directions between 8 cameras + that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata + and the unit translations directions between some camera pairs are computed from their + global translations. """ # Using toy dataset in SfMdata for example. - poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) - # Rotations of the cameras in the world frame - wRc. - rotations = gtsam.Values() - # Normalized translation directions for pairs of cameras - from first camera to second, - # in the coordinate frame of the first camera. - translation_directions = [] - for i in range(0, len(poses) - 2): + wTc = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + # Rotations of the cameras in the world frame. + wRc_values = gtsam.Values() + # Normalized translation directions from camera i to camera j + # in the coordinate frame of camera i. + i_iZj_list = [] + for i in range(0, len(wTc) - 2): # Add the rotation. - rotations.insert(i, poses[i].rotation()) + wRc_values.insert(i, wTc[i].rotation()) # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): - i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate( - poses[j].translation() - poses[i].translation())) - translation_directions.append(gtsam.BinaryMeasurementUnit3( - i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + i_iZj = gtsam.Unit3(wTc[i].rotation().unrotate( + wTc[j].translation() - wTc[i].translation())) + i_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. - rotations.insert(len(poses) - 1, poses[-1].rotation()) - rotations.insert(len(poses) - 2, poses[-2].rotation()) - return (rotations, translation_directions) + wRc_values.insert(len(wTc) - 1, wTc[-1].rotation()) + wRc_values.insert(len(wTc) - 2, wTc[-2].rotation()) + return (wRc_values, i_iZj_list) -def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, - rotations: gtsam.Values) -> gtsam.Values: - """Estimate poses given rotations normalized translation directions between cameras. +def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, + wRc_values: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations and normalized translation directions between cameras. Args: - relative_translations -- List of normalized translation directions between camera pairs, each direction - is from the first camera to the second, in the frame of the first camera. - rotations -- Rotations of the cameras in the world frame. + iZj_list -- List of normalized translation direction measurements between camera pairs, + Z here refers to measurements. The measurements are of camera j with reference + to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit + vector to j in i's frame and is not a transformation. + wRc_values -- Rotations of the cameras in the world frame. Returns: Values -- Estimated poses. """ - # Some hyperparameters, values used from 1dsfm. - max_1dsfm_projection_directions = 48 - outlier_weight_threshold = 0.1 + # Convert the translation direction measurements to world frame using the rotations. + w_iZj_list = gtsam.BinaryMeasurementsUnit3() + for i_iZj in i_iZj_list: + w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) + .rotate(i_iZj.measured().point3())) + w_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) - # Convert the translation directions to global frame using the rotations. - w_relative_translations = gtsam.BinaryMeasurementsUnit3() - for relative_translation in relative_translations: - w_relative_translation = gtsam.Unit3(rotations.atRot3(relative_translation.key1()) - .rotate(relative_translation.measured().point3())) - w_relative_translations.append(gtsam.BinaryMeasurementUnit3(relative_translation.key1(), - relative_translation.key2(), - w_relative_translation, - relative_translation.noiseModel())) - - # Indices of measurements that are to be used as projection directions. These are randomly chosen. - sampled_indices = np.random.choice(len(w_relative_translations), min( - max_1dsfm_projection_directions, len(w_relative_translations)), replace=False) + # Indices of measurements that are to be used as projection directions. + # These are randomly chosen. + sampled_indices = np.random.choice(len(w_iZj_list), min( + MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)), replace=False) # Sample projection directions from the measurements. - projection_directions = [ - w_relative_translations[idx].measured() for idx in sampled_indices] + projection_directions = [w_iZj_list[idx].measured() + for idx in sampled_indices] outlier_weights = [] # Find the outlier weights for each direction using MFAS. for direction in projection_directions: - algorithm = gtsam.MFAS(w_relative_translations, direction) + algorithm = gtsam.MFAS(w_iZj_list, direction) outlier_weights.append(algorithm.computeOutlierWeights()) - # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, - # where weights are proportional to the probability of the edge being an outlier. + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys + # (camera IDs) to a weight, where weights are proportional to the probability of the edge + # being an outlier. avg_outlier_weights = defaultdict(float) for outlier_weight_dict in outlier_weights: for keypair, weight in outlier_weight_dict.items(): avg_outlier_weights[keypair] += weight / len(outlier_weights) # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. - inlier_w_relative_translations = gtsam.BinaryMeasurementsUnit3() - [inlier_w_relative_translations.append(Z) for Z in w_relative_translations - if avg_outlier_weights[(Z.key1(), Z.key2())] < outlier_weight_threshold] + w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() + [w_iZj_inliers.append(Z) for Z in w_iZj_list + if avg_outlier_weights[(Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] # Run the optimizer to obtain translations for normalized directions. - w_translations = gtsam.TranslationRecovery( - inlier_w_relative_translations).run() + wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() - poses = gtsam.Values() - for key in rotations.keys(): - poses.insert(key, gtsam.Pose3( - rotations.atRot3(key), w_translations.atPoint3(key))) - return poses + wTc_values = gtsam.Values() + for key in wRc_values.keys(): + wTc_values.insert(key, gtsam.Pose3( + wRc_values.atRot3(key), wtc_values.atPoint3(key))) + return wTc_values def main(): - rotations, translation_directions = get_data() - poses = estimate_poses(translation_directions, rotations) + wRc_values, w_iZj_list = get_data() + wTc_values = estimate_poses(w_iZj_list, wRc_values) print("**** Translation averaging output ****") - print(poses) + print(wTc_values) print("**************************************") From a490017669578711140d48f5aaa22b77ce7efc9d Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 1 Oct 2020 22:19:17 -0700 Subject: [PATCH 09/12] outlier rejection in separate fn and other readability changes --- .../examples/TranslationAveragingExample.py | 104 ++++++++++-------- 1 file changed, 61 insertions(+), 43 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index a374dc630..d843f8702 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -15,7 +15,7 @@ Date: September 2020 """ from collections import defaultdict -from typing import Tuple, List +from typing import List, Tuple import numpy as np @@ -28,59 +28,48 @@ OUTLIER_WEIGHT_THRESHOLD = 0.1 def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: - """"Returns global rotations and unit translation directions between 8 cameras - that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata - and the unit translations directions between some camera pairs are computed from their + """"Returns global rotations and unit translation directions between 8 cameras + that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata + and the unit translations directions between some camera pairs are computed from their global translations. """ # Using toy dataset in SfMdata for example. - wTc = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j # in the coordinate frame of camera i. i_iZj_list = [] - for i in range(0, len(wTc) - 2): + for i in range(0, len(wTc_list) - 2): # Add the rotation. - wRc_values.insert(i, wTc[i].rotation()) + wRi = wTc_list[i].rotation() + wRc_values.insert(i, wRi) # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): - i_iZj = gtsam.Unit3(wTc[i].rotation().unrotate( - wTc[j].translation() - wTc[i].translation())) + # Compute the translation from pose i to pose j, in the world reference frame. + w_itj = wTc_list[j].translation() - wTc_list[i].translation() + # Obtain the translation in the camera i's reference frame. + i_itj = wRi.unrotate(w_itj) + # Compute the normalized unit translation direction. + i_iZj = gtsam.Unit3(i_itj) i_iZj_list.append(gtsam.BinaryMeasurementUnit3( i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. - wRc_values.insert(len(wTc) - 1, wTc[-1].rotation()) - wRc_values.insert(len(wTc) - 2, wTc[-2].rotation()) - return (wRc_values, i_iZj_list) + wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation()) + wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation()) + return wRc_values, i_iZj_list -def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, - wRc_values: gtsam.Values) -> gtsam.Values: - """Estimate poses given rotations and normalized translation directions between cameras. +def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: + """Removes outliers from a list of Unit3 measurements that are the + translation directions from camera i to camera j in the world frame.""" - Args: - iZj_list -- List of normalized translation direction measurements between camera pairs, - Z here refers to measurements. The measurements are of camera j with reference - to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit - vector to j in i's frame and is not a transformation. - wRc_values -- Rotations of the cameras in the world frame. + # Indices of measurements that are to be used as projection directions. + # These are randomly chosen. All sampled directions must be unique. + num_directions_to_sample = min( + MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)) + sampled_indices = np.random.choice( + len(w_iZj_list), num_directions_to_sample, replace=False) - Returns: - Values -- Estimated poses. - """ - - # Convert the translation direction measurements to world frame using the rotations. - w_iZj_list = gtsam.BinaryMeasurementsUnit3() - for i_iZj in i_iZj_list: - w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) - .rotate(i_iZj.measured().point3())) - w_iZj_list.append(gtsam.BinaryMeasurementUnit3( - i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) - - # Indices of measurements that are to be used as projection directions. - # These are randomly chosen. - sampled_indices = np.random.choice(len(w_iZj_list), min( - MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)), replace=False) # Sample projection directions from the measurements. projection_directions = [w_iZj_list[idx].measured() for idx in sampled_indices] @@ -91,8 +80,8 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, algorithm = gtsam.MFAS(w_iZj_list, direction) outlier_weights.append(algorithm.computeOutlierWeights()) - # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys - # (camera IDs) to a weight, where weights are proportional to the probability of the edge + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys + # (camera IDs) to a weight, where weights are proportional to the probability of the edge # being an outlier. avg_outlier_weights = defaultdict(float) for outlier_weight_dict in outlier_weights: @@ -101,8 +90,37 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() - [w_iZj_inliers.append(Z) for Z in w_iZj_list - if avg_outlier_weights[(Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] + [w_iZj_inliers.append(Z) for Z in w_iZj_list if avg_outlier_weights[( + Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] + + return w_iZj_inliers + + +def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, + wRc_values: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations and normalized translation directions between cameras. + + Args: + i_iZj_list: List of normalized translation direction measurements between camera pairs, + Z here refers to measurements. The measurements are of camera j with reference + to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit + vector to j in i's frame and is not a transformation. + wRc_values: Rotations of the cameras in the world frame. + + Returns: + Values: Estimated poses. + """ + + # Convert the translation direction measurements to world frame using the rotations. + w_iZj_list = gtsam.BinaryMeasurementsUnit3() + for i_iZj in i_iZj_list: + w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) + .rotate(i_iZj.measured().point3())) + w_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) + + # Remove the outliers in the unit translation directions. + w_iZj_inliers = prune_to_inliers(w_iZj_list) # Run the optimizer to obtain translations for normalized directions. wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() @@ -115,8 +133,8 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, def main(): - wRc_values, w_iZj_list = get_data() - wTc_values = estimate_poses(w_iZj_list, wRc_values) + wRc_values, i_iZj_list = get_data() + wTc_values = estimate_poses(i_iZj_list, wRc_values) print("**** Translation averaging output ****") print(wTc_values) print("**************************************") From 695f75bc8d37cd820108a399c7f992472b4d0b20 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 2 Oct 2020 07:56:41 -0700 Subject: [PATCH 10/12] readability changes --- python/gtsam/examples/TranslationAveragingExample.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index d843f8702..683008749 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -32,8 +32,8 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. """ - # Using toy dataset in SfMdata for example. - wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) + fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 + wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j @@ -88,10 +88,10 @@ def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryM for keypair, weight in outlier_weight_dict.items(): avg_outlier_weights[keypair] += weight / len(outlier_weights) - # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. + # Remove w_iZj that have weight greater than threshold, these are outliers. w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() - [w_iZj_inliers.append(Z) for Z in w_iZj_list if avg_outlier_weights[( - Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD] + [w_iZj_inliers.append(Z) for w_iZj in w_iZj_list if avg_outlier_weights[( + w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] return w_iZj_inliers @@ -108,7 +108,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, wRc_values: Rotations of the cameras in the world frame. Returns: - Values: Estimated poses. + gtsam.Values: Estimated poses. """ # Convert the translation direction measurements to world frame using the rotations. From f11ce11678b8cac8341d9fb3f4dda4dd7931e084 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 2 Oct 2020 08:03:28 -0700 Subject: [PATCH 11/12] fixing one variable that was not renamed --- python/gtsam/examples/TranslationAveragingExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 683008749..7e8c96b15 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -90,7 +90,7 @@ def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryM # Remove w_iZj that have weight greater than threshold, these are outliers. w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() - [w_iZj_inliers.append(Z) for w_iZj in w_iZj_list if avg_outlier_weights[( + [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[( w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] return w_iZj_inliers From 03ca9053421dcb51c63065a03a5cc99081e8a529 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 2 Oct 2020 23:44:55 -0700 Subject: [PATCH 12/12] removing shared ptr, iostream, renaming --- gtsam/sfm/MFAS.cpp | 2 -- gtsam/sfm/tests/testMFAS.cpp | 2 -- python/gtsam/examples/TranslationAveragingExample.py | 4 ++-- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index bc66d0711..4cd983ecd 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -7,8 +7,6 @@ #include -#include - #include #include #include diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index b2daf0d2e..362027d5d 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -9,8 +9,6 @@ #include -#include - using namespace std; using namespace gtsam; diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 7e8c96b15..054b61126 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -59,7 +59,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: return wRc_values, i_iZj_list -def prune_to_inliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: +def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: """Removes outliers from a list of Unit3 measurements that are the translation directions from camera i to camera j in the world frame.""" @@ -120,7 +120,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) # Remove the outliers in the unit translation directions. - w_iZj_inliers = prune_to_inliers(w_iZj_list) + w_iZj_inliers = filter_outliers(w_iZj_list) # Run the optimizer to obtain translations for normalized directions. wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()