GaussNewtonOptimizer.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 
23 
24 namespace gtsam {
25 
26 typedef internal::NonlinearOptimizerState State;
27 
28 /* ************************************************************************* */
30  const Values& initialValues,
33  graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues)))),
34  params_(ensureHasOrdering(params, graph)) {}
35 
37  const Values& initialValues, const Ordering& ordering)
39  graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues)))) {
41 }
42 
43 /* ************************************************************************* */
45  gttic(GaussNewtonOptimizer_Iterate);
46 
47  // Linearize graph
48  gttic(GaussNewtonOptimizer_Linearize);
50  gttoc(GaussNewtonOptimizer_Linearize);
51 
52  // Solve Factor Graph
53  gttic(GaussNewtonOptimizer_Solve);
54  const VectorValues delta = solve(*linear, params_);
55  gttoc(GaussNewtonOptimizer_Solve);
56 
57  // Maybe show output
59  delta.print("delta");
60 
61  // Create new state with new values and new error
62  Values newValues = state_->values.retract(delta);
63  state_.reset(new State(std::move(newValues), graph_.error(newValues), state_->iterations + 1));
64 
65  return linear;
66 }
67 
68 /* ************************************************************************* */
71  if (!params.ordering)
72  params.ordering = Ordering::Create(params.orderingType, graph);
73  return params;
74 }
75 
76 } /* namespace gtsam */
const NonlinearFactorGraph & graph() const
return the graph with nonlinear factors
static Ordering Create(OrderingType orderingType, const FACTOR_GRAPH &graph)
void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition: BFloat16.h:88
NonlinearFactorGraph graph
static enum @1107 ordering
const GaussNewtonParams & params() const
GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph &graph) const
static const SmartProjectionParams params
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
std::unique_ptr< internal::NonlinearOptimizerState > state_
PIMPL&#39;d state.
#define gttic(label)
Definition: timing.h:295
Factor Graph Values.
Linear Factor Graph where all factors are Gaussians.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
double error(const Values &values) const
internal::DoglegState State
traits
Definition: chartTesting.h:28
virtual VectorValues solve(const GaussianFactorGraph &gfg, const NonlinearOptimizerParams &params) const
GaussianFactorGraph::shared_ptr iterate() override
#define gttoc(label)
Definition: timing.h:296
NonlinearFactorGraph graph_
The graph with nonlinear factors.
Private class for NonlinearOptimizer state.
static double error
Definition: testRot3.cpp:37
Ordering::OrderingType orderingType
The method of ordering use during variable elimination (default COLAMD)
std::optional< Ordering > ordering
The optional variable elimination ordering, or empty to use COLAMD (default: empty) ...
double error() const
return error in current optimizer state
GaussNewtonOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GaussNewtonParams &params=GaussNewtonParams())


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:15