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 numpy as np

import gtsam

# 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)