12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleaned up cpplint errors - no functionality change

release/4.3a0
Frank Dellaert 2018-09-29 00:06:19 -04:00
parent 224299ccb9
commit e6c0d7334f
2 changed files with 401 additions and 243 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/ISAM2.cpp
View File

@ -247,7 +247,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) {
GaussianFactorGraph cachedBoundary;
for(sharedClique orphan: orphans) {

604
gtsam/nonlinear/ISAM2.h
View File

@ -11,7 +11,8 @@
/**
* @file ISAM2.h
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid
* relinearization.
* @author Michael Kaess, Richard Roberts
*/
@ -19,9 +20,12 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <string>
#include <vector>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <boost/variant.hpp>
@ -34,21 +38,29 @@ namespace gtsam {
* ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&).
*/
struct GTSAM_EXPORT ISAM2GaussNewtonParams {
double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001)
double
wildfireThreshold; ///< Continue updating the linear delta only when
///< changes are above this threshold (default: 0.001)
/** Specify parameters as constructor arguments */
ISAM2GaussNewtonParams(
double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold
) : wildfireThreshold(_wildfireThreshold) {}
double _wildfireThreshold =
0.001 ///< see ISAM2GaussNewtonParams public variables,
///< ISAM2GaussNewtonParams::wildfireThreshold
)
: wildfireThreshold(_wildfireThreshold) {}
void print(const std::string str = "") const {
std::cout << str << "type: ISAM2GaussNewtonParams\n";
std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
std::cout.flush();
using std::cout;
cout << str << "type: ISAM2GaussNewtonParams\n";
cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
cout.flush();
}
double getWildfireThreshold() const { return wildfireThreshold; }
void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
void setWildfireThreshold(double wildfireThreshold) {
this->wildfireThreshold = wildfireThreshold;
}
};
/**
@ -59,50 +71,80 @@ struct GTSAM_EXPORT ISAM2GaussNewtonParams {
*/
struct GTSAM_EXPORT ISAM2DoglegParams {
double initialDelta; ///< The initial trust region radius for Dogleg
double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode
bool verbose; ///< Whether Dogleg prints iteration and convergence information
double
wildfireThreshold; ///< Continue updating the linear delta only when
///< changes are above this threshold (default: 1e-5)
DoglegOptimizerImpl::TrustRegionAdaptationMode
adaptationMode; ///< See description in
///< DoglegOptimizerImpl::TrustRegionAdaptationMode
bool
verbose; ///< Whether Dogleg prints iteration and convergence information
/** Specify parameters as constructor arguments */
ISAM2DoglegParams(
double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta
double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold
DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode
double _wildfireThreshold =
1e-5, ///< see ISAM2DoglegParams::wildfireThreshold
DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode =
DoglegOptimizerImpl::
SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode
bool _verbose = false ///< see ISAM2DoglegParams::verbose
) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold),
adaptationMode(_adaptationMode), verbose(_verbose) {}
)
: initialDelta(_initialDelta),
wildfireThreshold(_wildfireThreshold),
adaptationMode(_adaptationMode),
verbose(_verbose) {}
void print(const std::string str = "") const {
std::cout << str << "type: ISAM2DoglegParams\n";
std::cout << str << "initialDelta: " << initialDelta << "\n";
std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
std::cout << str << "adaptationMode: " << adaptationModeTranslator(adaptationMode) << "\n";
std::cout.flush();
using std::cout;
cout << str << "type: ISAM2DoglegParams\n";
cout << str << "initialDelta: " << initialDelta << "\n";
cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
cout << str
<< "adaptationMode: " << adaptationModeTranslator(adaptationMode)
<< "\n";
cout.flush();
}
double getInitialDelta() const { return initialDelta; }
double getWildfireThreshold() const { return wildfireThreshold; }
std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); };
bool isVerbose() const { return verbose; };
std::string getAdaptationMode() const {
return adaptationModeTranslator(adaptationMode);
}
bool isVerbose() const { return verbose; }
void setInitialDelta(double initialDelta) {
this->initialDelta = initialDelta;
}
void setWildfireThreshold(double wildfireThreshold) {
this->wildfireThreshold = wildfireThreshold;
}
void setAdaptationMode(const std::string& adaptationMode) {
this->adaptationMode = adaptationModeTranslator(adaptationMode);
}
void setVerbose(bool verbose) { this->verbose = verbose; }
void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; }
void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); }
void setVerbose(bool verbose) { this->verbose = verbose; };
std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const;
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const;
std::string adaptationModeTranslator(
const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode)
const;
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(
const std::string& adaptationMode) const;
};
/**
* @addtogroup ISAM2
* Parameters for the ISAM2 algorithm. Default parameter values are listed below.
* Parameters for the ISAM2 algorithm. Default parameter values are listed
* below.
*/
typedef FastMap<char,Vector> ISAM2ThresholdMap;
typedef FastMap<char, Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct GTSAM_EXPORT ISAM2Params {
typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams
typedef boost::variant<double, FastMap<char,Vector> > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds
typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
OptimizationParams; ///< Either ISAM2GaussNewtonParams or
///< ISAM2DoglegParams
typedef boost::variant<double, FastMap<char, Vector> >
RelinearizationThreshold; ///< Either a constant relinearization
///< threshold or a per-variable-type set of
///< thresholds
/** Optimization parameters, this both selects the nonlinear optimization
* method and specifies its parameters, either ISAM2GaussNewtonParams or
@ -122,27 +164,36 @@ struct GTSAM_EXPORT ISAM2Params {
* entries would be added with:
* \code
FastMap<char,Vector> thresholds;
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished();
// 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] =
Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds;
\endcode
*/
RelinearizationThreshold relinearizeThreshold;
int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
int relinearizeSkip; ///< Only relinearize any variables every
///< relinearizeSkip calls to ISAM2::update (default:
///< 10)
bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true)
bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize
///< any variables (default: true)
bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error
///< before and after the update, to return in
///< ISAM2Result from update()
enum Factorization { CHOLESKY, QR };
/** Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
* Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when
* uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is
* slower but more numerically stable in poorly-conditioned problems. We suggest using the default of Cholesky
* unless gtsam sometimes throws IndefiniteLinearSystemException when your problem's Hessian is actually positive
* definite. For positive definite problems, numerical error accumulation can cause the problem to become
* numerically negative or indefinite as solving proceeds, especially when using Cholesky.
/** Specifies whether to use QR or CHOESKY numerical factorization (default:
* CHOLESKY). Cholesky is faster but potentially numerically unstable for
* poorly-conditioned problems, which can occur when uncertainty is very low
* in some variables (or dimensions of variables) and very high in others. QR
* is slower but more numerically stable in poorly-conditioned problems. We
* suggest using the default of Cholesky unless gtsam sometimes throws
* IndefiniteLinearSystemException when your problem's Hessian is actually
* positive definite. For positive definite problems, numerical error
* accumulation can cause the problem to become numerically negative or
* indefinite as solving proceeds, especially when using Cholesky.
*/
Factorization factorization;
@ -153,92 +204,151 @@ struct GTSAM_EXPORT ISAM2Params {
*/
bool cacheLinearizedFactors;
KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
KeyFormatter
keyFormatter; ///< A KeyFormatter for when keys are printed during
///< debugging (default: DefaultKeyFormatter)
bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false)
bool enableDetailedResults; ///< Whether to compute and return
///< ISAM2Result::detailedResults, this can
///< increase running time (default: false)
/** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false).
* This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate
* significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance
* is desired over correctness. Use with caution.
/** Check variables for relinearization in tree-order, stopping the check once
* a variable does not need to be relinearized (default: false). This can
* improve speed by only checking a small part of the top of the tree.
* However, variables below the check cut-off can accumulate significant
* deltas without triggering relinearization. This is particularly useful in
* exploration scenarios where real-time performance is desired over
* correctness. Use with caution.
*/
bool enablePartialRelinearizationCheck;
/// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to
/// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of
/// having to search for slots every time a factor is added.
/// When you will be removing many factors, e.g. when using ISAM2 as a
/// fixed-lag smoother, enable this option to add factors in the first
/// available factor slots, to avoid accumulating NULL factor slots, at the
/// cost of having to search for slots every time a factor is added.
bool findUnusedFactorSlots;
/** Specify parameters as constructor arguments */
ISAM2Params(
OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold
int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization
bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors
const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
enableDetailedResults(false), enablePartialRelinearizationCheck(false),
/**
* Specify parameters as constructor arguments
* See the documentation of member variables above.
*/
ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(),
RelinearizationThreshold _relinearizeThreshold = 0.1,
int _relinearizeSkip = 10, bool _enableRelinearization = true,
bool _evaluateNonlinearError = false,
Factorization _factorization = ISAM2Params::CHOLESKY,
bool _cacheLinearizedFactors = true,
const KeyFormatter& _keyFormatter =
DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
)
: optimizationParams(_optimizationParams),
relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip),
enableRelinearization(_enableRelinearization),
evaluateNonlinearError(_evaluateNonlinearError),
factorization(_factorization),
cacheLinearizedFactors(_cacheLinearizedFactors),
keyFormatter(_keyFormatter),
enableDetailedResults(false),
enablePartialRelinearizationCheck(false),
findUnusedFactorSlots(false) {}
/// print iSAM2 parameters
void print(const std::string& str = "") const {
std::cout << str << "\n";
if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
boost::get<ISAM2GaussNewtonParams>(optimizationParams).print("optimizationParams: ");
else if(optimizationParams.type() == typeid(ISAM2DoglegParams))
boost::get<ISAM2DoglegParams>(optimizationParams).print("optimizationParams: ");
using std::cout;
cout << str << "\n";
static const std::string kStr("optimizationParams: ");
if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
boost::get<ISAM2GaussNewtonParams>(optimizationParams).print();
else if (optimizationParams.type() == typeid(ISAM2DoglegParams))
boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
else
std::cout << "optimizationParams: " << "{unknown type}" << "\n";
if(relinearizeThreshold.type() == typeid(double))
std::cout << "relinearizeThreshold: " << boost::get<double>(relinearizeThreshold) << "\n";
else
{
std::cout << "relinearizeThreshold: " << "{mapped}" << "\n";
for(const ISAM2ThresholdMapValue& value: boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n";
cout << kStr << "{unknown type}\n";
cout << "relinearizeThreshold: ";
if (relinearizeThreshold.type() == typeid(double)) {
cout << boost::get<double>(relinearizeThreshold) << "\n";
} else {
cout << "{mapped}\n";
for (const ISAM2ThresholdMapValue& value :
boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
cout << " '" << value.first
<< "' -> [" << value.second.transpose() << " ]\n";
}
}
std::cout << "relinearizeSkip: " << relinearizeSkip << "\n";
std::cout << "enableRelinearization: " << enableRelinearization << "\n";
std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n";
std::cout << "factorization: " << factorizationTranslator(factorization) << "\n";
std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n";
std::cout << "enableDetailedResults: " << enableDetailedResults << "\n";
std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n";
std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n";
std::cout.flush();
cout << "relinearizeSkip: " << relinearizeSkip << "\n";
cout << "enableRelinearization: " << enableRelinearization
<< "\n";
cout << "evaluateNonlinearError: " << evaluateNonlinearError
<< "\n";
cout << "factorization: "
<< factorizationTranslator(factorization) << "\n";
cout << "cacheLinearizedFactors: " << cacheLinearizedFactors
<< "\n";
cout << "enableDetailedResults: " << enableDetailedResults
<< "\n";
cout << "enablePartialRelinearizationCheck: "
<< enablePartialRelinearizationCheck << "\n";
cout << "findUnusedFactorSlots: " << findUnusedFactorSlots
<< "\n";
cout.flush();
}
/// @name Getters and Setters for all properties
/// @{
OptimizationParams getOptimizationParams() const { return this->optimizationParams; }
RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; }
OptimizationParams getOptimizationParams() const {
return this->optimizationParams;
}
RelinearizationThreshold getRelinearizeThreshold() const {
return relinearizeThreshold;
}
int getRelinearizeSkip() const { return relinearizeSkip; }
bool isEnableRelinearization() const { return enableRelinearization; }
bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
std::string getFactorization() const { return factorizationTranslator(factorization); }
std::string getFactorization() const {
return factorizationTranslator(factorization);
}
bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
KeyFormatter getKeyFormatter() const { return keyFormatter; }
bool isEnableDetailedResults() const { return enableDetailedResults; }
bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; }
bool isEnablePartialRelinearizationCheck() const {
return enablePartialRelinearizationCheck;
}
void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; }
void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; }
void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; }
void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; }
void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; }
void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); }
void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; }
void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; }
void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; }
void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; }
void setOptimizationParams(OptimizationParams optimizationParams) {
this->optimizationParams = optimizationParams;
}
void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
this->relinearizeThreshold = relinearizeThreshold;
}
void setRelinearizeSkip(int relinearizeSkip) {
this->relinearizeSkip = relinearizeSkip;
}
void setEnableRelinearization(bool enableRelinearization) {
this->enableRelinearization = enableRelinearization;
}
void setEvaluateNonlinearError(bool evaluateNonlinearError) {
this->evaluateNonlinearError = evaluateNonlinearError;
}
void setFactorization(const std::string& factorization) {
this->factorization = factorizationTranslator(factorization);
}
void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
this->cacheLinearizedFactors = cacheLinearizedFactors;
}
void setKeyFormatter(KeyFormatter keyFormatter) {
this->keyFormatter = keyFormatter;
}
void setEnableDetailedResults(bool enableDetailedResults) {
this->enableDetailedResults = enableDetailedResults;
}
void setEnablePartialRelinearizationCheck(
bool enablePartialRelinearizationCheck) {
this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
}
GaussianFactorGraph::Eliminate getEliminationFunction() const {
return factorization == CHOLESKY
@ -275,8 +385,9 @@ struct GTSAM_EXPORT ISAM2Result {
* delta, as computed by ISAM2::calculateEstimate().
* \li New variables will be evaluated at their initialization points passed
* into the current call to update.
* \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError
* is set to \c true, because there is some cost to this computation.
* \par Note: This will only be computed if
* ISAM2Params::evaluateNonlinearError is set to \c true, because there is
* some cost to this computation.
*/
boost::optional<double> errorBefore;
@ -286,8 +397,9 @@ struct GTSAM_EXPORT ISAM2Result {
* variables have undergone one linear update. Variable values are
* again computed by combining their linearization points with their
* partial linear deltas, by ISAM2::calculateEstimate().
* \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError
* is set to \c true, because there is some cost to this computation.
* \par Note: This will only be computed if
* ISAM2Params::evaluateNonlinearError is set to \c true, because there is
* some cost to this computation.
*/
boost::optional<double> errorAfter;
@ -309,7 +421,8 @@ struct GTSAM_EXPORT ISAM2Result {
*/
size_t variablesReeliminated;
/** The number of factors that were included in reelimination of the Bayes' tree. */
/** The number of factors that were included in reelimination of the Bayes'
* tree. */
size_t factorsRecalculated;
/** The number of cliques in the Bayes' Tree */
@ -332,14 +445,29 @@ struct GTSAM_EXPORT ISAM2Result {
* observed, new, or on the path up to the root clique from another
* reeliminated variable. */
bool isReeliminated;
bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold
bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold
bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement.
bool isObserved; ///< Whether the variable was just involved in new factors
bool isAboveRelinThreshold; ///< Whether the variable was just
///< relinearized due to being above the
///< relinearization threshold
bool isRelinearizeInvolved; ///< Whether the variable was below the
///< relinearization threshold but was
///< relinearized by being involved in a
///< factor with a variable above the
///< relinearization threshold
bool isRelinearized; /// Whether the variable was relinearized, either by
/// being above the relinearization threshold or by
/// involvement.
bool isObserved; ///< Whether the variable was just involved in new
///< factors
bool isNew; ///< Whether the variable itself was just added
bool inRootClique; ///< Whether the variable is in the root clique
VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false),
isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {}
VariableStatus()
: isReeliminated(false),
isAboveRelinThreshold(false),
isRelinearizeInvolved(false),
isRelinearized(false),
isObserved(false),
isNew(false),
inRootClique(false) {}
};
/** The status of each variable during this update, see VariableStatus.
@ -351,24 +479,27 @@ struct GTSAM_EXPORT ISAM2Result {
* Detail for information about the results data stored here. */
boost::optional<DetailedResults> detail;
void print(const std::string str = "") const {
std::cout << str << " Reelimintated: " << variablesReeliminated << " Relinearized: " << variablesRelinearized << " Cliques: " << cliques << std::endl;
using std::cout;
cout << str << " Reelimintated: " << variablesReeliminated
<< " Relinearized: " << variablesRelinearized
<< " Cliques: " << cliques << std::endl;
}
/** Getters and Setters */
size_t getVariablesRelinearized() const { return variablesRelinearized; };
size_t getVariablesReeliminated() const { return variablesReeliminated; };
size_t getCliques() const { return cliques; };
size_t getVariablesRelinearized() const { return variablesRelinearized; }
size_t getVariablesReeliminated() const { return variablesReeliminated; }
size_t getCliques() const { return cliques; }
};
/**
* Specialized Clique structure for ISAM2, incorporating caching and gradient contribution
* Specialized Clique structure for ISAM2, incorporating caching and gradient
* contribution
* TODO: more documentation
*/
class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph>
{
public:
class GTSAM_EXPORT ISAM2Clique
: public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
public:
typedef ISAM2Clique This;
typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
typedef boost::shared_ptr<This> shared_ptr;
@ -383,13 +514,16 @@ public:
/// Default constructor
ISAM2Clique() : Base() {}
/// Copy constructor, does *not* copy solution pointers as these are invalid in different trees.
ISAM2Clique(const ISAM2Clique& other) :
Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {}
/// Copy constructor, does *not* copy solution pointers as these are invalid
/// in different trees.
ISAM2Clique(const ISAM2Clique& other)
: Base(other),
cachedFactor_(other.cachedFactor_),
gradientContribution_(other.gradientContribution_) {}
/// Assignment operator, does *not* copy solution pointers as these are invalid in different trees.
ISAM2Clique& operator=(const ISAM2Clique& other)
{
/// Assignment operator, does *not* copy solution pointers as these are
/// invalid in different trees.
ISAM2Clique& operator=(const ISAM2Clique& other) {
Base::operator=(other);
cachedFactor_ = other.cachedFactor_;
gradientContribution_ = other.gradientContribution_;
@ -397,7 +531,8 @@ public:
}
/// Overridden to also store the remaining factor and gradient contribution
void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult);
void setEliminationResult(
const FactorGraphType::EliminationResult& eliminationResult);
/** Access the cached factor */
Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
@ -405,44 +540,45 @@ public:
/** Access the gradient contribution */
const Vector& gradientContribution() const { return gradientContribution_; }
bool equals(const This& other, double tol=1e-9) const;
bool equals(const This& other, double tol = 1e-9) const;
/** print this node */
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
private:
void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
ar & BOOST_SERIALIZATION_NVP(gradientContribution_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(cachedFactor_);
ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
}
}; // \struct ISAM2Clique
/**
* @addtogroup ISAM2
* Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
* Implementation of the full ISAM2 algorithm for incremental nonlinear
* optimization.
*
* The typical cycle of using this class to create an instance by providing ISAM2Params
* to the constructor, then add measurements and variables as they arrive using the update()
* method. At any time, calculateEstimate() may be called to obtain the current
* estimate of all variables.
* The typical cycle of using this class to create an instance by providing
* ISAM2Params to the constructor, then add measurements and variables as they
* arrive using the update() method. At any time, calculateEstimate() may be
* called to obtain the current estimate of all variables.
*
*/
class GTSAM_EXPORT ISAM2: public BayesTree<ISAM2Clique> {
protected:
class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
protected:
/** The current linearization point */
Values theta_;
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
/** VariableIndex lets us look up factors by involved variable and keeps track
* of dimensions */
VariableIndex variableIndex_;
/** The linear delta from the last linear solution, an update to the estimate in theta
/** The linear delta from the last linear solution, an update to the estimate
* in theta
*
* This is \c mutable because it is a "cached" variable - it is not updated
* until either requested with getDelta() or calculateEstimate(), or needed
@ -450,8 +586,10 @@ protected:
*/
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 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
/** A cumulative mask for the variables that were replaced and have not yet
* been updated in the linear solution delta_, this is only used internally,
@ -461,9 +599,11 @@ protected:
* This is \c mutable because it is used internally to not update delta_
* until it is needed.
*/
mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way
mutable KeySet
deltaReplacedMask_; // TODO: Make sure accessed in the right way
/** All original nonlinear factors are stored here to use during relinearization */
/** All original nonlinear factors are stored here to use during
* relinearization */
NonlinearFactorGraph nonlinearFactors_;
/** The current linear factors, which are only updated as needed */
@ -479,10 +619,10 @@ protected:
* variables and thus cannot have their linearization points changed. */
KeySet fixedVariables_;
int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization
public:
int update_count_; ///< Counter incremented every update(), used to determine
///< periodic relinearization
public:
typedef ISAM2 This; ///< This class
typedef BayesTree<ISAM2Clique> Base; ///< The BayesTree base class
typedef Base::Clique Clique; ///< A clique
@ -490,9 +630,10 @@ public:
typedef Base::Cliques Cliques; ///< List of Clique typedef from base class
/** Create an empty ISAM2 instance */
ISAM2(const ISAM2Params& params);
explicit ISAM2(const ISAM2Params& params);
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
/** Create an empty ISAM2 instance using the default set of parameters (see
* ISAM2Params) */
ISAM2();
/** default virtual destructor */
@ -513,26 +654,31 @@ public:
* thresholds.
*
* @param newFactors The new factors to be added to the system
* @param newTheta Initialization points for new variables to be added to the system.
* You must include here all new variables occuring in newFactors (which were not already
* in the system). There must not be any variables here that do not occur in newFactors,
* and additionally, variables that were already in the system must not be included here.
* @param newTheta Initialization points for new variables to be added to the
* system. You must include here all new variables occuring in newFactors
* (which were not already in the system). There must not be any variables
* here that do not occur in newFactors, and additionally, variables that were
* already in the system must not be included here.
* @param removeFactorIndices Indices of factors to remove from system
* @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
* large (Params::relinearizeThreshold), regardless of the relinearization interval
* (Params::relinearizeSkip).
* @param constrainedKeys is an optional map of keys to group labels, such that a variable can
* be constrained to a particular grouping in the BayesTree
* @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization
* point, regardless of the size of the linear delta
* @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless
* of the size of the linear delta. This allows the provided keys to be reordered.
* @param force_relinearize Relinearize any variables whose delta magnitude is
* sufficiently large (Params::relinearizeThreshold), regardless of the
* relinearization interval (Params::relinearizeSkip).
* @param constrainedKeys is an optional map of keys to group labels, such
* that a variable can be constrained to a particular grouping in the
* BayesTree
* @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will
* hold at a constant linearization point, regardless of the size of the
* linear delta
* @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will
* re-eliminate, regardless of the size of the linear delta. This allows the
* provided keys to be reordered.
* @return An ISAM2Result struct containing information about the update
*/
virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
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<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);
@ -542,48 +688,50 @@ public:
* requested to be marginalized. Marginalization leaves a linear
* approximation of the marginal in the system, and the linearization points
* of any variables involved in this linear marginal become fixed. The set
* fixed variables will include any key involved with the marginalized variables
* in the original factors, and possibly additional ones due to fill-in.
* fixed variables will include any key involved with the marginalized
* variables in the original factors, and possibly additional ones due to
* fill-in.
*
* If provided, 'marginalFactorsIndices' will be augmented with the factor graph
* indices of the marginal factors added during the 'marginalizeLeaves' call
* If provided, 'marginalFactorsIndices' will be augmented with the factor
* graph indices of the marginal factors added during the 'marginalizeLeaves'
* call
*
* If provided, 'deletedFactorsIndices' will be augmented with the factor graph
* indices of any factor that was removed during the 'marginalizeLeaves' call
* If provided, 'deletedFactorsIndices' will be augmented with the factor
* graph indices of any factor that was removed during the 'marginalizeLeaves'
* call
*/
void marginalizeLeaves(const FastList<Key>& leafKeys,
void marginalizeLeaves(
const FastList<Key>& leafKeys,
boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
/// Access the current linearization point
const Values& getLinearizationPoint() const {
return theta_;
}
const Values& getLinearizationPoint() const { return theta_; }
/// Check whether variable with given key exists in linearization point
bool valueExists(Key key) const {
return theta_.exists(key);
}
bool valueExists(Key key) const { return theta_.exists(key); }
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
/** Compute an estimate from the incomplete linear delta computed during the
* last update. This delta is incomplete because it was not updated below
* wildfire_threshold. If only a single variable is needed, it is faster to
* call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const;
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables.
/** Compute an estimate for a single variable using its incomplete linear
* delta computed during the last update. This is faster than calling the
* no-argument version of calculateEstimate, which operates on all variables.
* @param key
* @return
*/
template<class VALUE>
template <class VALUE>
VALUE calculateEstimate(Key key) const;
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables. This is a non-templated version
* that returns a Value base class for use with the MATLAB wrapper.
/** Compute an estimate for a single variable using its incomplete linear
* delta computed during the last update. This is faster than calling the
* no-argument version of calculateEstimate, which operates on all variables.
* This is a non-templated version that returns a Value base class for use
* with the MATLAB wrapper.
* @param key
* @return
*/
@ -598,7 +746,8 @@ public:
/** Internal implementation functions */
struct Impl;
/** Compute an estimate using a complete delta computed by a full back-substitution.
/** Compute an estimate using a complete delta computed by a full
* back-substitution.
*/
Values calculateBestEstimate() const;
@ -609,7 +758,9 @@ public:
double error(const VectorValues& x) const;
/** Access the set of nonlinear factors */
const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
const NonlinearFactorGraph& getFactorsUnsafe() const {
return nonlinearFactors_;
}
/** Access the nonlinear variable index */
const VariableIndex& getVariableIndex() const { return variableIndex_; }
@ -629,9 +780,10 @@ public:
/** prints out clique statistics */
void printStats() const { getCliqueData().getStats().print(); }
/** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d
* \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also
* gradient(const GaussianBayesNet&, const VectorValues&).
/** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert
* \Sigma^{-1} R x - d \right\Vert^2 \f$, centered around zero. The gradient
* about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&,
* const VectorValues&).
*
* @return A VectorValues storing the gradient.
*/
@ -639,20 +791,24 @@ public:
/// @}
protected:
protected:
FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans);
virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
virtual boost::shared_ptr<KeySet> recalculate(
const KeySet& markedKeys, const KeySet& relinKeys,
const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
const boost::optional<FastMap<Key, int> >& constrainKeys,
ISAM2Result& result);
void updateDelta(bool forceFullSolve = false) const;
}; // ISAM2
/// traits
template<> struct traits<ISAM2> : public Testable<ISAM2> {};
template <>
struct traits<ISAM2> : public Testable<ISAM2> {};
/// Optimize the BayesTree, starting from the root.
/// @param replaced Needs to contain
@ -665,19 +821,21 @@ template<> struct traits<ISAM2> : public Testable<ISAM2> {};
/// and recursive backsubstitution might eventually stop if none of the changed
/// variables are contained in the subtree.
/// @return The number of variables that were solved for
template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const KeySet& replaced, VectorValues& delta);
template <class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
const KeySet& replaced, VectorValues& delta);
template<class CLIQUE>
template <class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
double threshold, const KeySet& replaced, VectorValues& delta);
double threshold, const KeySet& replaced,
VectorValues& delta);
/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
template<class CLIQUE>
/// calculate the number of non-zero entries for the tree starting at clique
/// (use root for complete matrix)
template <class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
} /// namespace gtsam
} // namespace gtsam
#include <gtsam/nonlinear/ISAM2-inl.h>
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/nonlinear/ISAM2-inl.h>