Working on ISAM2
Richard Roberts
parent
981148a92d
commit
c0ccec4656
|
|
@ -22,6 +22,14 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::setEliminationResult(
|
||||
const typename FactorGraphType::EliminationResult& eliminationResult)
|
||||
{
|
||||
conditional_ = eliminationResult.first;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
|
||||
|
|
|
|||
|
|
@ -80,6 +80,11 @@ namespace gtsam {
|
|||
derived_weak_ptr parent_;
|
||||
std::vector<derived_ptr> children;
|
||||
|
||||
/// Fill the elimination result produced during elimination. Here this just stores the
|
||||
/// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique
|
||||
/// to also cache the remaining factor.
|
||||
void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
|
|||
|
|
@ -195,8 +195,8 @@ namespace gtsam {
|
|||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
eliminationFunction(gatheredFactors, Ordering(node->keys));
|
||||
|
||||
// Store conditional in BayesTree clique
|
||||
myData.bayesTreeNode->conditional_ = eliminationResult.first;
|
||||
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
|
||||
myData.bayesTreeNode->setEliminationResult(eliminationResult);
|
||||
|
||||
// Store remaining factor in parent's gathered factors
|
||||
if(!eliminationResult.second->empty())
|
||||
|
|
|
|||
|
|
@ -15,14 +15,13 @@
|
|||
* @author Michael Kaess, Richard Roberts
|
||||
*/
|
||||
|
||||
#if 0
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
#include <boost/range/adaptors.hpp>
|
||||
#include <boost/range/algorithm.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
namespace br { using namespace boost::range; using namespace boost::adaptors; }
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
|
@ -85,6 +84,38 @@ std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorizatio
|
|||
return s;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2Clique::setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult)
|
||||
{
|
||||
conditional_ = eliminationResult.first;
|
||||
cachedFactor_ = eliminationResult.second;
|
||||
// Compute gradient contribution
|
||||
gradientContribution_.resize(conditional_->cols() - 1);
|
||||
// Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons
|
||||
gradientContribution_ = -conditional_->get_R().transpose() * conditional_->get_d(),
|
||||
-conditional_->get_S().transpose() * conditional_->get_d();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool ISAM2Clique::equals(const This& other, double tol) const {
|
||||
return Base::equals(other) &&
|
||||
((!cachedFactor_ && !other.cachedFactor_)
|
||||
|| (cachedFactor_ && other.cachedFactor_
|
||||
&& cachedFactor_->equals(*other.cachedFactor_, tol)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const
|
||||
{
|
||||
Base::print(s,formatter);
|
||||
if(cachedFactor_)
|
||||
cachedFactor_->print(s + "Cached: ", formatter);
|
||||
else
|
||||
std::cout << s << "Cached empty" << std::endl;
|
||||
if(gradientContribution_.rows() != 0)
|
||||
gtsam::print(gradientContribution_, "Gradient contribution: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ISAM2::ISAM2(const ISAM2Params& params):
|
||||
deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
|
||||
|
|
@ -105,14 +136,12 @@ ISAM2::ISAM2(const ISAM2& other) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ISAM2& ISAM2::operator=(const ISAM2& rhs) {
|
||||
ISAM2& ISAM2::operator=(const ISAM2& rhs)
|
||||
{
|
||||
// Copy BayesTree
|
||||
this->Base::operator=(rhs);
|
||||
|
||||
// Copy our variables
|
||||
// When we have Permuted<...>, it is only necessary to copy this permuted
|
||||
// view and not the original, because copying the permuted view automatically
|
||||
// copies the original.
|
||||
theta_ = rhs.theta_;
|
||||
variableIndex_ = rhs.variableIndex_;
|
||||
delta_ = rhs.delta_;
|
||||
|
|
@ -122,16 +151,9 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
|
|||
deltaUptodate_ = rhs.deltaUptodate_;
|
||||
deltaReplacedMask_ = rhs.deltaReplacedMask_;
|
||||
nonlinearFactors_ = rhs.nonlinearFactors_;
|
||||
|
||||
linearFactors_ = GaussianFactorGraph();
|
||||
linearFactors_.reserve(rhs.linearFactors_.size());
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) {
|
||||
linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); }
|
||||
|
||||
ordering_ = rhs.ordering_;
|
||||
linearFactors_ = rhs.linearFactors_;
|
||||
params_ = rhs.params_;
|
||||
doglegDelta_ = rhs.doglegDelta_;
|
||||
|
||||
lastAffectedVariableCount = rhs.lastAffectedVariableCount;
|
||||
lastAffectedFactorCount = rhs.lastAffectedFactorCount;
|
||||
lastAffectedCliqueCount = rhs.lastAffectedCliqueCount;
|
||||
|
|
@ -143,15 +165,15 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
|
||||
FastList<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const {
|
||||
static const bool debug = false;
|
||||
if(debug) cout << "Getting affected factors for ";
|
||||
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
|
||||
if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } }
|
||||
if(debug) cout << endl;
|
||||
|
||||
FactorGraph<NonlinearFactor > allAffected;
|
||||
NonlinearFactorGraph allAffected;
|
||||
FastList<size_t> indices;
|
||||
BOOST_FOREACH(const Index key, keys) {
|
||||
BOOST_FOREACH(const Key key, keys) {
|
||||
// const list<size_t> l = nonlinearFactors_.factors(key);
|
||||
// indices.insert(indices.begin(), l.begin(), l.end());
|
||||
const VariableIndex::Factors& factors(variableIndex_[key]);
|
||||
|
|
@ -172,9 +194,9 @@ FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
|
|||
// retrieve all factors that ONLY contain the affected variables
|
||||
// (note that the remaining stuff is summarized in the cached factors)
|
||||
|
||||
FactorGraph<GaussianFactor>::shared_ptr
|
||||
ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const {
|
||||
|
||||
GaussianFactorGraph::shared_ptr
|
||||
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const
|
||||
{
|
||||
gttic(getAffectedFactors);
|
||||
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
|
||||
gttoc(getAffectedFactors);
|
||||
|
|
@ -183,33 +205,32 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const Fas
|
|||
|
||||
gttic(affectedKeysSet);
|
||||
// for fast lookup below
|
||||
FastSet<Index> affectedKeysSet;
|
||||
FastSet<Key> affectedKeysSet;
|
||||
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
|
||||
gttoc(affectedKeysSet);
|
||||
|
||||
gttic(check_candidates_and_linearize);
|
||||
FactorGraph<GaussianFactor>::shared_ptr linearized = boost::make_shared<FactorGraph<GaussianFactor> >();
|
||||
GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>();
|
||||
BOOST_FOREACH(size_t idx, candidates) {
|
||||
bool inside = true;
|
||||
bool useCachedLinear = params_.cacheLinearizedFactors;
|
||||
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
|
||||
Index var = ordering_[key];
|
||||
if(affectedKeysSet.find(var) == affectedKeysSet.end()) {
|
||||
if(affectedKeysSet.find(key) == affectedKeysSet.end()) {
|
||||
inside = false;
|
||||
break;
|
||||
}
|
||||
if(useCachedLinear && relinKeys.find(var) != relinKeys.end())
|
||||
if(useCachedLinear && relinKeys.find(key) != relinKeys.end())
|
||||
useCachedLinear = false;
|
||||
}
|
||||
if(inside) {
|
||||
if(useCachedLinear) {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
assert(linearFactors_[idx]);
|
||||
assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys());
|
||||
assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys());
|
||||
#endif
|
||||
linearized->push_back(linearFactors_[idx]);
|
||||
} else {
|
||||
GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_);
|
||||
GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_);
|
||||
linearized->push_back(linearFactor);
|
||||
if(params_.cacheLinearizedFactors) {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
|
|
@ -235,20 +256,20 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
|
|||
GaussianFactorGraph cachedBoundary;
|
||||
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
// find the last variable that was eliminated
|
||||
Index key = (*orphan)->frontals().back();
|
||||
// retrieve the cached factor and add to boundary
|
||||
cachedBoundary.push_back(orphan->cachedFactor());
|
||||
if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }
|
||||
}
|
||||
|
||||
return cachedBoundary;
|
||||
}
|
||||
|
||||
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& markedKeys,
|
||||
const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices,
|
||||
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
|
||||
const FastVector<Key>& observedKeys,
|
||||
const FastSet<Key>& unusedIndices,
|
||||
const boost::optional<FastMap<Key,int> >& constrainKeys,
|
||||
ISAM2Result& result)
|
||||
{
|
||||
// TODO: new factors are linearized twice, the newFactors passed in are not used.
|
||||
|
||||
const bool debug = ISDEBUG("ISAM2 recalculate");
|
||||
|
|
@ -275,10 +296,10 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
|
||||
if(debug) {
|
||||
cout << "markedKeys: ";
|
||||
BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; }
|
||||
BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; }
|
||||
cout << endl;
|
||||
cout << "observedKeys: ";
|
||||
BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; }
|
||||
BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; }
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
|
|
@ -287,7 +308,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
// (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
|
||||
gttic(removetop);
|
||||
Cliques orphans;
|
||||
BayesNet<GaussianConditional> affectedBayesNet;
|
||||
GaussianBayesNet affectedBayesNet;
|
||||
this->removeTop(markedKeys, affectedBayesNet, orphans);
|
||||
gttoc(removetop);
|
||||
|
||||
|
|
@ -307,17 +328,19 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
|
||||
// ordering provides all keys in conditionals, there cannot be others because path to root included
|
||||
gttic(affectedKeys);
|
||||
FastList<Index> affectedKeys = affectedBayesNet.ordering();
|
||||
FastList<Key> affectedKeys;
|
||||
BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet)
|
||||
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
|
||||
gttoc(affectedKeys);
|
||||
|
||||
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result
|
||||
|
||||
if(affectedKeys.size() >= theta_.size() * batchThreshold) {
|
||||
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Key>()); // Will return this result
|
||||
|
||||
if(affectedKeys.size() >= theta_.size() * batchThreshold)
|
||||
{
|
||||
gttic(batch);
|
||||
|
||||
gttic(add_keys);
|
||||
BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); }
|
||||
br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end()));
|
||||
gttoc(add_keys);
|
||||
|
||||
gttic(reorder);
|
||||
|
|
@ -341,6 +364,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; }
|
||||
}
|
||||
}
|
||||
Ordering order = Ordering::COLAMDConstrained()
|
||||
Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
|
||||
Permutation::shared_ptr colamdInverse(colamd->inverse());
|
||||
gttoc(CCOLAMD);
|
||||
|
|
@ -1163,5 +1187,3 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
|
|||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -19,8 +19,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
|
|
@ -344,10 +342,11 @@ struct GTSAM_EXPORT ISAM2Result {
|
|||
* Specialized Clique structure for ISAM2, incorporating caching and gradient contribution
|
||||
* TODO: more documentation
|
||||
*/
|
||||
class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
|
||||
class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph>
|
||||
{
|
||||
public:
|
||||
typedef ISAM2Clique This;
|
||||
typedef BayesTreeCliqueBase<This,GaussianConditional> Base;
|
||||
typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
typedef GaussianConditional ConditionalType;
|
||||
|
|
@ -356,29 +355,8 @@ public:
|
|||
Base::FactorType::shared_ptr cachedFactor_;
|
||||
Vector gradientContribution_;
|
||||
|
||||
/** Construct from a conditional */
|
||||
ISAM2Clique(const sharedConditional& conditional) : Base(conditional) {
|
||||
throw std::runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); }
|
||||
|
||||
/** Construct from an elimination result */
|
||||
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<ConditionalType::FactorType> >& result) :
|
||||
Base(result.first), cachedFactor_(result.second),
|
||||
gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
|
||||
// Compute gradient contribution
|
||||
const ConditionalType& conditional(*result.first);
|
||||
// Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons
|
||||
gradientContribution_ << -conditional.get_R().transpose() * conditional.get_d(),
|
||||
-conditional.get_S().transpose() * conditional.get_d();
|
||||
}
|
||||
|
||||
/** Produce a deep copy, copying the cached factor and gradient contribution */
|
||||
shared_ptr clone() const {
|
||||
shared_ptr copy(new ISAM2Clique(std::make_pair(
|
||||
sharedConditional(new ConditionalType(*Base::conditional_)),
|
||||
cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr())));
|
||||
copy->gradientContribution_ = gradientContribution_;
|
||||
return copy;
|
||||
}
|
||||
/// Overridden to also store the remaining factor and gradient contribution
|
||||
void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
|
||||
|
||||
/** Access the cached factor */
|
||||
Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
|
||||
|
|
@ -386,35 +364,10 @@ public:
|
|||
/** Access the gradient contribution */
|
||||
const Vector& gradientContribution() const { return gradientContribution_; }
|
||||
|
||||
bool equals(const This& other, double tol=1e-9) const {
|
||||
return Base::equals(other) &&
|
||||
((!cachedFactor_ && !other.cachedFactor_)
|
||||
|| (cachedFactor_ && other.cachedFactor_
|
||||
&& cachedFactor_->equals(*other.cachedFactor_, tol)));
|
||||
}
|
||||
bool equals(const This& other, double tol=1e-9) const;
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const {
|
||||
Base::print(s,formatter);
|
||||
if(cachedFactor_)
|
||||
cachedFactor_->print(s + "Cached: ", formatter);
|
||||
else
|
||||
std::cout << s << "Cached empty" << std::endl;
|
||||
if(gradientContribution_.rows() != 0)
|
||||
gtsam::print(gradientContribution_, "Gradient contribution: ");
|
||||
}
|
||||
|
||||
void permuteWithInverse(const Permutation& inversePermutation) {
|
||||
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
|
||||
Base::permuteWithInverse(inversePermutation);
|
||||
}
|
||||
|
||||
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
|
||||
bool changed = Base::reduceSeparatorWithInverse(inverseReduction);
|
||||
if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction);
|
||||
return changed;
|
||||
}
|
||||
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -438,7 +391,7 @@ private:
|
|||
* estimate of all variables.
|
||||
*
|
||||
*/
|
||||
class ISAM2: public BayesTree<GaussianConditional, ISAM2Clique> {
|
||||
class ISAM2: public BayesTree<ISAM2Clique> {
|
||||
|
||||
protected:
|
||||
|
||||
|
|
@ -481,7 +434,7 @@ protected:
|
|||
* This is \c mutable because it is used internally to not update delta_
|
||||
* until it is needed.
|
||||
*/
|
||||
mutable std::vector<bool> deltaReplacedMask_;
|
||||
mutable FastMap<Key,bool> deltaReplacedMask_; // TODO: Make sure accessed in the right way
|
||||
|
||||
/** All original nonlinear factors are stored here to use during relinearization */
|
||||
NonlinearFactorGraph nonlinearFactors_;
|
||||
|
|
@ -489,10 +442,6 @@ protected:
|
|||
/** The current linear factors, which are only updated as needed */
|
||||
mutable GaussianFactorGraph linearFactors_;
|
||||
|
||||
/** The current elimination ordering Symbols to Index (integer) keys.
|
||||
* We keep it up to date as we add and reorder variables. */
|
||||
Ordering ordering_;
|
||||
|
||||
/** The current parameters */
|
||||
ISAM2Params params_;
|
||||
|
||||
|
|
@ -506,7 +455,7 @@ protected:
|
|||
public:
|
||||
|
||||
typedef ISAM2 This; ///< This class
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
|
||||
typedef BayesTree<ISAM2Clique> Base; ///< The BayesTree base class
|
||||
|
||||
/** Create an empty ISAM2 instance */
|
||||
GTSAM_EXPORT ISAM2(const ISAM2Params& params);
|
||||
|
|
@ -615,9 +564,6 @@ public:
|
|||
/** Access the set of nonlinear factors */
|
||||
GTSAM_EXPORT const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||
|
||||
/** Access the current ordering */
|
||||
GTSAM_EXPORT const Ordering& getOrdering() const { return ordering_; }
|
||||
|
||||
/** Access the nonlinear variable index */
|
||||
GTSAM_EXPORT const VariableIndex& getVariableIndex() const { return variableIndex_; }
|
||||
|
||||
|
|
@ -637,12 +583,12 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
GTSAM_EXPORT FastList<size_t> getAffectedFactors(const FastList<Index>& keys) const;
|
||||
GTSAM_EXPORT FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const;
|
||||
GTSAM_EXPORT FastList<size_t> getAffectedFactors(const FastList<Key>& keys) const;
|
||||
GTSAM_EXPORT GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const;
|
||||
GTSAM_EXPORT GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
|
||||
|
||||
GTSAM_EXPORT boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
|
||||
const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
|
||||
GTSAM_EXPORT boost::shared_ptr<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
|
||||
const FastVector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
|
||||
// void linear_update(const GaussianFactorGraph& newFactors);
|
||||
GTSAM_EXPORT void updateDelta(bool forceFullSolve = false) const;
|
||||
|
||||
|
|
@ -740,5 +686,3 @@ GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g);
|
|||
|
||||
#include <gtsam/nonlinear/ISAM2-inl.h>
|
||||
#include <gtsam/nonlinear/ISAM2-impl.h>
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue