From c5e24dbae4828800904bf533e93b02a10c7dff43 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Thu, 21 Oct 2021 10:37:00 -0400
Subject: [PATCH] add LAGO example to Python

---
 .../gtsam/examples/Pose2SLAMExample_lago.py   | 71 +++++++++++++++++++
 1 file changed, 71 insertions(+)
 create mode 100644 python/gtsam/examples/Pose2SLAMExample_lago.py

diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py
new file mode 100644
index 000000000..beac0f8e0
--- /dev/null
+++ b/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)