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)
73  return params;
74 }
75 
76 } /* namespace gtsam */
gttoc
#define gttoc(label)
Definition: timing.h:296
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
gtsam::NonlinearOptimizer::graph
const NonlinearFactorGraph & graph() const
return the graph with nonlinear factors
Definition: NonlinearOptimizer.h:118
gtsam::NonlinearOptimizer::solve
virtual VectorValues solve(const GaussianFactorGraph &gfg, const NonlinearOptimizerParams &params) const
Definition: NonlinearOptimizer.cpp:132
NonlinearOptimizerState.h
Private class for NonlinearOptimizer state.
gtsam::internal::DoglegState
Definition: DoglegOptimizer.cpp:54
gtsam::Ordering::Create
static Ordering Create(OrderingType orderingType, const FACTOR_GRAPH &graph)
Definition: inference/Ordering.h:217
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition: NonlinearOptimizerParams.h:46
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::GaussNewtonOptimizer::ensureHasOrdering
GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph &graph) const
Definition: GaussNewtonOptimizer.cpp:69
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::NonlinearOptimizerParams::orderingType
Ordering::OrderingType orderingType
The method of ordering use during variable elimination (default COLAMD)
Definition: NonlinearOptimizerParams.h:47
GaussNewtonOptimizer.h
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::GaussNewtonOptimizer::params
const GaussNewtonParams & params() const
Definition: GaussNewtonOptimizer.h:82
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::State
internal::DoglegState State
Definition: DoglegOptimizer.cpp:62
VectorValues.h
Factor Graph Values.
gtsam::NonlinearOptimizer::graph_
NonlinearFactorGraph graph_
The graph with nonlinear factors.
Definition: NonlinearOptimizer.h:78
ordering
static enum @1096 ordering
gtsam
traits
Definition: SFMdata.h:40
gtsam::NonlinearOptimizer
Definition: NonlinearOptimizer.h:75
error
static double error
Definition: testRot3.cpp:37
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::GaussNewtonOptimizer::iterate
GaussianFactorGraph::shared_ptr iterate() override
Definition: GaussNewtonOptimizer.cpp:44
gtsam::GaussNewtonOptimizer::params_
GaussNewtonParams params_
Definition: GaussNewtonOptimizer.h:41
gtsam::NonlinearOptimizerParams::DELTA
@ DELTA
Definition: NonlinearOptimizerParams.h:39
gtsam::GaussNewtonOptimizer::GaussNewtonOptimizer
GaussNewtonOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GaussNewtonParams &params=GaussNewtonParams())
Definition: GaussNewtonOptimizer.cpp:29
gtsam::NonlinearOptimizerParams::ordering
std::optional< Ordering > ordering
The optional variable elimination ordering, or empty to use COLAMD (default: empty)
Definition: NonlinearOptimizerParams.h:108
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::NonlinearOptimizer::state_
std::unique_ptr< internal::NonlinearOptimizerState > state_
PIMPL'd state.
Definition: NonlinearOptimizer.h:80
gtsam::Ordering
Definition: inference/Ordering.h:33
gttic
#define gttic(label)
Definition: timing.h:295


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:34