Added ISAM2 result struct to return information about updates

gtsam/nonlinear/ISAM2-inl.h

@@ -156,7 +156,7 @@ ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
 
 template<class Conditional, class Values>
 boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
-    const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors) {
+    const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, ISAM2Result& result) {
 
   // TODO:  new factors are linearized twice, the newFactors passed in are not used.
 
@@ -298,6 +298,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
 
     toc(3,"batch");
 
+    result.variablesReeliminated = affectedKeysSet->size();
+
     lastAffectedMarkedCount = markedKeys.size();
     lastAffectedVariableCount = affectedKeysSet->size();
     lastAffectedFactorCount = factors.size();
@@ -318,6 +320,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
 
     if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
 
+    result.variablesReeliminated = affectedAndNewKeys.size();
     lastAffectedMarkedCount = markedKeys.size();
     lastAffectedVariableCount = affectedKeys.size();
     lastAffectedFactorCount = factors.size();
@@ -416,7 +419,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
 
 /* ************************************************************************* */
 template<class Conditional, class Values>
-void ISAM2<Conditional, Values>::update(
+ISAM2Result ISAM2<Conditional, Values>::update(
     const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta, bool force_relinearize) {
 
   static const bool debug = ISDEBUG("ISAM2 update");
@@ -431,22 +434,28 @@ void ISAM2<Conditional, Values>::update(
   lastAffectedMarkedCount = 0;
   lastBacksubVariableCount = 0;
   lastNnzTop = 0;
+  ISAM2Result result;
 
   if(verbose) {
     cout << "ISAM2::update\n";
     this->print("ISAM2: ");
   }
 
-  tic(1,"push_back factors");
+  tic(0,"push_back factors");
   // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
   if(debug || verbose) newFactors.print("The new factors are: ");
   nonlinearFactors_.push_back(newFactors);
-  toc(1,"push_back factors");
+  toc(0,"push_back factors");
 
-  tic(2,"add new variables");
+  tic(1,"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(2,"add new variables");
+  toc(1,"add new variables");
+
+  tic(2,"evaluate error before");
+  if(params_.evaluateNonlinearError)
+    result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
+  toc(2,"evaluate error before");
 
   tic(3,"gather involved keys");
   // 3. Mark linear update
@@ -481,11 +490,14 @@ void ISAM2<Conditional, Values>::update(
       Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_);
     toc(6,"expmap");
 
+    result.variablesRelinearized = markedRelinMask.size();
+
 #ifndef NDEBUG
     lastRelinVariables_ = markedRelinMask;
 #endif
   } else {
 #ifndef NDEBUG
+    result.variablesRelinearized = 0;
     lastRelinVariables_ = vector<bool>(ordering_.nVars(), false);
 #endif
   }
@@ -506,7 +518,7 @@ void ISAM2<Conditional, Values>::update(
   // 8. Redo top of Bayes tree
   boost::shared_ptr<FastSet<Index> > replacedKeys;
   if(!markedKeys.empty() || !newKeys.empty())
-    replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors);
+    replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors, result);
   toc(8,"recalculate");
 
   tic(9,"solve");
@@ -532,6 +544,13 @@ void ISAM2<Conditional, Values>::update(
 #endif
   }
   toc(9,"solve");
+
+  tic(10,"evaluate error after");
+  if(params_.evaluateNonlinearError)
+    result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
+  toc(10,"evaluate error after");
+
+  return result;
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/ISAM2.h

@@ -50,15 +50,69 @@ struct ISAM2Params {
   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)
+  bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
 
   /** 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
+      bool _enableRelinearization = true, ///< ISAM2Params::enableRelinearization
+      bool _evaluateNonlinearError = false ///< ISAM2Params::evaluateNonlinearError
   ) : wildfireThreshold(_wildfireThreshold), relinearizeThreshold(_relinearizeThreshold),
-      relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization) {}
+      relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
+      evaluateNonlinearError(_evaluateNonlinearError) {}
+};
+
+/**
+ * @ingroup ISAM2
+ * This struct is returned from ISAM2::update() and contains information about
+ * the update that is useful for determining whether the solution is
+ * converging, and about how much work was required for the update.  See member
+ * variables for details and information about each entry.
+ */
+struct ISAM2Result {
+  /** The nonlinear error of all of the factors, \a including new factors and
+   * variables added during the current call to ISAM2::update().  This error is
+   * calculated using the following variable values:
+   * \li Pre-existing variables will be evaluated by combining their
+   * linearization point before this call to update, with their partial linear
+   * delta, as computed by ISAM2::calculateEstimate().
+   * \li New variables will be evaluated at their initialization points passed
+   * into the current call to update.
+   * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError
+   * is set to \c true, because there is some cost to this computation.
+   */
+  boost::optional<double> errorBefore;
+
+  /** The nonlinear error of all of the factors computed after the current
+   * update, meaning that variables above the relinearization threshold
+   * (ISAM2Params::relinearizeThreshold) have been relinearized and new
+   * variables have undergone one linear update.  Variable values are
+   * again computed by combining their linearization points with their
+   * partial linear deltas, by ISAM2::calculateEstimate().
+   * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError
+   * is set to \c true, because there is some cost to this computation.
+   */
+  boost::optional<double> errorAfter;
+
+  /** The number of variables that were relinearized because their linear
+   * deltas exceeded the reslinearization threshold
+   * (ISAM2Params::relinearizeThreshold), combined with any additional
+   * variables that had to be relinearized because they were involved in
+   * the same factor as a variable above the relinearization threshold.
+   * On steps where no relinearization is considered
+   * (see ISAM2Params::relinearizeSkip), this count will be zero.
+   */
+  size_t variablesRelinearized;
+
+  /** The number of variables that were reeliminated as parts of the Bayes'
+   * Tree were recalculated, due to new factors.  When loop closures occur,
+   * this count will be large as the new loop-closing factors will tend to
+   * involve variables far away from the root, and everything up to the root
+   * will be reeliminated.
+   */
+  size_t variablesReeliminated;
 };
 
 /**
@@ -142,8 +196,9 @@ public:
    * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
    * large (Params::relinearizeThreshold), regardless of the relinearization interval
    * (Params::relinearizeSkip).
+   * @return An ISAM2Result struct containing information about the update
    */
-  void update(const NonlinearFactorGraph<VALUES>& newFactors, const VALUES& newTheta,
+  ISAM2Result update(const NonlinearFactorGraph<VALUES>& newFactors, const VALUES& newTheta,
       bool force_relinearize = false);
 
   /** Access the current linearization point */
@@ -189,7 +244,7 @@ private:
   FactorGraph<CacheFactor> getCachedBoundaryFactors(Cliques& orphans);
 
   boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys,
-      const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors = FactorGraph<GaussianFactor>::shared_ptr());
+      const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
 
 }; // ISAM2