Loading...
Searching...
No Matches
HybridNonlinearISAM.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
namespace gtsam {
class GTSAM_EXPORT HybridNonlinearISAM {
protected:
gtsam::HybridGaussianISAM isam_;
Values linPoint_;
DiscreteValues assignment_;
HybridNonlinearFactorGraph factors_;
int reorderInterval_;
int reorderCounter_;
HybridGaussianFactorGraph::Eliminate eliminationFunction_;
public:
HybridNonlinearISAM(
int reorderInterval = 1,
const HybridGaussianFactorGraph::Eliminate& eliminationFunction =
HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
: reorderInterval_(reorderInterval),
reorderCounter_(0),
eliminationFunction_(eliminationFunction) {}
Values estimate();
// /** find the marginal covariance for a single variable */
// Matrix marginalCovariance(Key key) const;
// access
const HybridGaussianISAM& bayesTree() const { return isam_; }
void prune(const size_t maxNumberLeaves) { isam_.prune(maxNumberLeaves); }
const Values& getLinearizationPoint() const { return linPoint_; }
const DiscreteValues& assignment() const { return assignment_; }
const HybridNonlinearFactorGraph& getFactorsUnsafe() const {
return factors_;
}
int reorderInterval() const { return reorderInterval_; }
int reorderCounter() const { return reorderCounter_; }
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
void printStats() const;
void saveGraph(const std::string& s,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
void update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none);
void reorder_relinearize();
};
} // namespace gtsam
HybridNonlinearFactorGraph.h
Nonlinear hybrid factor graph that uses type erasure.
HybridGaussianISAM.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
gtsam::HybridGaussianISAM
Definition HybridGaussianISAM.h:34
gtsam::HybridNonlinearFactorGraph
Definition HybridNonlinearFactorGraph.h:33
gtsam::HybridNonlinearISAM::getFactorsUnsafe
const HybridNonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition HybridNonlinearISAM.h:96
gtsam::HybridNonlinearISAM::assignment_
DiscreteValues assignment_
The discrete assignment.
Definition HybridNonlinearISAM.h:36
gtsam::HybridNonlinearISAM::getLinearizationPoint
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition HybridNonlinearISAM.h:90
gtsam::HybridNonlinearISAM::HybridNonlinearISAM
HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph::Eliminate &eliminationFunction=HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition HybridNonlinearISAM.h:59
gtsam::HybridNonlinearISAM::factors_
HybridNonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition HybridNonlinearISAM.h:39
gtsam::HybridNonlinearISAM::assignment
const DiscreteValues & assignment() const
Return the current discrete assignment.
Definition HybridNonlinearISAM.h:93
gtsam::HybridNonlinearISAM::reorderCounter
int reorderCounter() const
TODO: comment.
Definition HybridNonlinearISAM.h:102
gtsam::HybridNonlinearISAM::prune
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition HybridNonlinearISAM.h:87
gtsam::HybridNonlinearISAM::reorderInterval_
int reorderInterval_
The reordering interval and counter.
Definition HybridNonlinearISAM.h:42
gtsam::HybridNonlinearISAM::reorderInterval
int reorderInterval() const
get counters
Definition HybridNonlinearISAM.h:101
gtsam::HybridNonlinearISAM::linPoint_
Values linPoint_
The current linearization point.
Definition HybridNonlinearISAM.h:33
gtsam::HybridNonlinearISAM::isam_
gtsam::HybridGaussianISAM isam_
The internal iSAM object.
Definition HybridNonlinearISAM.h:30
gtsam::HybridNonlinearISAM::eliminationFunction_
HybridGaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition HybridNonlinearISAM.h:46
gtsam::HybridNonlinearISAM::bayesTree
const HybridGaussianISAM & bayesTree() const
find the marginal covariance for a single variable
Definition HybridNonlinearISAM.h:80
gtsam::EliminateableFactorGraph< HybridGaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
Definition EliminateableFactorGraph.h:89
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65