add LAGO example to Python

python/gtsam/examples/Pose2SLAMExample_lago.py

@@ -0,0 +1,71 @@
+"""
+GTSAM Copyright 2010, 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
+
+A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
+using LAGO (Linear Approximation for Graph Optimization).
+Output is written to a file, in g2o format
+
+Reference:
+L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
+approximation for planar pose graph optimization, IJRR, 2014.
+
+L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
+for graph-based simultaneous localization and mapping, RSS, 2011.
+
+Author: Luca Carlone (C++), John Lambert (Python)
+"""
+
+import argparse
+from argparse import Namespace
+
+import numpy as np
+
+import gtsam
+from gtsam import Pose2, PriorFactorPose2, Values
+
+
+def vector3(x: float, y: float, z: float) -> np.ndarray:
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=float)
+
+
+def run(args: Namespace) -> None:
+    """Run LAGO on input data stored in g2o file."""
+    g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None else args.input
+
+    graph = gtsam.NonlinearFactorGraph()
+    graph, initial = gtsam.readG2o(g2oFile)
+
+    # Add prior on the pose having index (key) = 0
+    priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
+    graph.add(PriorFactorPose2(0, Pose2(), priorModel))
+    graph.print()
+
+    print("Computing LAGO estimate")
+    estimateLago: Values = lago.initialize(graph)
+    print("done!")
+
+    if args.output is None:
+        estimateLago.print("estimateLago")
+    else:
+        outputFile = args.output
+        print("Writing results to file: ", outputFile)
+        graphNoKernel = gtsam.NonlinearFactorGraph()
+        graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
+        gtsam.writeG2o(graphNoKernel, estimateLago, outputFile)
+        print("Done! ")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description="A 2D Pose SLAM example that reads input from g2o, "
+        "converts it to a factor graph and does the optimization. "
+        "Output is written on a file, in g2o format"
+    )
+    parser.add_argument("-i", "--input", help="input file g2o format")
+    parser.add_argument("-o", "--output", help="the path to the output file with optimized graph")
+    run(args)