ConcurrentBatchSmoother.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 
22 #include <gtsam/base/timing.h>
23 #include <gtsam/base/debug.h>
24 
25 namespace gtsam {
26 
27 /* ************************************************************************* */
28 void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
29  std::cout << s;
30  std::cout << " Factors:" << std::endl;
31  for(const NonlinearFactor::shared_ptr& factor: factors_) {
32  PrintNonlinearFactor(factor, " ", keyFormatter);
33  }
34  theta_.print("Values:\n");
35 }
36 
37 /* ************************************************************************* */
38 bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) const {
39  const ConcurrentBatchSmoother* smoother = dynamic_cast<const ConcurrentBatchSmoother*>(&rhs);
40  return smoother
41  && factors_.equals(smoother->factors_)
42  && theta_.equals(smoother->theta_)
43  && ordering_.equals(smoother->ordering_)
44  && delta_.equals(smoother->delta_)
46 }
47 
48 /* ************************************************************************* */
50  const std::optional< std::vector<size_t> >& removeFactorIndices) {
51 
52  gttic(update);
53 
54  // Create the return result meta-data
55  Result result;
56 
57  // Update all of the internal variables with the new information
58  gttic(augment_system);
59  {
60  // Add the new variables to theta
61  theta_.insert(newTheta);
62 
63  // Add new variables to the end of the ordering
64  for(const auto key: newTheta.keys()) {
65  ordering_.push_back(key);
66  }
67 
68  // Augment Delta
69  delta_.insert(newTheta.zeroVectors());
70 
71  // Add the new factors to the graph, updating the variable index
72  insertFactors(newFactors);
73 
74  if(removeFactorIndices)
75  removeFactors(*removeFactorIndices);
76  }
77  gttoc(augment_system);
78 
79  if(factors_.size() > 0) {
80  // Reorder the system to ensure efficient optimization (and marginalization) performance
81  gttic(reorder);
82  reorder();
83  gttoc(reorder);
84 
85  // Optimize the factors using a modified version of L-M
86  gttic(optimize);
87  result = optimize();
88  gttoc(optimize);
89  }
90 
91  // TODO: The following code does considerable work, much of which could be redundant given the previous optimization step
92  // Refactor this code to reduce computational burden
93 
94  // Calculate the marginal on the separator from the smoother factors
95  if(separatorValues_.size() > 0) {
96  gttic(presync);
98  gttoc(presync);
99  }
100 
101  gttoc(update);
102 
103  return result;
104 }
105 
106 /* ************************************************************************* */
108 
109  gttic(presync);
110 
111  gttoc(presync);
112 }
113 
114 /* ************************************************************************* */
116 
117  gttic(get_summarized_factors);
118 
119  // Copy the previous calculated smoother summarization factors into the output
120  summarizedFactors.push_back(smootherSummarization_);
121 
122  // Copy the separator values into the output
123  separatorValues.insert(separatorValues_);
124 
125  gttoc(get_summarized_factors);
126 }
127 
128 /* ************************************************************************* */
129 void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
130  const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) {
131 
133 
134  // Remove the previous filter summarization from the graph
136 
137  // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
138  for(const auto key: smootherValues.keys()) {
139  if(!theta_.exists(key)) {
140  // If this a new key for theta_, also add to ordering and delta.
141  const auto& value = smootherValues.at(key);
142  delta_.insert(key, Vector::Zero(value.dim()));
144  ordering_.push_back(key);
145  } else {
146  // If the key already existed in theta_, just update.
147  const auto& value = smootherValues.at(key);
149  }
150  }
151  for(const auto key: separatorValues.keys()) {
152  if(!theta_.exists(key)) {
153  // If this a new key for theta_, also add to ordering and delta.
154  const auto& value = separatorValues.at(key);
155  delta_.insert(key, Vector::Zero(value.dim()));
157  ordering_.push_back(key);
158  } else {
159  // If the key already existed in theta_, just update.
160  const auto& value = separatorValues.at(key);
162  }
163  }
164 
165  // Insert the new smoother factors
166  insertFactors(smootherFactors);
167 
168  // Insert the new filter summarized factors
169  filterSummarizationSlots_ = insertFactors(summarizedFactors);
170 
171  // Update the list of root keys
172  separatorValues_ = separatorValues;
173 
175 }
176 
177 /* ************************************************************************* */
179 
180  gttic(postsync);
181 
182  gttoc(postsync);
183 }
184 
185 /* ************************************************************************* */
187 
188  gttic(insert_factors);
189 
190  // create the output vector
191  std::vector<size_t> slots;
192  slots.reserve(factors.size());
193 
194  // Insert the factor into an existing hole in the factor graph, if possible
195  for(const NonlinearFactor::shared_ptr& factor: factors) {
196  size_t slot;
197  if(availableSlots_.size() > 0) {
198  slot = availableSlots_.front();
199  availableSlots_.pop();
200  factors_.replace(slot, factor);
201  } else {
202  slot = factors_.size();
203  factors_.push_back(factor);
204  }
205  slots.push_back(slot);
206  }
207 
208  gttoc(insert_factors);
209 
210  return slots;
211 }
212 
213 /* ************************************************************************* */
214 void ConcurrentBatchSmoother::removeFactors(const std::vector<size_t>& slots) {
215 
216  gttic(remove_factors);
217 
218  // For each factor slot to delete...
219  for(size_t slot: slots) {
220 
221  // Remove the factor from the graph
222  factors_.remove(slot);
223 
224  // Mark the factor slot as available
225  availableSlots_.push(slot);
226  }
227 
228  gttoc(remove_factors);
229 }
230 
231 /* ************************************************************************* */
233 
234  // Recalculate the variable index
236 
237  KeyVector separatorKeys = separatorValues_.keys();
238  ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, KeyVector(separatorKeys.begin(), separatorKeys.end()));
239 
240 }
241 
242 /* ************************************************************************* */
244 
245  // Create output result structure
246  Result result;
247  result.nonlinearVariables = theta_.size() - separatorValues_.size();
248  result.linearVariables = separatorValues_.size();
249 
250  // Pull out parameters we'll use
253 
254  // Create a Values that holds the current evaluation point
255  Values evalpoint = theta_.retract(delta_);
256  result.error = factors_.error(evalpoint);
257  if(result.error < parameters_.errorTol) {
258  return result;
259  }
260 
261  // Use a custom optimization loop so the linearization points can be controlled
262  double previousError;
263  VectorValues newDelta;
264  do {
265  previousError = result.error;
266 
267  // Do next iteration
268  gttic(optimizer_iteration);
269  {
270  // Linearize graph around the linearization point
271  GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_);
272 
273  // Keep increasing lambda until we make make progress
274  while(true) {
275  if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
276  std::cout << "trying lambda = " << lambda << std::endl;
277 
278  // Add prior factors at the current solution
279  gttic(damp);
280  GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
281  dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size());
282  {
283  // for each of the variables, add a prior at the current solution
284  for(const VectorValues::KeyValuePair& key_value: delta_) {
285  size_t dim = key_value.second.size();
286  Matrix A = Matrix::Identity(dim,dim);
287  Vector b = key_value.second;
289  GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model));
290  dampedFactorGraph.push_back(prior);
291  }
292  }
293  gttoc(damp);
294  if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
295  dampedFactorGraph.print("damped");
296  result.lambdas++;
297 
298  gttic(solve);
299  // Solve Damped Gaussian Factor Graph
300  newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction());
301  // update the evalpoint with the new delta
302  evalpoint = theta_.retract(newDelta);
303  gttoc(solve);
304 
305  if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
306  std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
307  if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
308  newDelta.print("delta");
309 
310  // Evaluate the new error
311  gttic(compute_error);
312  double error = factors_.error(evalpoint);
313  gttoc(compute_error);
314 
315  if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
316  std::cout << "next error = " << error << std::endl;
317 
318  if(error < result.error) {
319  // Keep this change
320  // Update the error value
321  result.error = error;
322  // Update the linearization point
323  theta_ = evalpoint;
324  // Reset the deltas to zeros
325  delta_.setZero();
326  // Put the linearization points and deltas back for specific variables
327  if(separatorValues_.size() > 0) {
329  for(const auto key: separatorValues_.keys()) {
330  delta_.at(key) = newDelta.at(key);
331  }
332  }
333 
334  // Decrease lambda for next time
336  // End this lambda search iteration
337  break;
338  } else {
339  // Reject this change
341  // The maximum lambda has been used. Print a warning and end the search.
342  std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
343  break;
344  } else {
345  // Increase lambda and continue searching
347  }
348  }
349  } // end while
350  }
351  gttoc(optimizer_iteration);
352 
353  if (lmVerbosity >= LevenbergMarquardtParams::LAMBDA)
354  std::cout << "using lambda = " << lambda << std::endl;
355 
356  result.iterations++;
357  } while(result.iterations < (size_t)parameters_.maxIterations &&
359 
360  return result;
361 }
362 
363 /* ************************************************************************* */
365 
366  // The smoother summarization factors are the resulting marginal factors on the separator
367  // variables that result from marginalizing out all of the other variables
368  // These marginal factors will be cached for later transmission to the filter using
369  // linear container factors
370 
371  // Create a nonlinear factor graph without the filter summarization factors
373  for(size_t slot: filterSummarizationSlots_) {
374  graph.remove(slot);
375  }
376 
377  // Get the set of separator keys
378  const KeySet separatorKeys = separatorValues_.keySet();
379 
380  // Calculate the marginal factors on the separator
382 }
383 
384 /* ************************************************************************* */
385 void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) {
386  std::cout << indent;
387  if(factor) {
388  if(std::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
389  std::cout << "l( ";
390  } else {
391  std::cout << "f( ";
392  }
393  for(Key key: *factor) {
394  std::cout << keyFormatter(key) << " ";
395  }
396  std::cout << ")" << std::endl;
397  } else {
398  std::cout << "{ nullptr }" << std::endl;
399  }
400 }
401 
402 /* ************************************************************************* */
403 void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) {
404  std::cout << indent;
405  if(factor) {
406  std::cout << "g( ";
407  for(Key key: *factor) {
408  std::cout << keyFormatter(key) << " ";
409  }
410  std::cout << ")" << std::endl;
411  } else {
412  std::cout << "{ nullptr }" << std::endl;
413  }
414 }
415 
416 /* ************************************************************************* */
417 }
gtsam::VectorValues::print
void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
Definition: VectorValues.cpp:153
gtsam::ConcurrentBatchSmoother::getSummarizedFactors
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
Definition: ConcurrentBatchSmoother.cpp:115
gttoc
#define gttoc(label)
Definition: timing.h:296
timing.h
Timing utilities.
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Definition: inference/Ordering.h:112
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
gtsam::NonlinearFactorGraph::equals
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Definition: NonlinearFactorGraph.cpp:97
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
Definition: NonlinearOptimizerParams.h:44
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:218
gtsam::ConcurrentBatchSmoother::PrintNonlinearFactor
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
Definition: ConcurrentBatchSmoother.cpp:385
gtsam::ConcurrentBatchSmoother::ordering_
Ordering ordering_
The current ordering used to calculate the linear deltas.
Definition: ConcurrentBatchSmoother.h:160
gtsam::Values::size
size_t size() const
Definition: Values.h:178
ConcurrentBatchSmoother.h
A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoothing interface...
s
RealScalar s
Definition: level1_cplx_impl.h:126
LinearContainerFactor.h
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
gtsam::VectorValues::at
Vector & at(Key j)
Definition: VectorValues.h:142
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition: FactorGraph-inst.h:37
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::NonlinearOptimizerParams::SILENT
@ SILENT
Definition: NonlinearOptimizerParams.h:39
gtsam::ConcurrentBatchSmoother
Definition: ConcurrentBatchSmoother.h:31
gtsam::ConcurrentBatchSmoother::optimize
Result optimize()
Definition: ConcurrentBatchSmoother.cpp:243
gtsam::ConcurrentBatchSmoother::updateSmootherSummarization
void updateSmootherSummarization()
Definition: ConcurrentBatchSmoother.cpp:364
gtsam::ConcurrentBatchSmoother::variableIndex_
VariableIndex variableIndex_
The current variable index, which allows efficient factor lookup by variable.
Definition: ConcurrentBatchSmoother.h:162
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::ConcurrentBatchSmoother::parameters_
LevenbergMarquardtParams parameters_
LM parameters.
Definition: ConcurrentBatchSmoother.h:157
gtsam::FastSet< Key >
GaussianJunctionTree.h
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
gtsam::ConcurrentBatchSmoother::print
void print(const std::string &s="Concurrent Batch Smoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: ConcurrentBatchSmoother.cpp:28
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::LevenbergMarquardtParams::TRYDELTA
@ TRYDELTA
Definition: LevenbergMarquardtParams.h:40
gtsam::ConcurrentBatchSmoother::removeFactors
void removeFactors(const std::vector< size_t > &slots)
Definition: ConcurrentBatchSmoother.cpp:214
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Definition: NonlinearOptimizer.cpp:182
gtsam::ConcurrentBatchSmoother::delta_
VectorValues delta_
The current set of linear deltas from the linearization point.
Definition: ConcurrentBatchSmoother.h:161
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
A
Definition: test_numpy_dtypes.cpp:298
gtsam::ConcurrentBatchSmoother::theta_
Values theta_
Current linearization point of all variables in the smoother.
Definition: ConcurrentBatchSmoother.h:159
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::internal::calculateMarginalFactors
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
Definition: ConcurrentFilteringAndSmoothing.cpp:54
gtsam::ConcurrentBatchSmoother::update
virtual Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< std::vector< size_t > > &removeFactorIndices={})
Definition: ConcurrentBatchSmoother.cpp:49
gtsam::ConcurrentBatchSmoother::Result
Definition: ConcurrentBatchSmoother.h:38
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::ConcurrentSmoother
Definition: ConcurrentFilteringAndSmoothing.h:101
gtsam::ConcurrentBatchSmoother::factors_
NonlinearFactorGraph factors_
The set of all factors currently in the smoother.
Definition: ConcurrentBatchSmoother.h:158
gtsam::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
Definition: FactorGraph.h:374
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::LevenbergMarquardtParams::TRYLAMBDA
@ TRYLAMBDA
Definition: LevenbergMarquardtParams.h:40
gtsam::Values::equals
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
gtsam::ConcurrentBatchSmoother::PrintLinearFactor
static void PrintLinearFactor(const GaussianFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
Definition: ConcurrentBatchSmoother.cpp:403
gtsam::ConcurrentBatchSmoother::postsync
void postsync() override
Definition: ConcurrentBatchSmoother.cpp:178
gtsam::LevenbergMarquardtParams::lambdaUpperBound
double lambdaUpperBound
The maximum lambda to try before assuming the optimization has failed (default: 1e5)
Definition: LevenbergMarquardtParams.h:51
gtsam::ConcurrentBatchSmoother::synchronize
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
Definition: ConcurrentBatchSmoother.cpp:129
gtsam::LevenbergMarquardtParams::lambdaInitial
double lambdaInitial
The initial Levenberg-Marquardt damping term (default: 1e-5)
Definition: LevenbergMarquardtParams.h:49
gtsam::Ordering::equals
bool equals(const Ordering &other, double tol=1e-9) const
Definition: Ordering.cpp:308
gtsam::VariableIndex
Definition: VariableIndex.h:41
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
lambda
static double lambda[]
Definition: jv.c:524
gtsam::VectorValues::norm
double norm() const
Definition: VectorValues.cpp:255
key
const gtsam::Symbol key('X', 0)
gtsam::FactorGraph::remove
void remove(size_t i)
Definition: FactorGraph.h:371
gtsam::ConcurrentBatchSmoother::filterSummarizationSlots_
std::vector< size_t > filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
Definition: ConcurrentBatchSmoother.h:165
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Definition: Values.cpp:260
gtsam::b
const G & b
Definition: Group.h:79
gtsam::ConcurrentBatchSmoother::reorder
void reorder()
Definition: ConcurrentBatchSmoother.cpp:232
gtsam::Values::keySet
KeySet keySet() const
Definition: Values.cpp:227
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam::VectorValues::equals
bool equals(const VectorValues &x, double tol=1e-9) const
Definition: VectorValues.cpp:161
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
gtsam::LevenbergMarquardtParams::LAMBDA
@ LAMBDA
Definition: LevenbergMarquardtParams.h:40
gtsam::LevenbergMarquardtParams::VerbosityLM
VerbosityLM
Definition: LevenbergMarquardtParams.h:39
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::ConcurrentBatchSmoother::equals
bool equals(const ConcurrentSmoother &rhs, double tol=1e-9) const override
Definition: ConcurrentBatchSmoother.cpp:38
gtsam::Values
Definition: Values.h:65
gtsam::ConcurrentBatchSmoother::insertFactors
std::vector< size_t > insertFactors(const NonlinearFactorGraph &factors)
Definition: ConcurrentBatchSmoother.cpp:186
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::VectorValues::size
size_t size() const
Definition: VectorValues.h:130
gtsam::ConcurrentBatchSmoother::smootherSummarization_
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
Definition: ConcurrentBatchSmoother.h:168
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition: NonlinearOptimizerParams.h:43
gtsam::ConcurrentBatchSmoother::separatorValues_
Values separatorValues_
The linearization points of the separator variables. These should not be updated during optimization.
Definition: ConcurrentBatchSmoother.h:164
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition: NonlinearOptimizerParams.h:42
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::NonlinearOptimizerParams::errorTol
double errorTol
The maximum total error to stop iterating (default 0.0)
Definition: NonlinearOptimizerParams.h:45
gtsam::LevenbergMarquardtParams::lambdaFactor
double lambdaFactor
The amount by which to multiply or divide lambda when adjusting lambda (default: 10....
Definition: LevenbergMarquardtParams.h:50
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:238
gtsam::FactorGraph::reserve
void reserve(size_t size)
Definition: FactorGraph.h:143
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: GaussianFactorGraph.cpp:309
gtsam::VectorValues::KeyValuePair
value_type KeyValuePair
Typedef to pair<Key, Vector>
Definition: VectorValues.h:88
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::LevenbergMarquardtParams::verbosityLM
VerbosityLM verbosityLM
The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
Definition: LevenbergMarquardtParams.h:53
gtsam::ConcurrentBatchSmoother::presync
void presync() override
Definition: ConcurrentBatchSmoother.cpp:107
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::VectorValues::setZero
void setZero()
Definition: VectorValues.cpp:131
gtsam::NonlinearOptimizerParams::getEliminationFunction
GaussianFactorGraph::Eliminate getEliminationFunction() const
Definition: NonlinearOptimizerParams.h:137
test_callbacks.value
value
Definition: test_callbacks.py:160
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::LevenbergMarquardtParams::DAMPED
@ DAMPED
Definition: LevenbergMarquardtParams.h:40
debug.h
Global debugging flags.
gtsam::ConcurrentBatchSmoother::availableSlots_
std::queue< size_t > availableSlots_
The set of available factor graph slots caused by deleting factors.
Definition: ConcurrentBatchSmoother.h:163


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