gtsam 4.1.1
gtsam
InitializePose3.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
21#pragma once
22
23#include <gtsam/geometry/Rot3.h>
28
29#include <map>
30#include <vector>
31
32namespace gtsam {
33
34typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
35typedef std::map<Key, Rot3> KeyRotMap;
36
37struct GTSAM_EXPORT InitializePose3 {
38 static GaussianFactorGraph buildLinearOrientationGraph(
39 const NonlinearFactorGraph& g);
40
41 static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
42
46 static Values computeOrientationsChordal(
47 const NonlinearFactorGraph& pose3Graph);
48
52 static Values computeOrientationsGradient(
53 const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
54 size_t maxIter = 10000, const bool setRefFrame = true);
55
56 static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
57 KeyVectorMap* adjEdgesMap,
58 KeyRotMap* factorId2RotMap);
59
60 static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
61 const double b);
62
67 static NonlinearFactorGraph buildPose3graph(
68 const NonlinearFactorGraph& graph);
69
73 static Values computePoses(const Values& initialRot,
74 NonlinearFactorGraph* poseGraph,
75 bool singleIter = true);
76
81 static Values initializeOrientations(const NonlinearFactorGraph& graph);
82
88 static Values initialize(const NonlinearFactorGraph& graph,
89 const Values& givenGuess, bool useGradient = false);
90
92 static Values initialize(const NonlinearFactorGraph& graph);
93};
94} // end of namespace gtsam
3D rotation represented as a rotation matrix or quaternion
Graph algorithm using boost library.
Linear Factor Graph where all factors are Gaussians.
Factor Graph Values.
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Definition: Rot3.h:59
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
Definition: InitializePose3.h:37