From 4dbd006d7d6160b4f83f08268cf03788d3df4cad Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 10:18:39 -0400 Subject: [PATCH 01/10] add LAGO (for Pose2) to python wrapper --- gtsam/slam/slam.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 1c04fd14c0..e41dee433c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -334,5 +334,9 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { Vector evaluateError(const T& R1, const T& R2); }; + +#include +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); } // namespace gtsam From c5e24dbae4828800904bf533e93b02a10c7dff43 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 10:37:00 -0400 Subject: [PATCH 02/10] 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 0000000000..beac0f8e0a --- /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) From 769c75c01f14ccf73be3f77da7f21b81e9c8bf47 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 11:03:29 -0600 Subject: [PATCH 03/10] use nested namespace in wrapper from Varun's suggestion --- gtsam/slam/slam.i | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e41dee433c..24139444c1 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -336,7 +336,9 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { }; #include -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); - +namespace lago { + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); +} + } // namespace gtsam From b9f10cdb153eed5863421cc30cf51602c632d333 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 11:04:06 -0600 Subject: [PATCH 04/10] use nested namespace --- python/gtsam/examples/Pose2SLAMExample_lago.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py index beac0f8e0a..92a219d211 100644 --- a/python/gtsam/examples/Pose2SLAMExample_lago.py +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -46,7 +46,7 @@ def run(args: Namespace) -> None: graph.print() print("Computing LAGO estimate") - estimateLago: Values = lago.initialize(graph) + estimateLago: Values = gtsam.lago.initialize(graph) print("done!") if args.output is None: From 3ce02ba21e58039f2c75e6223d3553a6fe1443cb Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 16:08:33 -0400 Subject: [PATCH 05/10] fix typos in python example file --- python/gtsam/examples/Pose2SLAMExample_lago.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py index 92a219d211..b50b387590 100644 --- a/python/gtsam/examples/Pose2SLAMExample_lago.py +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -43,7 +43,7 @@ def run(args: Namespace) -> None: # 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(graph) print("Computing LAGO estimate") estimateLago: Values = gtsam.lago.initialize(graph) @@ -68,4 +68,5 @@ def run(args: Namespace) -> None: ) 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) From 84d291003f954d5682771cc3b0dc22452b3f0a83 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 16:14:44 -0400 Subject: [PATCH 06/10] add lago unit test, since lago namespace cannot be imported properly be wrapper --- python/gtsam/tests/test_lago.py | 34 +++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 python/gtsam/tests/test_lago.py diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py new file mode 100644 index 0000000000..f1e62ca92e --- /dev/null +++ b/python/gtsam/tests/test_lago.py @@ -0,0 +1,34 @@ +""" +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 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 test_lago() -> 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(vector3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) From bc68ecb5ab2a01566de479eea741e14edcfcc43e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 01:18:51 -0400 Subject: [PATCH 07/10] use unittest framework instead of pytest --- python/gtsam/tests/test_lago.py | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index f1e62ca92e..5b836ccdff 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -8,6 +8,8 @@ Author: John Lambert (Python) """ +import unittest + import numpy as np import gtsam @@ -19,16 +21,23 @@ def vector3(x: float, y: float, z: float) -> np.ndarray: return np.array([x, y, z], dtype=float) -def test_lago() -> None: - """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" - g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") +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(vector3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) - graph = gtsam.NonlinearFactorGraph() - graph, initial = gtsam.readG2o(g2oFile) + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) - # 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)) - estimateLago: Values = gtsam.lago.initialize(graph) - assert isinstance(estimateLago, Values) +if __name__ == "__main__": + unittest.main() From 97fee355ee52077753b2a343245425381e8799bc Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 09:13:17 -0400 Subject: [PATCH 08/10] add missing default arg on useOdometricPath --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 24139444c1..60000dbab1 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -337,8 +337,8 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { #include namespace lago { + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); - gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); } } // namespace gtsam From d48b7371bb4a0f019168f54b98079110b4500076 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 15:05:05 -0400 Subject: [PATCH 09/10] use Point3 instead of artificial vector3 --- python/gtsam/tests/test_lago.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index 5b836ccdff..8ed5dd943f 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -13,12 +13,7 @@ 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) +from gtsam import Point3, Pose2, PriorFactorPose2, Values class TestLago(unittest.TestCase): @@ -32,7 +27,7 @@ def test_initialize(self) -> None: 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)) + priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) graph.add(PriorFactorPose2(0, Pose2(), priorModel)) estimateLago: Values = gtsam.lago.initialize(graph) From a93c58abd6e510f22528f0fb380fd4e97e33a986 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 15:05:37 -0400 Subject: [PATCH 10/10] use Point3 instead of artificial vector3 --- python/gtsam/examples/Pose2SLAMExample_lago.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py index b50b387590..d8cddde0b3 100644 --- a/python/gtsam/examples/Pose2SLAMExample_lago.py +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -25,12 +25,7 @@ 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) +from gtsam import Point3, Pose2, PriorFactorPose2, Values def run(args: Namespace) -> None: @@ -41,7 +36,7 @@ def run(args: Namespace) -> None: 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)) + priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) graph.add(PriorFactorPose2(0, Pose2(), priorModel)) print(graph)