test_lago.py

Go to the documentation of this file.

"""
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()