SimpleRotation.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 This example will perform a relatively trivial optimization on
10 a single variable with a single factor.
11 """
12 
13 import numpy as np
14 import gtsam
15 from gtsam.symbol_shorthand import X
16 
17 def main():
18  """
19  Step 1: Create a factor to express a unary constraint
20 
21  The "prior" in this case is the measurement from a sensor,
22  with a model of the noise on the measurement.
23 
24  The "Key" created here is a label used to associate parts of the
25  state (stored in "RotValues") with particular factors. They require
26  an index to allow for lookup, and should be unique.
27 
28  In general, creating a factor requires:
29  - A key or set of keys labeling the variables that are acted upon
30  - A measurement value
31  - A measurement model with the correct dimensionality for the factor
32  """
33  prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
34  prior.print('goal angle')
35  model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
36  key = X(1)
37  factor = gtsam.PriorFactorRot2(key, prior, model)
38 
39  """
40  Step 2: Create a graph container and add the factor to it
41 
42  Before optimizing, all factors need to be added to a Graph container,
43  which provides the necessary top-level functionality for defining a
44  system of constraints.
45 
46  In this case, there is only one factor, but in a practical scenario,
47  many more factors would be added.
48  """
50  graph.push_back(factor)
51  graph.print('full graph')
52 
53  """
54  Step 3: Create an initial estimate
55 
56  An initial estimate of the solution for the system is necessary to
57  start optimization. This system state is the "Values" instance,
58  which is similar in structure to a dictionary, in that it maps
59  keys (the label created in step 1) to specific values.
60 
61  The initial estimate provided to optimization will be used as
62  a linearization point for optimization, so it is important that
63  all of the variables in the graph have a corresponding value in
64  this structure.
65  """
66  initial = gtsam.Values()
67  initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
68  initial.print('initial estimate')
69 
70  """
71  Step 4: Optimize
72 
73  After formulating the problem with a graph of constraints
74  and an initial estimate, executing optimization is as simple
75  as calling a general optimization function with the graph and
76  initial estimate. This will yield a new RotValues structure
77  with the final state of the optimization.
78  """
79  result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
80  result.print('final result')
81 
82 
83 if __name__ == '__main__':
84  main()
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
#define X
Definition: icosphere.cpp:20
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594


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