SimpleRotation.cpp
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 
25 // In this example, a 2D rotation will be used as the variable of interest
26 #include <gtsam/geometry/Rot2.h>
27 
28 // Each variable in the system (poses) must be identified with a unique key.
29 // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
30 // Here we will use symbols
31 #include <gtsam/inference/Symbol.h>
32 
33 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
34 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
35 // We will apply a simple prior on the rotation. We do so via the `addPrior` convenience
36 // method in NonlinearFactorGraph.
37 
38 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
39 // are nonlinear factors, we will need a Nonlinear Factor Graph.
41 
42 // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
43 // nonlinear functions around an initial linearization point, then solve the linear system
44 // to update the linearization point. This happens repeatedly until the solver converges
45 // to a consistent set of variable values. This requires us to specify an initial guess
46 // for each variable, held in a Values container.
47 #include <gtsam/nonlinear/Values.h>
48 
49 // Finally, once all of the factors have been added to our factor graph, we will want to
50 // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
51 // GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
52 // standard Levenberg-Marquardt solver
54 
55 
56 using namespace std;
57 using namespace gtsam;
58 
59 const double degree = M_PI / 180;
60 
61 int main() {
76  Rot2 prior = Rot2::fromAngle(30 * degree);
77  prior.print("goal angle");
78  auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
79  Symbol key('x', 1);
80 
92  graph.print("full graph");
93 
110  Values initial;
111  initial.insert(key, Rot2::fromAngle(20 * degree));
112  initial.print("initial estimate");
113 
123  result.print("final result");
124 
125  return 0;
126 }
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
main
int main()
Definition: SimpleRotation.cpp:61
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
result
Values result
Definition: OdometryOptimize.cpp:8
Rot2.h
2D rotation
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
degree
const double degree
Definition: SimpleRotation.cpp:59
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
key
const gtsam::Symbol key('X', 0)
gtsam::Rot2
Definition: Rot2.h:35
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
initial
Definition: testScenarioRunner.cpp:148
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:238
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactorGraph.cpp:55


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:12