Pose3SLAMExample_initializePose3Chordal.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 Initialize PoseSLAM with Chordal init
10 Author: Luca Carlone, Frank Dellaert (python port)
11 """
12 # pylint: disable=invalid-name, E1101
13 
14 from __future__ import print_function
15 
16 import gtsam
17 import numpy as np
18 
19 
20 def main():
21  """Main runner."""
22  # Read graph from file
23  g2oFile = gtsam.findExampleDataFile("pose3example.txt")
24 
25  is3D = True
26  graph, initial = gtsam.readG2o(g2oFile, is3D)
27 
28  # Add prior on the first key. TODO: assumes first key ios z
30  np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
31  firstKey = initial.keys()[0]
32  graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
33 
34  # Initializing Pose3 - chordal relaxation
35  initialization = gtsam.InitializePose3.initialize(graph)
36 
37  print(initialization)
38 
39 
40 if __name__ == "__main__":
41  main()
gtsam::readG2o
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam::Pose3
Definition: Pose3.h:37
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam.examples.Pose3SLAMExample_initializePose3Chordal.main
def main()
Definition: Pose3SLAMExample_initializePose3Chordal.py:20
gtsam::InitializePose3::initialize
static Values initialize(const NonlinearFactorGraph &graph, const Values &givenGuess, bool useGradient=false)
Definition: InitializePose3.cpp:296
gtsam::noiseModel::Diagonal::Variances
static shared_ptr Variances(const Vector &variances, bool smart=true)
Definition: NoiseModel.cpp:284


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:12:45