68 lines
2.3 KiB
Python
68 lines
2.3 KiB
Python
"""
|
|
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)
|