Pose3SLAMExample_initializePose3Chordal.py

Go to the documentation of this file.

"""
GTSAM Copyright 2010-2018, 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
 
Initialize PoseSLAM with Chordal init
Author: Luca Carlone, Frank Dellaert (python port)
"""
# pylint: disable=invalid-name, E1101
 
from __future__ import print_function
 
import gtsam
import numpy as np
 
 
def main():
    """Main runner."""
    # Read graph from file
    g2oFile = gtsam.findExampleDataFile("pose3example.txt")
 
    is3D = True
    graph, initial = gtsam.readG2o(g2oFile, is3D)
 
    # Add prior on the first key. TODO: assumes first key ios z
    priorModel = gtsam.noiseModel.Diagonal.Variances(
        np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
    firstKey = initial.keys()[0]
    graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
 
    # Initializing Pose3 - chordal relaxation
    initialization = gtsam.InitializePose3.initialize(graph)
 
    print(initialization)
 
 
if __name__ == "__main__":
    main()