ISAM2 parameters struct instead of function arguments and hard-coded values, also documentation improvement and refactored IndicesFromFactors

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 VALUES>
-class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES> {};
+class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES> {
+public:
+  /** Create an empty ISAM2 instance */
+  GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES>(params) {}
+
+  /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
+  GaussianISAM2() : ISAM2<GaussianConditional, VALUES>() {}
+};
 
 // optimize the BayesTree, starting from the root
 void optimize2(const BayesTree<GaussianConditional>::sharedClique& root, VectorValues& delta);

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -4,10 +4,21 @@
  * @author  Michael Kaess, Richard Roberts
  */
 
+#include <gtsam/base/FastSet.h>
+
+#include <boost/foreach.hpp>
+
+#include <vector>
+
 namespace gtsam {
 
+template<class CONDITIONAL, class VALUES>
+struct ISAM2<CONDITIONAL, VALUES>::Impl {
+   static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
+   static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors);
+};
+
 /* ************************************************************************* */
-template <class UNIQUE>
 struct _VariableAdder {
   Ordering& ordering;
   Permuted<VectorValues>& vconfig;
@@ -36,7 +47,7 @@ void ISAM2<CONDITIONAL,VALUES>::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<ISAM2<CONDITIONAL,VALUES> > 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<CONDITIONAL,VALUES>::Impl::AddVariables(
   nodes.resize(ordering.nVars());
 }
 
+/* ************************************************************************* */
+template<class CONDITIONAL, class VALUES>
+FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors) {
+  FastSet<Index> indices;
+  BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
+    BOOST_FOREACH(const Symbol& key, factor->keys()) {
+      indices.insert(ordering[key]);
+    }
+  }
+  return indices;
+}
+
 }

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<class Conditional, class Values>
-ISAM2<Conditional, Values>::ISAM2() : BayesTree<Conditional>(), delta_(Permutation(), deltaUnpermuted_) {}
+ISAM2<Conditional, Values>::ISAM2(const ISAM2Params& params):
+    delta_(Permutation(), deltaUnpermuted_), params_(params) {}
+
+/* ************************************************************************* */
+template<class Conditional, class Values>
+ISAM2<Conditional, Values>::ISAM2():
+    delta_(Permutation(), deltaUnpermuted_) {}
 
 /** Create a Bayes Tree from a nonlinear factor graph */
 //template<class Conditional, class Values>
@@ -670,12 +676,10 @@ struct _SelectiveExpmapAndClear {
 /* ************************************************************************* */
 template<class Conditional, class Values>
 void ISAM2<Conditional, Values>::update(
-    const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta,
+    const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta, bool force_relinearize) {
-    double wildfire_threshold, double relinearize_threshold, bool relinearize, 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<Conditional, Values>::update(
 
   tic(3,"gather involved keys");
   // 3. Mark linear update
-  FastSet<Index> markedKeys;
+  FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
+  vector<Index> newKeys; newKeys.reserve(markedKeys.size());
+  newKeys.assign(markedKeys.begin(), markedKeys.end());                        // Make a copy of these, as we'll soon add to them
   FastSet<Index> structuralKeys;
-  vector<Index> newKeys; newKeys.reserve(newFactors.size() * 6);
+  if(structuralLast) structuralKeys = markedKeys;                              // If we're using structural-last ordering, make another copy
-  BOOST_FOREACH(const typename NonlinearFactor<Values>::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);
-    }
-  }
   toc(3,"gather involved keys");
 
-
   vector<bool> 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<delta_.size(); ++var) {
       //cout << var << ": " << delta_[var].transpose() << endl;
       double maxDelta = delta_[var].lpNorm<Eigen::Infinity>();
-      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<Conditional, Values>::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<Conditional, Values>::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<delta_.container().size(); ++j)

gtsam/nonlinear/ISAM2.h

@@ -30,10 +30,35 @@
 namespace gtsam {
 
 /**
+ * @defgroup ISAM2
+ */
+
+/**
+ * @ingroup ISAM2
+ * Parameters for the ISAM2 algorithm.  Default parameter values are listed below.
+ */
+struct ISAM2Params {
+  double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001)
+  double relinearizeThreshold; ///< Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0.1)
+  int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
+  bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true)
+
+  /** Specify parameters as constructor arguments */
+  ISAM2Params(
+      double _wildfireThreshold = 0.001, ///< ISAM2Params::wildfireThreshold
+      double _relinearizeThreshold = 0.1, ///< ISAM2Params::relinearizeThreshold
+      int _relinearizeSkip = 10, ///< ISAM2Params::relinearizeSkip
+      bool _enableRelinearization = true ///< ISAM2Params::enableRelinearization
+  ) : wildfireThreshold(_wildfireThreshold), relinearizeThreshold(_relinearizeThreshold),
+      relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization) {}
+};
+
+/**
+ * @ingroup ISAM2
  * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
  *
- * The typical cycle of using this class to create an instance using the default
+ * The typical cycle of using this class to create an instance by providing ISAM2Params
- * constructor, then add measurements and variables as they arrive using the update()
+ * to the constructor, then add measurements and variables as they arrive using the update()
  * method.  At any time, calculateEstimate() may be called to obtain the current
  * estimate of all variables.
  */
@@ -69,6 +94,9 @@ protected:
    */
   Ordering ordering_;
 
+  /** The current parameters */
+  ISAM2Params params_;
+
 private:
 #ifndef NDEBUG
   std::vector<bool> lastRelinVariables_;
@@ -82,6 +110,9 @@ public:
   typedef ISAM2<CONDITIONAL, VALUES> 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<CONDITIONAL>::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
+   * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
-   * updated.
+   * large (Params::relinearizeThreshold), regardless of the relinearization interval
-   * @param relinearize_threshold The threshold on the linear delta below which a variable
+   * (Params::relinearizeSkip).
-   * will not be relinearized.
-   * @param relinearize
    */
   void update(const NonlinearFactorGraph<VALUES>& 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 {
+  struct Impl;
-    static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
-  };
 
   /** Compute an estimate using a complete delta computed by a full back-substitution.
    */

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<planarSLAM::Values> isam;
+  GaussianISAM2<planarSLAM::Values> 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;
   }