-
Notifications
You must be signed in to change notification settings - Fork 764
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #899 from borglab/add-pose2-lago-to-wrapper
- Loading branch information
Showing
3 changed files
with
112 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() |