Only do ISAM2 back-substitution when needed instead of during every update
Richard Roberts
parent
36253a39a0
commit
0d216c8878
|
|
@ -46,8 +46,8 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
|
|||
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
|
||||
* @param keyFormatter Formatter for printing nonlinear keys during debugging
|
||||
*/
|
||||
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering,
|
||||
typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||
Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
|
||||
|
|
@ -121,12 +121,15 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
|
|||
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
|
||||
const ReorderingMode& reorderingMode);
|
||||
|
||||
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
|
||||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
|
||||
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||
Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||
|
||||
theta.insert(newTheta);
|
||||
|
|
@ -153,6 +156,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
|
|||
assert(ordering.size() == delta.size());
|
||||
}
|
||||
assert(ordering.nVars() >= nodes.size());
|
||||
replacedKeys.resize(ordering.nVars(), false);
|
||||
nodes.resize(ordering.nVars());
|
||||
}
|
||||
|
||||
|
|
@ -229,7 +233,8 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
|
|||
invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
|
||||
#endif
|
||||
|
||||
assert(values.size() == ordering.size());
|
||||
assert(values.size() == ordering.nVars());
|
||||
assert(delta.size() == ordering.nVars());
|
||||
Values::iterator key_value;
|
||||
Ordering::const_iterator key_index;
|
||||
for(key_value = values.begin(), key_index = ordering.begin();
|
||||
|
|
@ -354,4 +359,43 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
|
||||
|
||||
size_t lastBacksubVariableCount;
|
||||
|
||||
if (wildfireThreshold <= 0.0) {
|
||||
// Threshold is zero or less, so do a full recalculation
|
||||
// Collect dimensions and allocate new VectorValues
|
||||
vector<size_t> dims(delta.size());
|
||||
for(size_t j=0; j<delta.size(); ++j)
|
||||
dims[j] = delta->dim(j);
|
||||
VectorValues newDelta(dims);
|
||||
|
||||
// Optimize full solution delta
|
||||
optimize2(root, newDelta);
|
||||
|
||||
// Copy solution into delta
|
||||
delta.permutation() = Permutation::Identity(delta.size());
|
||||
delta.container() = newDelta;
|
||||
|
||||
lastBacksubVariableCount = delta.size();
|
||||
|
||||
} else {
|
||||
// Optimize with wildfire
|
||||
lastBacksubVariableCount = optimize2(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
|
||||
|
||||
#ifndef NDEBUG
|
||||
for(size_t j=0; j<delta.container().size(); ++j)
|
||||
assert(delta.container()[j].unaryExpr(&isfinite<double>).all());
|
||||
#endif
|
||||
}
|
||||
|
||||
// Clear replacedKeys
|
||||
replacedKeys.assign(replacedKeys.size(), false);
|
||||
|
||||
return lastBacksubVariableCount;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ static const double batchThreshold = 0.65;
|
|||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
|
||||
delta_(Permutation(), deltaUnpermuted_), params_(params) {
|
||||
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) {
|
||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
|
||||
|
|
@ -50,7 +50,7 @@ ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
|
|||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
ISAM2<CONDITIONAL, GRAPH>::ISAM2():
|
||||
delta_(Permutation(), deltaUnpermuted_) {
|
||||
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) {
|
||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
|
||||
|
|
@ -413,13 +413,21 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
lastBacksubVariableCount = 0;
|
||||
lastNnzTop = 0;
|
||||
ISAM2Result result;
|
||||
const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0);
|
||||
|
||||
if(verbose) {
|
||||
cout << "ISAM2::update\n";
|
||||
this->print("ISAM2: ");
|
||||
}
|
||||
|
||||
tic(0,"push_back factors");
|
||||
// Update delta if we need it to check relinearization later
|
||||
if(relinearizeThisStep) {
|
||||
tic(0, "updateDelta");
|
||||
updateDelta(disableReordering);
|
||||
toc(0, "updateDelta");
|
||||
}
|
||||
|
||||
tic(1,"push_back factors");
|
||||
// Add the new factor indices to the result struct
|
||||
result.newFactorsIndices.resize(newFactors.size());
|
||||
for(size_t i=0; i<newFactors.size(); ++i)
|
||||
|
|
@ -438,19 +446,19 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
|
||||
// Remove removed factors from the variable index so we do not attempt to relinearize them
|
||||
variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
|
||||
toc(0,"push_back factors");
|
||||
toc(1,"push_back factors");
|
||||
|
||||
tic(1,"add new variables");
|
||||
tic(2,"add new variables");
|
||||
// 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
|
||||
Impl::AddVariables(newTheta, theta_, delta_, ordering_, Base::nodes_);
|
||||
toc(1,"add new variables");
|
||||
Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_);
|
||||
toc(2,"add new variables");
|
||||
|
||||
tic(2,"evaluate error before");
|
||||
tic(3,"evaluate error before");
|
||||
if(params_.evaluateNonlinearError)
|
||||
result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
|
||||
toc(2,"evaluate error before");
|
||||
toc(3,"evaluate error before");
|
||||
|
||||
tic(3,"gather involved keys");
|
||||
tic(4,"gather involved keys");
|
||||
// 3. Mark linear update
|
||||
FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
|
||||
// Also mark keys involved in removed factors
|
||||
|
|
@ -462,11 +470,11 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
// is a vector of size_t, so the constructor unintentionally resolves to
|
||||
// vector(size_t count, Index value) instead of the iterator constructor.
|
||||
FastVector<Index> newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
|
||||
toc(3,"gather involved keys");
|
||||
toc(4,"gather involved keys");
|
||||
|
||||
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
|
||||
if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps
|
||||
tic(4,"gather relinearize keys");
|
||||
if (relinearizeThisStep) {
|
||||
tic(5,"gather relinearize keys");
|
||||
vector<bool> markedRelinMask(ordering_.nVars(), false);
|
||||
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
|
||||
FastSet<Index> relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
|
||||
|
|
@ -475,19 +483,19 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
// Add the variables being relinearized to the marked keys
|
||||
BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
|
||||
markedKeys.insert(relinKeys.begin(), relinKeys.end());
|
||||
toc(4,"gather relinearize keys");
|
||||
toc(5,"gather relinearize keys");
|
||||
|
||||
tic(5,"fluid find_all");
|
||||
tic(6,"fluid find_all");
|
||||
// 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
|
||||
if (!relinKeys.empty() && this->root())
|
||||
Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator
|
||||
toc(5,"fluid find_all");
|
||||
toc(6,"fluid find_all");
|
||||
|
||||
tic(6,"expmap");
|
||||
tic(7,"expmap");
|
||||
// 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
|
||||
if (!relinKeys.empty())
|
||||
Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_);
|
||||
toc(6,"expmap");
|
||||
toc(7,"expmap");
|
||||
|
||||
result.variablesRelinearized = markedKeys.size();
|
||||
|
||||
|
|
@ -501,7 +509,7 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
#endif
|
||||
}
|
||||
|
||||
tic(7,"linearize new");
|
||||
tic(8,"linearize new");
|
||||
tic(1,"linearize");
|
||||
// 7. Linearize new factors
|
||||
FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
|
||||
|
|
@ -511,9 +519,9 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
// Augment the variable index with the new factors
|
||||
variableIndex_.augment(*linearFactors);
|
||||
toc(2,"augment VI");
|
||||
toc(7,"linearize new");
|
||||
toc(8,"linearize new");
|
||||
|
||||
tic(8,"recalculate");
|
||||
tic(9,"recalculate");
|
||||
// 8. Redo top of Bayes tree
|
||||
// Convert constrained symbols to indices
|
||||
boost::optional<FastSet<Index> > constrainedIndices;
|
||||
|
|
@ -526,49 +534,17 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
boost::shared_ptr<FastSet<Index> > replacedKeys;
|
||||
if(!markedKeys.empty() || !newKeys.empty())
|
||||
replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result);
|
||||
toc(8,"recalculate");
|
||||
|
||||
tic(9,"solve");
|
||||
// 9. Solve
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
|
||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
||||
const ISAM2GaussNewtonParams& gaussNewtonParams =
|
||||
boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
|
||||
if (gaussNewtonParams.wildfireThreshold <= 0.0 || disableReordering) {
|
||||
VectorValues newDelta(theta_.dims(ordering_));
|
||||
optimize2(this->root(), newDelta);
|
||||
if(debug) newDelta.print("newDelta: ");
|
||||
assert(newDelta.size() == delta_.size());
|
||||
delta_.permutation() = Permutation::Identity(delta_.size());
|
||||
delta_.container() = newDelta;
|
||||
lastBacksubVariableCount = theta_.size();
|
||||
} else {
|
||||
vector<bool> replacedKeysMask(variableIndex_.size(), false);
|
||||
// Update replaced keys mask (accumulates until back-substitution takes place)
|
||||
if(replacedKeys) {
|
||||
BOOST_FOREACH(const Index var, *replacedKeys) {
|
||||
replacedKeysMask[var] = true; } }
|
||||
lastBacksubVariableCount = optimize2(this->root(), gaussNewtonParams.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_
|
||||
deltaReplacedMask_[var] = true; } }
|
||||
toc(9,"recalculate");
|
||||
|
||||
#ifndef NDEBUG
|
||||
for(size_t j=0; j<delta_.container().size(); ++j)
|
||||
assert(delta_.container()[j].unaryExpr(&isfinite<double>).all());
|
||||
#endif
|
||||
}
|
||||
} else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
|
||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
||||
const ISAM2DoglegParams& doglegParams =
|
||||
boost::get<ISAM2DoglegParams>(params_.optimizationParams);
|
||||
// Do one Dogleg iteration
|
||||
tic(1, "Dogleg Iterate");
|
||||
DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate(
|
||||
*doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose);
|
||||
toc(1, "Dogleg Iterate");
|
||||
// Update Delta and linear step
|
||||
doglegDelta_ = doglegResult.Delta;
|
||||
delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation
|
||||
delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
|
||||
}
|
||||
toc(9,"solve");
|
||||
//tic(9,"solve");
|
||||
// 9. Solve
|
||||
if(debug) delta_.print("delta_: ");
|
||||
//toc(9,"solve");
|
||||
|
||||
tic(10,"evaluate error after");
|
||||
if(params_.evaluateNonlinearError)
|
||||
|
|
@ -576,10 +552,48 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
|||
toc(10,"evaluate error after");
|
||||
|
||||
result.cliques = this->nodes().size();
|
||||
deltaUptodate_ = false;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
void ISAM2<CONDITIONAL, GRAPH>::updateDelta(bool forceFullSolve) const {
|
||||
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
|
||||
// If using Gauss-Newton, update with wildfireThreshold
|
||||
const ISAM2GaussNewtonParams& gaussNewtonParams =
|
||||
boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
|
||||
const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
|
||||
tic(0, "Wildfire update");
|
||||
lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold);
|
||||
toc(0, "Wildfire update");
|
||||
|
||||
} else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
|
||||
// If using Dogleg, do a Dogleg step
|
||||
const ISAM2DoglegParams& doglegParams =
|
||||
boost::get<ISAM2DoglegParams>(params_.optimizationParams);
|
||||
|
||||
// Do one Dogleg iteration
|
||||
tic(1, "Dogleg Iterate");
|
||||
DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate(
|
||||
*doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose);
|
||||
toc(1, "Dogleg Iterate");
|
||||
|
||||
// Update Delta and linear step
|
||||
doglegDelta_ = doglegResult.Delta;
|
||||
delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation
|
||||
delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
|
||||
|
||||
// Clear replaced mask
|
||||
deltaReplacedMask_.assign(deltaReplacedMask_.size(), false);
|
||||
}
|
||||
|
||||
deltaUptodate_ = true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
|
||||
|
|
@ -587,7 +601,7 @@ Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
|
|||
// handles Permuted<VectorValues>
|
||||
Values ret(theta_);
|
||||
vector<bool> mask(ordering_.nVars(), true);
|
||||
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
|
||||
Impl::ExpmapMasked(ret, getDelta(), ordering_, mask);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -608,6 +622,14 @@ Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
|
|||
return theta_.retract(delta, ordering_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
|
||||
if(!deltaUptodate_)
|
||||
updateDelta();
|
||||
return delta_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
|
||||
|
|
|
|||
|
|
@ -283,19 +283,42 @@ protected:
|
|||
/** The linear delta from the last linear solution, an update to the estimate in theta */
|
||||
VectorValues deltaUnpermuted_;
|
||||
|
||||
/** @brief The permutation through which the deltaUnpermuted_ is
|
||||
/** The permutation through which the deltaUnpermuted_ is
|
||||
* referenced.
|
||||
*
|
||||
* Permuting Vector entries would be slow, so for performance we
|
||||
* instead maintain this permutation through which we access the linear delta
|
||||
* indirectly
|
||||
*
|
||||
* This is \c mutable because it is a "cached" variable - it is not updated
|
||||
* until either requested with getDelta() or calculateEstimate(), or needed
|
||||
* during update() to evaluate whether to relinearize variables.
|
||||
*/
|
||||
Permuted<VectorValues> delta_;
|
||||
mutable Permuted<VectorValues> delta_;
|
||||
|
||||
/** Indicates whether the current delta is up-to-date, only used
|
||||
* internally - delta will always be updated if necessary when it is
|
||||
* requested with getDelta() or calculateEstimate().
|
||||
*
|
||||
* This is \c mutable because it is used internally to not update delta_
|
||||
* until it is needed.
|
||||
*/
|
||||
mutable bool deltaUptodate_;
|
||||
|
||||
/** 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,
|
||||
* delta will always be updated if necessary when requested with getDelta()
|
||||
* or calculateEstimate().
|
||||
*
|
||||
* This is \c mutable because it is used internally to not update delta_
|
||||
* until it is needed.
|
||||
*/
|
||||
mutable std::vector<bool> deltaReplacedMask_;
|
||||
|
||||
/** All original nonlinear factors are stored here to use during relinearization */
|
||||
GRAPH nonlinearFactors_;
|
||||
|
||||
/** @brief The current elimination ordering Symbols to Index (integer) keys.
|
||||
/** The current elimination ordering Symbols to Index (integer) keys.
|
||||
*
|
||||
* We keep it up to date as we add and reorder variables.
|
||||
*/
|
||||
|
|
@ -305,7 +328,7 @@ protected:
|
|||
ISAM2Params params_;
|
||||
|
||||
/** The current Dogleg Delta (trust region radius) */
|
||||
boost::optional<double> doglegDelta_;
|
||||
mutable boost::optional<double> doglegDelta_;
|
||||
|
||||
private:
|
||||
#ifndef NDEBUG
|
||||
|
|
@ -337,6 +360,8 @@ public:
|
|||
newISAM2->variableIndex_ = variableIndex_;
|
||||
newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
|
||||
newISAM2->delta_ = delta_;
|
||||
newISAM2->deltaUptodate_ = deltaUptodate_;
|
||||
newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
|
||||
newISAM2->nonlinearFactors_ = nonlinearFactors_;
|
||||
newISAM2->ordering_ = ordering_;
|
||||
newISAM2->params_ = params_;
|
||||
|
|
@ -404,7 +429,7 @@ public:
|
|||
Values calculateBestEstimate() const;
|
||||
|
||||
/** Access the current delta, computed during the last call to update */
|
||||
const Permuted<VectorValues>& getDelta() const { return delta_; }
|
||||
const Permuted<VectorValues>& getDelta() const;
|
||||
|
||||
/** Access the set of nonlinear factors */
|
||||
const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||
|
|
@ -416,7 +441,7 @@ public:
|
|||
size_t lastAffectedFactorCount;
|
||||
size_t lastAffectedCliqueCount;
|
||||
size_t lastAffectedMarkedCount;
|
||||
size_t lastBacksubVariableCount;
|
||||
mutable size_t lastBacksubVariableCount;
|
||||
size_t lastNnzTop;
|
||||
|
||||
ISAM2Params params() const { return params_; }
|
||||
|
|
@ -433,6 +458,7 @@ private:
|
|||
const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
|
||||
const boost::optional<FastSet<size_t> >& constrainKeys, ISAM2Result& result);
|
||||
// void linear_update(const GaussianFactorGraph& newFactors);
|
||||
void updateDelta(bool forceFullSolve = false) const;
|
||||
|
||||
}; // ISAM2
|
||||
|
||||
|
|
|
|||
|
|
@ -47,6 +47,8 @@ TEST(ISAM2, AddVariables) {
|
|||
|
||||
Permuted<VectorValues> delta(permutation, deltaUnpermuted);
|
||||
|
||||
vector<bool> replacedKeys(2, false);
|
||||
|
||||
Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
|
||||
|
||||
GaussianISAM2<>::Nodes nodes(2);
|
||||
|
|
@ -75,17 +77,20 @@ TEST(ISAM2, AddVariables) {
|
|||
|
||||
Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
|
||||
|
||||
vector<bool> replacedKeysExpected(3, false);
|
||||
|
||||
Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
|
||||
|
||||
GaussianISAM2<>::Nodes nodesExpected(
|
||||
3, GaussianISAM2<>::sharedClique());
|
||||
|
||||
// Expand initial state
|
||||
GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes);
|
||||
GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
|
||||
|
||||
EXPECT(assert_equal(thetaExpected, theta));
|
||||
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
|
||||
EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
|
||||
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
|
||||
EXPECT(assert_equal(orderingExpected, ordering));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue