Loading...
Searching...
No Matches
ISAM2.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
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/ISAM2Result.h>
#include <gtsam/nonlinear/ISAM2UpdateParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <vector>
namespace gtsam {
class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
protected:
Values theta_;
VariableIndex variableIndex_;
mutable VectorValues delta_;
mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores
// the Gauss-Newton update
mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and
// is updated incrementally
mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in
// the right way
NonlinearFactorGraph nonlinearFactors_;
mutable GaussianFactorGraph linearFactors_;
ISAM2Params params_;
mutable boost::optional<double> doglegDelta_;
KeySet fixedVariables_;
int update_count_;
public:
using This = ISAM2;
using Base = BayesTree<ISAM2Clique>;
using Clique = Base::Clique;
using sharedClique = Base::sharedClique;
using Cliques = Base::Cliques;
explicit ISAM2(const ISAM2Params& params);
ISAM2();
virtual ~ISAM2() {}
virtual bool equals(const ISAM2& other, double tol = 1e-9) const;
virtual ISAM2Result update(
const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const FactorIndices& removeFactorIndices = FactorIndices(),
const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
bool force_relinearize = false);
virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
const Values& newTheta,
const ISAM2UpdateParams& updateParams);
void marginalizeLeaves(
const FastList<Key>& leafKeys,
boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
const Values& getLinearizationPoint() const { return theta_; }
bool valueExists(Key key) const { return theta_.exists(key); }
Values calculateEstimate() const;
template <class VALUE>
VALUE calculateEstimate(Key key) const {
const Vector& delta = getDelta()[key];
return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}
const Value& calculateEstimate(Key key) const;
Matrix marginalCovariance(Key key) const;
Values calculateBestEstimate() const;
const VectorValues& getDelta() const;
double error(const VectorValues& x) const;
const NonlinearFactorGraph& getFactorsUnsafe() const {
return nonlinearFactors_;
}
const VariableIndex& getVariableIndex() const { return variableIndex_; }
const KeySet& getFixedVariables() const { return fixedVariables_; }
const ISAM2Params& params() const { return params_; }
void printStats() const { getCliqueData().getStats().print(); }
VectorValues gradientAtZero() const;
protected:
void recalculate(const ISAM2UpdateParams& updateParams,
const KeySet& relinKeys, ISAM2Result* result);
// Do a batch step - reorder and relinearize all variables
void recalculateBatch(const ISAM2UpdateParams& updateParams,
KeySet* affectedKeysSet, ISAM2Result* result);
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
GaussianFactorGraph relinearizeAffectedFactors(
const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
const KeySet& relinKeys);
void recalculateIncremental(const ISAM2UpdateParams& updateParams,
const KeySet& relinKeys,
const FastList<Key>& affectedKeys,
KeySet* affectedKeysSet, Cliques* orphans,
ISAM2Result* result);
void addVariables(const Values& newTheta,
ISAM2Result::DetailedResults* detail = 0);
void removeVariables(const KeySet& unusedKeys);
void updateDelta(bool forceFullSolve = false) const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
ar & BOOST_SERIALIZATION_NVP(theta_);
ar & BOOST_SERIALIZATION_NVP(variableIndex_);
ar & BOOST_SERIALIZATION_NVP(delta_);
ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
ar & BOOST_SERIALIZATION_NVP(RgProd_);
ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
ar & BOOST_SERIALIZATION_NVP(linearFactors_);
ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
ar & BOOST_SERIALIZATION_NVP(update_count_);
}
}; // ISAM2
template <>
struct traits<ISAM2> : public Testable<ISAM2> {};
} // namespace gtsam
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
ISAM2UpdateParams.h
Class that stores extra params for ISAM2::update().
ISAM2Clique.h
Specialized iSAM2 Clique.
ISAM2Result.h
Class that stores detailed iSAM2 result.
ISAM2Params.h
Parameters for iSAM 2.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
gtsam::BayesTree< ISAM2Clique >::sharedClique
boost::shared_ptr< Clique > sharedClique
Definition BayesTree.h:74
gtsam::BayesTree< ISAM2Clique >::BayesTree
BayesTree()
Definition BayesTree.h:109
gtsam::BayesTree< ISAM2Clique >::Cliques
FastList< sharedClique > Cliques
Definition BayesTree.h:89
gtsam::BayesTree< ISAM2Clique >::Clique
ISAM2Clique Clique
Definition BayesTree.h:73
gtsam::BayesTree< ISAM2Clique >::getCliqueData
BayesTreeCliqueData getCliqueData() const
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::ISAM2
Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
Definition ISAM2.h:45
gtsam::ISAM2::fixedVariables_
KeySet fixedVariables_
Set of variables that are involved with linear factors from marginalized variables and thus cannot ha...
Definition ISAM2.h:94
gtsam::ISAM2::deltaReplacedMask_
KeySet deltaReplacedMask_
A cumulative mask for the variables that were replaced and have not yet been updated in the linear so...
Definition ISAM2.h:76
gtsam::ISAM2::update_count_
int update_count_
Counter incremented every update(), used to determine periodic relinearization.
Definition ISAM2.h:96
gtsam::ISAM2::Base
BayesTree< ISAM2Clique > Base
The BayesTree base class.
Definition ISAM2.h:101
gtsam::ISAM2::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition ISAM2.h:224
gtsam::ISAM2::~ISAM2
virtual ~ISAM2()
default virtual destructor
Definition ISAM2.h:114
gtsam::ISAM2::getFixedVariables
const KeySet & getFixedVariables() const
Access the nonlinear variable index.
Definition ISAM2.h:265
gtsam::ISAM2::getDelta
const VectorValues & getDelta() const
Access the current delta, computed during the last call to update.
Definition ISAM2.cpp:807
gtsam::ISAM2::Cliques
Base::Cliques Cliques
List of Cliques.
Definition ISAM2.h:104
gtsam::ISAM2::Clique
Base::Clique Clique
A clique.
Definition ISAM2.h:102
gtsam::ISAM2::printStats
void printStats() const
prints out clique statistics
Definition ISAM2.h:270
gtsam::ISAM2::nonlinearFactors_
NonlinearFactorGraph nonlinearFactors_
All original nonlinear factors are stored here to use during relinearization.
Definition ISAM2.h:81
gtsam::ISAM2::getVariableIndex
const VariableIndex & getVariableIndex() const
Access the nonlinear variable index.
Definition ISAM2.h:262
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition ISAM2.h:103
gtsam::ISAM2::delta_
VectorValues delta_
The linear delta from the last linear solution, an update to the estimate in theta.
Definition ISAM2.h:61
gtsam::ISAM2::ISAM2
ISAM2(const ISAM2Params &params)
Create an empty ISAM2 instance.
Definition ISAM2.cpp:40
gtsam::ISAM2::variableIndex_
VariableIndex variableIndex_
VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
Definition ISAM2.h:52
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition ISAM2.h:205
gtsam::ISAM2::theta_
Values theta_
The current linearization point.
Definition ISAM2.h:48
gtsam::ISAM2::params_
ISAM2Params params_
The current parameters.
Definition ISAM2.h:87
gtsam::ISAM2::doglegDelta_
boost::optional< double > doglegDelta_
The current Dogleg Delta (trust region radius).
Definition ISAM2.h:90
gtsam::ISAM2::linearFactors_
GaussianFactorGraph linearFactors_
The current linear factors, which are only updated as needed.
Definition ISAM2.h:84
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Access the set of nonlinear factors.
Definition ISAM2.h:257
gtsam::ISAM2::This
ISAM2 This
This class.
Definition ISAM2.h:100
gtsam::ISAM2::valueExists
bool valueExists(Key key) const
Check whether variable with given key exists in linearization point.
Definition ISAM2.h:208
gtsam::ISAM2Params
Definition ISAM2Params.h:135
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
gtsam::ISAM2Result::DetailedResults
A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:117
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65