Loading...
Searching...
No Matches
InitializePose3.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/graph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <map>
#include <vector>
namespace gtsam {
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
typedef std::map<Key, Rot3> KeyRotMap;
struct GTSAM_EXPORT InitializePose3 {
static GaussianFactorGraph buildLinearOrientationGraph(
const NonlinearFactorGraph& g);
static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
static Values computeOrientationsChordal(
const NonlinearFactorGraph& pose3Graph);
static Values computeOrientationsGradient(
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
size_t maxIter = 10000, const bool setRefFrame = true);
static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
KeyVectorMap* adjEdgesMap,
KeyRotMap* factorId2RotMap);
static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
const double b);
static NonlinearFactorGraph buildPose3graph(
const NonlinearFactorGraph& graph);
static Values computePoses(const Values& initialRot,
NonlinearFactorGraph* poseGraph,
bool singleIter = true);
static Values initializeOrientations(const NonlinearFactorGraph& graph);
static Values initialize(const NonlinearFactorGraph& graph,
const Values& givenGuess, bool useGradient = false);
static Values initialize(const NonlinearFactorGraph& graph);
};
} // end of namespace gtsam
Rot3.h
3D rotation represented as a rotation matrix or quaternion
graph.h
Graph algorithm using boost library.
VectorValues.h
Factor Graph Values.
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::InitializePose3
Definition InitializePose3.h:37
gtsam::InitializePose3::initialize
static Values initialize(const NonlinearFactorGraph &graph, const Values &givenGuess, bool useGradient=false)
"extract" the Pose3 subgraph of the original graph, get orientations from relative orientation measur...
Definition InitializePose3.cpp:298
gtsam::InitializePose3::computePoses
static Values computePoses(const Values &initialRot, NonlinearFactorGraph *poseGraph, bool singleIter=true)
Use Gauss-Newton optimizer to optimize for poses given rotation estimates.
Definition InitializePose3.cpp:290
gtsam::InitializePose3::computeOrientationsChordal
static Values computeOrientationsChordal(const NonlinearFactorGraph &pose3Graph)
Return the orientations of a graph including only BetweenFactors<Pose3>.
Definition InitializePose3.cpp:104
gtsam::InitializePose3::buildPose3graph
static NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph &graph)
Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node.
Definition InitializePose3.cpp:97
gtsam::InitializePose3::computeOrientationsGradient
static Values computeOrientationsGradient(const NonlinearFactorGraph &pose3Graph, const Values &givenGuess, size_t maxIter=10000, const bool setRefFrame=true)
Return the orientations of a graph including only BetweenFactors<Pose3>.
Definition InitializePose3.cpp:119
gtsam::InitializePose3::initializeOrientations
static Values initializeOrientations(const NonlinearFactorGraph &graph)
"extract" the Pose3 subgraph of the original graph, get orientations from relative orientation measur...
Definition InitializePose3.cpp:280