Detailed results optionally returned by ISAM2::update, with the status of each variable

gtsam/nonlinear/ISAM2.cpp

@@ -206,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
 }
 
 boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
-    const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& newKeys,
+    const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys,
     const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
 
   // TODO:  new factors are linearized twice, the newFactors passed in are not used.
@@ -237,8 +237,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     cout << "markedKeys: ";
     BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; }
     cout << endl;
-    cout << "newKeys: ";
+    cout << "observedKeys: ";
-    BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; }
+    BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; }
     cout << endl;
   }
 
@@ -270,12 +270,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
   FastList<Index> affectedKeys = affectedBayesNet.ordering();
   toc(2,"affectedKeys");
 
+  boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result
+
   if(affectedKeys.size() >= theta_.size() * batchThreshold) {
 
     tic(3,"batch");
 
     tic(0,"add keys");
-    boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>());
     BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); }
     toc(0,"add keys");
 
@@ -296,8 +297,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
         }
       }
     } else {
-      if(theta_.size() > newKeys.size()) { // Only if some variables are unconstrained
+      if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained
-        BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; }
+        BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; }
       }
     }
     Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
@@ -338,15 +339,17 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     this->insert(newRoot);
     toc(6,"insert");
 
-    toc(3,"batch");
-
     result.variablesReeliminated = affectedKeysSet->size();
 
     lastAffectedMarkedCount = markedKeys.size();
     lastAffectedVariableCount = affectedKeysSet->size();
     lastAffectedFactorCount = linearFactors_.size();
 
-    return affectedKeysSet;
+    // Reeliminated keys for detailed results
+    if(params_.enableDetailedResults)
+      BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; }
+
+    toc(3,"batch");
 
   } else {
 
@@ -355,7 +358,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     // 2. Add the new factors \Factors' into the resulting factor graph
     FastList<Index> affectedAndNewKeys;
     affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
-    affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
+    affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end());
     tic(1,"relinearizeAffected");
     GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
     if(debug) factors.print("Relinearized factors: ");
@@ -363,6 +366,10 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
 
     if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
 
+    // Reeliminated keys for detailed results
+    if(params_.enableDetailedResults)
+      BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; }
+
     result.variablesReeliminated = affectedAndNewKeys.size();
     lastAffectedMarkedCount = markedKeys.size();
     lastAffectedVariableCount = affectedKeys.size();
@@ -391,7 +398,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     tic(1,"list to set");
     // create a partial reordering for the new and contaminated factors
     // markedKeys are passed in: those variables will be forced to the end in the ordering
-    boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>(markedKeys));
+    affectedKeysSet->insert(markedKeys.begin(), markedKeys.end());
     affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
     toc(1,"list to set");
 
@@ -404,7 +411,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
       reorderingMode.constrainedKeys = *constrainKeys;
     } else {
       reorderingMode.constrainedKeys = FastMap<Index,int>();
-      BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
+      BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
     }
     Impl::PartialSolveResult partialSolveResult =
         Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
@@ -467,9 +474,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     toc(7,"orphans");
 
     toc(4,"incremental");
-
-    return affectedKeysSet;
   }
+
+  // Root clique variables for detailed results
+  if(params_.enableDetailedResults)
+    BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; }
+
+  return affectedKeysSet;
 }
 
 /* ************************************************************************* */
@@ -490,6 +501,8 @@ ISAM2Result ISAM2::update(
   lastBacksubVariableCount = 0;
   lastNnzTop = 0;
   ISAM2Result result;
+  if(params_.enableDetailedResults)
+    result.detail = ISAM2Result::DetailedResults();
   const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0);
 
   if(verbose) {
@@ -528,6 +541,10 @@ ISAM2Result ISAM2::update(
   tic(2,"add new variables");
   // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
   Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_);
+  // New keys for detailed results
+  if(params_.enableDetailedResults) {
+    inverseOrdering_ = ordering_.invert();
+    BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
   toc(2,"add new variables");
 
   tic(3,"evaluate error before");
@@ -543,10 +560,13 @@ ISAM2Result ISAM2::update(
     FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
     markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
   }
+  // Observed keys for detailed results
+  if(params_.enableDetailedResults)
+    BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; }
   // NOTE: we use assign instead of the iterator constructor here because this
   // 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
+  FastVector<Index> observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
   toc(4,"gather involved keys");
 
   // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
@@ -558,6 +578,12 @@ ISAM2Result ISAM2::update(
     relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
     if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging
 
+    // Above relin threshold keys for detailed results
+    if(params_.enableDetailedResults) {
+      BOOST_FOREACH(Index index, relinKeys) {
+        result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true;
+        result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
+
     // Add the variables being relinearized to the marked keys
     BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
     markedKeys.insert(relinKeys.begin(), relinKeys.end());
@@ -565,8 +591,20 @@ ISAM2Result ISAM2::update(
 
     tic(6,"fluid find_all");
     // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
-    if (!relinKeys.empty() && this->root())
+    if (!relinKeys.empty() && this->root()) {
-      Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator
+      // add other cliques that have the marked ones in the separator
+      Impl::FindAll(this->root(), markedKeys, markedRelinMask);
+
+      // Relin involved keys for detailed results
+      if(params_.enableDetailedResults) {
+        FastSet<Index> involvedRelinKeys;
+        Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask);
+        BOOST_FOREACH(Index index, involvedRelinKeys) {
+          if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) {
+            result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true;
+            result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
+      }
+    }
     toc(6,"fluid find_all");
 
     tic(7,"expmap");
@@ -617,8 +655,8 @@ ISAM2Result ISAM2::update(
     }
   }
   boost::shared_ptr<FastSet<Index> > replacedKeys;
-  if(!markedKeys.empty() || !newKeys.empty())
+  if(!markedKeys.empty() || !observedKeys.empty())
-    replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result);
+    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if(replacedKeys) {

gtsam/nonlinear/ISAM2.h

@@ -127,6 +127,8 @@ struct ISAM2Params {
 
   KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
 
+  bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false)
+
   /** Specify parameters as constructor arguments */
   ISAM2Params(
       OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
@@ -140,7 +142,8 @@ struct ISAM2Params {
   ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
       relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
       evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
-      cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {}
+      cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
+      enableDetailedResults(false) {}
 };
 
 /**
@@ -201,6 +204,35 @@ struct ISAM2Result {
    * used later to refer to the factors in order to remove them.
    */
   FastVector<size_t> newFactorsIndices;
+
+  /** A struct holding detailed results, which must be enabled with
+   * ISAM2Params::enableDetailedResults.
+   */
+  struct DetailedResults {
+    /** The status of a single variable, this struct is stored in
+     * DetailedResults::variableStatus */
+    struct VariableStatus {
+      /** Whether the variable was just reeliminated, due to being relinearized,
+       * observed, new, or on the path up to the root clique from another
+       * reeliminated variable. */
+      bool isReeliminated;
+      bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold
+      bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold
+      bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement.
+      bool isObserved; ///< Whether the variable was just involved in new factors
+      bool isNew; ///< Whether the variable itself was just added
+      bool inRootClique; ///< Whether the variable is in the root clique
+      VariableStatus(): isReeliminated(false), isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {}
+    };
+
+    /** The status of each variable during this update, see VariableStatus.
+     */
+    FastMap<Key, VariableStatus> variableStatus;
+  };
+
+  /** Detailed results, if enabled by ISAM2Params::enableDetailedResults.  See
+   * Detail for information about the results data stored here. */
+  boost::optional<DetailedResults> detail;
 };
 
 struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
@@ -364,6 +396,9 @@ protected:
   /** The current Dogleg Delta (trust region radius) */
   mutable boost::optional<double> doglegDelta_;
 
+  /** The inverse ordering, only used for creating ISAM2Result::DetailedResults */
+  boost::optional<Ordering::InvertedMap> inverseOrdering_;
+
 private:
 #ifndef NDEBUG
   std::vector<bool> lastRelinVariables_;
@@ -468,7 +503,7 @@ private:
   GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
 
   boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
-      const FastVector<Index>& newKeys,
+      const FastVector<Index>& observedKeys,
       const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;