Made detail handling more explicit in update

gtsam/nonlinear/ISAM2-impl.h

@@ -218,7 +218,7 @@ struct GTSAM_EXPORT UpdateImpl {
   void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
     gttic(updateKeys);
     // Observed keys for detailed results
-    if (params_.enableDetailedResults) {
+    if (result->detail && params_.enableDetailedResults) {
       for (Key key : markedKeys) {
         result->detail->variableStatus[key].isObserved = true;
       }
@@ -359,8 +359,8 @@ struct GTSAM_EXPORT UpdateImpl {
   // Mark keys in \Delta above threshold \beta:
   KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
                                const VectorValues& delta,
-                               const KeySet& fixedVariables, KeySet* markedKeys,
+                               const KeySet& fixedVariables,
-                               ISAM2Result* result) const {
+                               KeySet* markedKeys) const {
     gttic(gatherRelinearizeKeys);
     // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
     KeySet relinKeys =
@@ -381,37 +381,41 @@ struct GTSAM_EXPORT UpdateImpl {
       }
     }
 
-    // Record above relinerization threshold keys in detailed results
-    if (params_.enableDetailedResults) {
-      for (Key key : relinKeys) {
-        result->detail->variableStatus[key].isAboveRelinThreshold = true;
-        result->detail->variableStatus[key].isRelinearized = true;
-      }
-    }
-
     // Add the variables being relinearized to the marked keys
     markedKeys->insert(relinKeys.begin(), relinKeys.end());
     return relinKeys;
   }
 
+  // Record relinerization threshold keys in detailed results
+  void recordRelinearizeDetail(const KeySet& relinKeys,
+                               ISAM2Result::DetailedResults* detail) const {
+    if (detail && params_.enableDetailedResults) {
+      for (Key key : relinKeys) {
+        detail->variableStatus[key].isAboveRelinThreshold = true;
+        detail->variableStatus[key].isRelinearized = true;
+      }
+    }
+  }
+
   // Mark all cliques that involve marked variables \Theta_{J} and all
   // their ancestors.
   void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys,
-                    KeySet* markedKeys, ISAM2Result* result) const {
+                    KeySet* markedKeys,
+                    ISAM2Result::DetailedResults* detail) const {
     gttic(fluidFindAll);
     for (const auto& root : roots)
       // add other cliques that have the marked ones in the separator
       root->findAll(relinKeys, markedKeys);
 
     // Relinearization-involved keys for detailed results
-    if (params_.enableDetailedResults) {
+    if (detail && params_.enableDetailedResults) {
       KeySet involvedRelinKeys;
       for (const auto& root : roots)
         root->findAll(relinKeys, &involvedRelinKeys);
       for (Key key : involvedRelinKeys) {
-        if (!result->detail->variableStatus[key].isAboveRelinThreshold) {
+        if (!detail->variableStatus[key].isAboveRelinThreshold) {
-          result->detail->variableStatus[key].isRelinearizeInvolved = true;
+          detail->variableStatus[key].isRelinearizeInvolved = true;
-          result->detail->variableStatus[key].isRelinearized = true;
+          detail->variableStatus[key].isRelinearized = true;
         }
       }
     }

gtsam/nonlinear/ISAM2.cpp

@@ -337,7 +337,7 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
   }
 
   // Root clique variables for detailed results
-  if (params_.enableDetailedResults) {
+  if (result->detail && params_.enableDetailedResults) {
     for (const auto& root : roots_)
       for (Key var : *root->conditional())
         result->detail->variableStatus[var].inRootClique = true;
@@ -346,9 +346,8 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
   return affectedKeysSet;
 }
 /* ************************************************************************* */
-void ISAM2::addVariables(
+void ISAM2::addVariables(const Values& newTheta,
-    const Values& newTheta,
+                         ISAM2Result::DetailedResults* detail) {
-    ISAM2Result::DetailedResults::StatusMap* variableStatus) {
   gttic(addNewVariables);
 
   theta_.insert(newTheta);
@@ -359,9 +358,9 @@ void ISAM2::addVariables(
   RgProd_.insert(newTheta.zeroVectors());
 
   // New keys for detailed results
-  if (variableStatus && params_.enableDetailedResults) {
+  if (detail && params_.enableDetailedResults) {
     for (Key key : newTheta.keys()) {
-      (*variableStatus)[key].isNew = true;
+      detail->variableStatus[key].isNew = true;
     }
   }
 }
@@ -430,7 +429,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
 
   // 2. Initialize any new variables \Theta_{new} and add
   // \Theta:=\Theta\cup\Theta_{new}.
-  addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0);
+  addVariables(newTheta, result.detail.get_ptr());
 
   gttic(evaluate_error_before);
   if (params_.evaluateNonlinearError)
@@ -448,11 +447,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
   if (relinearizeThisStep) {
     // 4. Mark keys in \Delta above threshold \beta:
     relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_,
-                                             &result.markedKeys, &result);
+                                             &result.markedKeys);
+    update.recordRelinearizeDetail(relinKeys, result.detail.get_ptr());
     if (!relinKeys.empty()) {
       // 5. Mark all cliques that involve marked variables \Theta_{J} and all
       // their ancestors.
-      update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result);
+      update.fluidFindAll(roots_, relinKeys, &result.markedKeys,
+                          result.detail.get_ptr());
       // 6. Update linearization point for marked variables:
       // \Theta_{J}:=\Theta_{J}+\Delta_{J}.
       UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_);

gtsam/nonlinear/ISAM2.h

@@ -307,9 +307,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    * @param newTheta Initial values for new variables
    * @param variableStatus optional detailed result structure
    */
-  void addVariables(
+  void addVariables(const Values& newTheta,
-      const Values& newTheta,
+                    ISAM2Result::DetailedResults* detail = 0);
-      ISAM2Result::DetailedResults::StatusMap* variableStatus = 0);
 
   /**
    * Remove variables from the ISAM2 system.

gtsam/nonlinear/ISAM2Params.h

@@ -234,8 +234,8 @@ struct GTSAM_EXPORT ISAM2Params {
               Factorization _factorization = ISAM2Params::CHOLESKY,
               bool _cacheLinearizedFactors = true,
               const KeyFormatter& _keyFormatter =
-                  DefaultKeyFormatter  ///< see ISAM2::Params::keyFormatter
+                  DefaultKeyFormatter,  ///< see ISAM2::Params::keyFormatter,
-              )
+              bool _enableDetailedResults = false)
       : optimizationParams(_optimizationParams),
         relinearizeThreshold(_relinearizeThreshold),
         relinearizeSkip(_relinearizeSkip),
@@ -244,7 +244,7 @@ struct GTSAM_EXPORT ISAM2Params {
         factorization(_factorization),
         cacheLinearizedFactors(_cacheLinearizedFactors),
         keyFormatter(_keyFormatter),
-        enableDetailedResults(false),
+        enableDetailedResults(_enableDetailedResults),
         enablePartialRelinearizationCheck(false),
         findUnusedFactorSlots(false) {}
 

tests/testGaussianISAM2.cpp

@@ -47,9 +47,11 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0,
 ISAM2 createSlamlikeISAM2(
     boost::optional<Values&> init_values = boost::none,
     boost::optional<NonlinearFactorGraph&> full_graph = boost::none,
-    const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true),
+    const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0,
+                                            0, false, true,
+                                            ISAM2Params::CHOLESKY, true,
+                                            DefaultKeyFormatter, true),
     size_t maxPoses = 10) {
-
   // These variables will be reused and accumulate factors and values
   ISAM2 isam(params);
   Values fullinit;