39 lines
1.1 KiB
Python
39 lines
1.1 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
|
|
|
|
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()
|