gtsam  4.1.0
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>
24 #include <gtsam/inference/graph.h>
28 
29 #include <map>
30 #include <vector>
31 
32 namespace gtsam {
33 
34 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
35 typedef std::map<Key, Rot3> KeyRotMap;
36 
37 struct 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
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
VectorValues.h
Factor Graph Values.
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Rot3
Definition: Rot3.h:59
gtsam::InitializePose3
Definition: InitializePose3.h:37
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
graph.h
Graph algorithm using boost library.
Rot3.h
3D rotation represented as a rotation matrix or quaternion