diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index f177c6a8c..be15624b0 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -14,8 +14,34 @@ namespace gtsam { template struct ISAM2::Impl { - static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); - static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param ordering Current ordering to be augmented with new variables + * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables + */ + static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); + + /** + * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol + * in each NonlinearFactor, obtains the index by calling ordering[symbol]. + * @param ordering The current ordering from which to obtain the variable indices + * @param factors The factors from which to extract the variables + * @return The set of variables indices from the factors + */ + static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); + + /** + * Find the set of variables to be relinearized according to relinearizeThreshold. + * Any variables in the VectorValues delta whose vector magnitude is greater than + * or equal to relinearizeThreshold are returned. + * @param delta The linear delta to check against the threshold + * @return The set of variable indices in delta whose magnitude is greater than or + * equal to relinearizeThreshold + */ + static FastSet CheckRelinearization(Permuted& delta, double relinearizeThreshold); }; /* ************************************************************************* */ @@ -70,4 +96,16 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Orderin return indices; } +/* ************************************************************************* */ +template +FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { + FastSet relinKeys; + for(Index var=0; var(); + if(maxDelta >= relinearizeThreshold) { + relinKeys.insert(var); + } + } +} + } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index b65ed65d6..b07f49f1c 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -716,21 +716,19 @@ void ISAM2::update( if(structuralLast) structuralKeys = markedKeys; // If we're using structural-last ordering, make another copy toc(3,"gather involved keys"); - vector markedRelinMask(ordering_.nVars(), false); - bool relinAny = false; // Check relinearization if we're at a 10th 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"); + vector markedRelinMask(ordering_.nVars(), false); + bool relinAny = false; // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - for(Index var=0; var(); - if(maxDelta >= params_.relinearizeThreshold || disableReordering) { - markedRelinMask[var] = true; - markedKeys.insert(var); - if(!relinAny) relinAny = true; - } - } + FastSet relinKeys = Impl::CheckRelinearization(delta_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, 0.0); // This is used for debugging + + BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } + markedKeys.insert(relinKeys.begin(), relinKeys.end()); + if(!relinKeys.empty()) + relinAny = true; toc(4,"gather relinearize keys"); tic(5,"fluid find_all"); @@ -739,37 +737,29 @@ void ISAM2::update( // mark all cliques that involve marked variables if(this->root()) find_all(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator - // richard commented these out since now using an array to mark keys - //affectedKeys.sort(); // remove duplicates - //affectedKeys.unique(); - // merge with markedKeys } - // richard commented these out since now using an array to mark keys - //markedKeys.splice(markedKeys.begin(), affectedKeys, affectedKeys.begin(), affectedKeys.end()); - //markedKeys.sort(); // remove duplicates - //markedKeys.unique(); -// BOOST_FOREACH(const Index var, affectedKeys) { -// markedKeys.push_back(var); -// } toc(5,"fluid find_all"); - } - tic(6,"expmap"); - // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (relinAny) { + tic(6,"expmap"); + // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (relinAny) { #ifndef NDEBUG - _SelectiveExpmapAndClear selectiveExpmap(delta_, ordering_, markedRelinMask); + _SelectiveExpmapAndClear selectiveExpmap(delta_, ordering_, markedRelinMask); #else - _SelectiveExpmap selectiveExpmap(delta_, ordering_, markedRelinMask); + _SelectiveExpmap selectiveExpmap(delta_, ordering_, markedRelinMask); #endif - theta_.apply(selectiveExpmap); -// theta_ = theta_.expmap(deltaMarked); - } - toc(6,"expmap"); + theta_.apply(selectiveExpmap); + } + toc(6,"expmap"); #ifndef NDEBUG - lastRelinVariables_ = markedRelinMask; + lastRelinVariables_ = markedRelinMask; #endif + } else { +#ifndef NDEBUG + lastRelinVariables_ = vector(ordering_.nVars(), false); +#endif + } tic(7,"linearize new"); tic(1,"linearize"); @@ -800,19 +790,6 @@ void ISAM2::update( delta_.permutation() = Permutation::Identity(delta_.size()); delta_.container() = newDelta; lastBacksubVariableCount = theta_.size(); - -//#ifndef NDEBUG -// FactorGraph linearfullJ = *nonlinearFactors_.linearize(theta_, ordering_); -// VectorValues deltafullJ = optimize(*GenericSequentialSolver(linearfullJ).eliminate()); -// FactorGraph linearfullH = -// *nonlinearFactors_.linearize(theta_, ordering_)->template convertCastFactors >(); -// VectorValues deltafullH = optimize(*GenericSequentialSolver(linearfullH).eliminate()); -// if(!assert_equal(deltafullJ, newDelta, 1e-2)) -// throw runtime_error("iSAM2 does not agree with full Jacobian solver"); -// if(!assert_equal(deltafullH, newDelta, 1e-2)) -// throw runtime_error("iSAM2 does not agree with full Hessian solver"); - //#endif - } else { vector replacedKeysMask(variableIndex_.size(), false); if(replacedKeys) { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 50137c3bb..975b94a08 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -92,6 +92,59 @@ TEST(ISAM2, AddVariables) { EXPECT(assert_equal(orderingExpected, ordering)); } +/* ************************************************************************* */ +//TEST(ISAM2, IndicesFromFactors) { +// +// using namespace gtsam::planarSLAM; +// typedef GaussianISAM2::Impl Impl; +// +// Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1); +// planarSLAM::Graph graph; +// graph.addPrior(PoseKey(0), Pose2(), sharedUnit(Pose2::dimension)); +// graph.addRange(PoseKey(0), PointKey(0), 1.0, sharedUnit(1)); +// +// FastSet expected; +// expected.insert(0); +// expected.insert(1); +// +// FastSet actual = Impl::IndicesFromFactors(ordering, graph); +// +// EXPECT(assert_equal(expected, actual)); +//} + +/* ************************************************************************* */ +//TEST(ISAM2, CheckRelinearization) { +// +// typedef GaussianISAM2::Impl Impl; +// +// // Create values where indices 1 and 3 are above the threshold of 0.1 +// VectorValues values; +// values.reserve(4, 10); +// values.push_back_preallocated(Vector_(2, 0.09, 0.09)); +// values.push_back_preallocated(Vector_(3, 0.11, 0.11, 0.09)); +// values.push_back_preallocated(Vector_(3, 0.09, 0.09, 0.09)); +// values.push_back_preallocated(Vector_(2, 0.11, 0.11)); +// +// // Create a permutation +// Permutation permutation(4); +// permutation[0] = 2; +// permutation[1] = 0; +// permutation[2] = 1; +// permutation[3] = 3; +// +// Permuted permuted(permutation, values); +// +// // After permutation, the indices above the threshold are 2 and 2 +// FastSet expected; +// expected.insert(2); +// expected.insert(3); +// +// // Indices checked by CheckRelinearization +// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// +// EXPECT(assert_equal(expected, actual)); +//} + /* ************************************************************************* */ TEST(ISAM2, optimize2) {