Merge pull request #899 from borglab/add-pose2-lago-to-wrapper

gtsam/slam/slam.i

@@ -334,5 +334,11 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
 
   Vector evaluateError(const T& R1, const T& R2);
 };
-
+  
+#include <gtsam/slam/lago.h>
+namespace lago {
+  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
+  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
+}
+  
 }  // namespace gtsam

python/gtsam/examples/Pose2SLAMExample_lago.py

@@ -0,0 +1,67 @@
+"""
+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 Point3, Pose2, PriorFactorPose2, Values
+
+
+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(Point3(1e-6, 1e-6, 1e-8))
+    graph.add(PriorFactorPose2(0, Pose2(), priorModel))
+    print(graph)
+
+    print("Computing LAGO estimate")
+    estimateLago: Values = gtsam.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")
+    args = parser.parse_args()
+    run(args)

python/gtsam/tests/test_lago.py

@@ -0,0 +1,38 @@
+"""
+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
+
+Author: John Lambert (Python)
+"""
+
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import Point3, Pose2, PriorFactorPose2, Values
+
+
+class TestLago(unittest.TestCase):
+    """Test selected LAGO methods."""
+
+    def test_initialize(self) -> None:
+        """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file."""
+        g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt")
+
+        graph = gtsam.NonlinearFactorGraph()
+        graph, initial = gtsam.readG2o(g2oFile)
+
+        # Add prior on the pose having index (key) = 0
+        priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8))
+        graph.add(PriorFactorPose2(0, Pose2(), priorModel))
+
+        estimateLago: Values = gtsam.lago.initialize(graph)
+        assert isinstance(estimateLago, Values)
+
+
+if __name__ == "__main__":
+    unittest.main()