diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 7a66eeaf2..00f637d1e 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -17,6 +17,7 @@ namespace gtsam { /** + * @ingroup ISAM2 * @brief The main ISAM2 class that is exposed to gtsam users, see ISAM2 for usage. * * This is a thin wrapper around an ISAM2 class templated on @@ -27,7 +28,14 @@ namespace gtsam { * variables. */ template -class GaussianISAM2 : public ISAM2 {}; +class GaussianISAM2 : public ISAM2 { +public: + /** Create an empty ISAM2 instance */ + GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} + + /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + GaussianISAM2() : ISAM2() {} +}; // optimize the BayesTree, starting from the root void optimize2(const BayesTree::sharedClique& root, VectorValues& delta); diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 2f950fe2c..f177c6a8c 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -4,10 +4,21 @@ * @author Michael Kaess, Richard Roberts */ +#include + +#include + +#include + 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); +}; + /* ************************************************************************* */ -template struct _VariableAdder { Ordering& ordering; Permuted& vconfig; @@ -36,7 +47,7 @@ void ISAM2::Impl::AddVariables( delta.container().reserve(delta->size() + newTheta.size(), delta->dim() + accumulate(dims.begin(), dims.end(), 0)); delta.permutation().resize(delta->size() + newTheta.size()); { - _VariableAdder > vadder(ordering, delta); + _VariableAdder vadder(ordering, delta); newTheta.apply(vadder); assert(delta.permutation().size() == delta.container().size()); assert(delta.container().dim() == delta.container().dimCapacity()); @@ -47,4 +58,16 @@ void ISAM2::Impl::AddVariables( nodes.resize(ordering.nVars()); } +/* ************************************************************************* */ +template +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { + FastSet indices; + BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { + BOOST_FOREACH(const Symbol& key, factor->keys()) { + indices.insert(ordering[key]); + } + } + return indices; +} + } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index f3199921c..b65ed65d6 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -36,9 +36,15 @@ static const double batchThreshold = 0.65; static const bool latestLast = true; static const bool structuralLast = false; -/** Create an empty Bayes Tree */ +/* ************************************************************************* */ template -ISAM2::ISAM2() : BayesTree(), delta_(Permutation(), deltaUnpermuted_) {} +ISAM2::ISAM2(const ISAM2Params& params): + delta_(Permutation(), deltaUnpermuted_), params_(params) {} + +/* ************************************************************************* */ +template +ISAM2::ISAM2(): + delta_(Permutation(), deltaUnpermuted_) {} /** Create a Bayes Tree from a nonlinear factor graph */ //template @@ -670,12 +676,10 @@ struct _SelectiveExpmapAndClear { /* ************************************************************************* */ template void ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, - double wildfire_threshold, double relinearize_threshold, bool relinearize, bool force_relinearize) { + const NonlinearFactorGraph& newFactors, const Values& newTheta, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); static const bool verbose = ISDEBUG("ISAM2 update verbose"); - if(disableReordering) { wildfire_threshold = -1.0; relinearize_threshold = 0.0; } static int count = 0; count++; @@ -705,30 +709,23 @@ void ISAM2::update( tic(3,"gather involved keys"); // 3. Mark linear update - FastSet markedKeys; + FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + vector newKeys; newKeys.reserve(markedKeys.size()); + newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them FastSet structuralKeys; - vector newKeys; newKeys.reserve(newFactors.size() * 6); - BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, newFactors) { - BOOST_FOREACH(const Symbol& key, factor->keys()) { - Index var = ordering_[key]; - markedKeys.insert(var); - if(structuralLast) structuralKeys.insert(var); - newKeys.push_back(var); - } - } + 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 || (relinearize && count%1 == 0)) { // todo: every n steps + if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps tic(4,"gather relinearize keys"); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. for(Index var=0; var(); - if(maxDelta >= relinearize_threshold) { + if(maxDelta >= params_.relinearizeThreshold || disableReordering) { markedRelinMask[var] = true; markedKeys.insert(var); if(!relinAny) relinAny = true; @@ -795,7 +792,7 @@ void ISAM2::update( tic(9,"solve"); // 9. Solve - if (wildfire_threshold<=0.) { + if (params_.wildfireThreshold <= 0.0 || disableReordering) { VectorValues newDelta(theta_.dims(ordering_)); optimize2(this->root(), newDelta); if(debug) newDelta.print("newDelta: "); @@ -821,7 +818,7 @@ void ISAM2::update( if(replacedKeys) { BOOST_FOREACH(const Index var, *replacedKeys) { replacedKeysMask[var] = true; } } - lastBacksubVariableCount = optimize2(this->root(), wildfire_threshold, replacedKeysMask, delta_); // modifies delta_ + lastBacksubVariableCount = optimize2(this->root(), params_.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_ #ifndef NDEBUG for(size_t j=0; j lastRelinVariables_; @@ -82,6 +110,9 @@ public: typedef ISAM2 This; ///< This class /** Create an empty ISAM2 instance */ + ISAM2(const ISAM2Params& params); + + /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ ISAM2(); typedef typename BayesTree::sharedClique sharedClique; ///< Shared pointer to a clique @@ -100,17 +131,14 @@ public: * 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 wildfire_threshold The threshold below which the linear solution delta is not - * updated. - * @param relinearize_threshold The threshold on the linear delta below which a variable - * will not be relinearized. - * @param relinearize + * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently + * large (Params::relinearizeThreshold), regardless of the relinearization interval + * (Params::relinearizeSkip). */ void update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, - double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true, bool force_relinearize = false); - /** Access the current linearization point + /** Access the current linearization point */ const VALUES& getLinearizationPoint() const {return theta_;} /** Compute an estimate from the incomplete linear delta computed during the last update. @@ -122,9 +150,7 @@ public: //@{ /** Internal implementation functions */ - struct Impl { - static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); - }; + struct Impl; /** Compute an estimate using a complete delta computed by a full back-substitution. */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 159206f90..0aea87e37 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -149,12 +149,11 @@ TEST(ISAM2, slamlike_solution) typedef planarSLAM::PointKey PointKey; // Set up parameters - double wildfire = 0.001; SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam; + GaussianISAM2 isam(ISAM2Params(0.001, 0.0, 0, false)); planarSLAM::Values fullinit; planarSLAM::Graph fullgraph; @@ -171,7 +170,7 @@ TEST(ISAM2, slamlike_solution) init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - isam.update(newfactors, init, wildfire, 0.0, false); + isam.update(newfactors, init); } EXPECT(isam_check(fullgraph, fullinit, isam)); @@ -186,7 +185,7 @@ TEST(ISAM2, slamlike_solution) init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - isam.update(newfactors, init, wildfire, 0.0, false); + isam.update(newfactors, init); } // Add odometry from time 5 to 6 and landmark measurement at time 5 @@ -205,7 +204,7 @@ TEST(ISAM2, slamlike_solution) fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - isam.update(newfactors, init, wildfire, 0.0, false); + isam.update(newfactors, init); ++ i; } @@ -219,7 +218,7 @@ TEST(ISAM2, slamlike_solution) init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - isam.update(newfactors, init, wildfire, 0.0, false); + isam.update(newfactors, init); } // Add odometry from time 10 to 11 and landmark measurement at time 10 @@ -234,7 +233,7 @@ TEST(ISAM2, slamlike_solution) init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - isam.update(newfactors, init, wildfire, 0.0, false); + isam.update(newfactors, init); ++ i; }