adding documentation for example

python/gtsam/examples/TranslationAveragingExample.py

@@ -1,17 +1,27 @@
-from collections import Counter
+"""
-import functools
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
-import operator
+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):
+        for j in range(i + 1, i + 3):
-            i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(poses[j].translation() - poses[i].translation()))
+            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(
+        w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2(), gtsam.Unit3(
-        ), gtsam.Unit3(rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel()))
+            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()