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 
91  graph.addPrior(key, prior, model);
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 }
const gtsam::Symbol key('X', 0)
virtual const Values & optimize()
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
noiseModel::Diagonal::shared_ptr model
#define M_PI
Definition: main.h:106
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
const double degree
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
Values result
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
traits
Definition: chartTesting.h:28
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void print(const std::string &s="theta") const
Definition: Rot2.cpp:46
2D rotation
int main()


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:43