99 lines
2.9 KiB
C++
99 lines
2.9 KiB
C++
/**
|
|
* @file ISAM2.h
|
|
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
|
* @author Michael Kaess
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <map>
|
|
#include <list>
|
|
#include <vector>
|
|
//#include <boost/serialization/map.hpp>
|
|
//#include <boost/serialization/list.hpp>
|
|
#include <stdexcept>
|
|
|
|
#include <gtsam/base/Testable.h>
|
|
#include <gtsam/inference/FactorGraph.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/inference/BayesNet.h>
|
|
#include <gtsam/inference/BayesTree.h>
|
|
#include <gtsam/inference/Key.h>
|
|
#include <gtsam/inference/SymbolMap.h>
|
|
|
|
namespace gtsam {
|
|
|
|
typedef SymbolMap<GaussianFactor::shared_ptr> CachedFactors;
|
|
|
|
template<class Conditional, class Config>
|
|
class ISAM2: public BayesTree<Conditional> {
|
|
|
|
protected:
|
|
|
|
// current linearization point
|
|
Config theta_;
|
|
|
|
// the linear solution, an update to the estimate in theta
|
|
VectorConfig delta_;
|
|
|
|
// for keeping all original nonlinear factors
|
|
NonlinearFactorGraph<Config> nonlinearFactors_;
|
|
|
|
// cached intermediate results for restarting computation in the middle
|
|
CachedFactors cached_;
|
|
|
|
public:
|
|
|
|
/** Create an empty Bayes Tree */
|
|
ISAM2();
|
|
|
|
/** Create a Bayes Tree from a Bayes Net */
|
|
ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config);
|
|
|
|
/** Destructor */
|
|
virtual ~ISAM2() {}
|
|
|
|
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
|
|
|
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
|
|
|
/**
|
|
* ISAM2.
|
|
*/
|
|
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
|
|
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
|
|
|
|
// needed to create initial estimates
|
|
const Config getLinearizationPoint() const {return theta_;}
|
|
|
|
// estimate based on incomplete delta (threshold!)
|
|
const Config calculateEstimate() const {return theta_.expmap(delta_);}
|
|
|
|
// estimate based on full delta (note that this is based on the current linearization point)
|
|
const Config calculateBestEstimate() const {return theta_.expmap(*optimize2(this->root()));}
|
|
|
|
const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
|
|
|
|
size_t lastAffectedVariableCount;
|
|
size_t lastAffectedFactorCount;
|
|
size_t lastAffectedCliqueCount;
|
|
size_t lastAffectedMarkedCount;
|
|
size_t lastBacksubVariableCount;
|
|
size_t lastNnzTop;
|
|
|
|
private:
|
|
|
|
std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const;
|
|
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys) const;
|
|
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
|
|
|
|
boost::shared_ptr<std::set<Symbol> > recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
|
|
void linear_update(const FactorGraph<GaussianFactor>& newFactors);
|
|
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
|
|
|
|
}; // ISAM2
|
|
|
|
} /// namespace gtsam
|