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>
27 
28 #include <map>
29 #include <vector>
30 
31 namespace gtsam {
32 
33 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
34 typedef std::map<Key, Rot3> KeyRotMap;
35 
36 struct GTSAM_EXPORT InitializePose3 {
38  const NonlinearFactorGraph& g);
39 
40  static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
41 
45  static Values computeOrientationsChordal(
46  const NonlinearFactorGraph& pose3Graph);
47 
51  static Values computeOrientationsGradient(
52  const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
53  size_t maxIter = 10000, const bool setRefFrame = true);
54 
55  static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
56  KeyVectorMap* adjEdgesMap,
57  KeyRotMap* factorId2RotMap);
58 
59  static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
60  const double b);
61 
66  static NonlinearFactorGraph buildPose3graph(
68 
72  static Values computePoses(const Values& initialRot,
73  NonlinearFactorGraph* poseGraph,
74  bool singleIter = true);
75 
81 
88  const Values& givenGuess, bool useGradient = false);
89 
92 };
93 } // end of namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::initialize::computePoses
static Values computePoses(const Values &initialRot, NonlinearFactorGraph *posegraph, bool singleIter=true)
Definition: InitializePose.h:58
Rot3.h
3D rotation represented as a rotation matrix or quaternion
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::KeyRotMap
std::map< Key, Rot3 > KeyRotMap
Definition: InitializePose3.h:34
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
VectorValues.h
Factor Graph Values.
gtsam::lago::initializeOrientations
VectorValues initializeOrientations(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:297
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::b
const G & b
Definition: Group.h:79
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gtsam::InitializePose3
Definition: InitializePose3.h:36
gtsam::lago::buildLinearOrientationGraph
GaussianFactorGraph buildLinearOrientationGraph(const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap &tree)
Definition: lago.cpp:165
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::lago::initialize
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
gtsam::KeyVectorMap
std::map< Key, std::vector< size_t > > KeyVectorMap
Definition: InitializePose3.h:33


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:46