borglab · akshay-krishnan · Oct 3, 2020 · Sep 19, 2020 · Sep 19, 2020 · Sep 20, 2020
diff --git 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<gtsam::Unit3> BinaryMeasurementUnit3;
@@ -3105,6 +3106,26 @@ class ShonanAveraging3 {
   pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
 };
 
+#include <gtsam/sfm/MFAS.h>
+
+class KeyPairDoubleMap {
+  KeyPairDoubleMap();
+  KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
+
+  size_t size() const;
+  bool empty() const;
+  void clear();
+  size_t at(const pair<size_t, size_t>& keypair) const;
+};
+
+class MFAS {
+  MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+       const gtsam::Unit3& projectionDirection);
+
+  gtsam::KeyPairDoubleMap computeOutlierWeights() const;
+  gtsam::KeyVector computeOrdering() const;
+};
+
 #include <gtsam/sfm/TranslationRecovery.h>
 class TranslationRecovery {
   TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,

diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp
@@ -111,10 +111,8 @@ void removeNodeFromGraph(const Key node,
   graph.erase(node);
 }
 
-MFAS::MFAS(const std::shared_ptr<vector<Key>>& 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
@@ -56,40 +56,28 @@ class MFAS {
   using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
 
  private:
-  // pointer to nodes in the graph
-  const std::shared_ptr<std::vector<Key>> nodes_;
-
   // edges with a direction such that all weights are positive
   // i.e, edges that originally had negative weights are flipped
   std::map<KeyPair, double> edgeWeights_;
 
  public:
   /**
-   * @brief Construct from the nodes in a graph and weighted directed edges
+   * @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 std::shared_ptr<std::vector<Key>> &nodes,
-       const std::map<KeyPair, double> &edgeWeights)
-      : nodes_(nodes), edgeWeights_
8000
(edgeWeights) {}
+  MFAS(const std::map<KeyPair, double> &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 std::shared_ptr<std::vector<Key>> &nodes,
-       const TranslationEdges &relativeTranslations,
+  MFAS(const TranslationEdges &relativeTranslations,
        const Unit3 &projectionDirection);
 
   /**
@@ -99,7 +87,7 @@ class MFAS {
   std::vector<Key> 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.
@@ -108,4 +96,6 @@ class MFAS {
   std::map<KeyPair, double> computeOutlierWeights() const;
 };
 
+typedef std::map<std::pair<Key, Key>, double> KeyPairDoubleMap;
+
 }  // namespace gtsam
diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp
@@ -6,7 +6,7 @@
  */
 
 #include <gtsam/sfm/MFAS.h>
-#include <iostream>
+
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -39,14 +39,13 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &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(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights2));
+  MFAS mfas_obj(getEdgeWeights(edges, weights2));
 
   vector<Key> ordered_nodes = mfas_obj.computeOrdering();
 
@@ -76,7 +75,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<vector<Key>>(nodes), getEdgeWeights(edges, weights1));
+  MFAS mfas_obj(getEdgeWeights(edges, weights1));
 
   vector<Key> ordered_nodes = mfas_obj.computeOrdering();
 

diff --git 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/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py
@@ -0,0 +1,144 @@
+"""
+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
+"""
+
+from collections import defaultdict
+from typing import List, Tuple
+
+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 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. """
+    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
+    # in the coordinate frame of camera i.
+    i_iZj_list = []
+    for i in range(0, len(wTc_list) - 2):
+        # Add the 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):
+            # 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_list) - 1, wTc_list[-1].rotation())
+    wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation())
+    return wRc_values, i_iZj_list
+
+
+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."""
+
+    # 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)
+
+    # Sample projection directions from the measurements.
+    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_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.
+    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_iZj that have weight greater than threshold, these are outliers.
+    w_iZj_inliers = gtsam.BinaryMeasurementsUnit3()
+    [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
+
+
+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:
+        gtsam.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 = filter_outliers(w_iZj_list)
+
+    # Run the optimizer to obtain translations for normalized directions.
+    wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
+
+    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():
+    wRc_values, i_iZj_list = get_data()
+    wTc_values = estimate_poses(i_iZj_list, wRc_values)
+    print("**** Translation averaging output ****")
+    print(wTc_values)
+    print("**************************************")
+
+
+if __name__ == '__main__':
+    main()
diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
@@ -12,3 +12,4 @@ py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
 py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
 py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
+py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");