From 6f7e92afdc1dfa5b85a73b9861dfec614a956b5f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 3 Jun 2019 14:12:31 -0400
Subject: [PATCH] Make error calculation concise

---
 gtsam/nonlinear/ISAM2-impl.h |  7 +++++++
 gtsam/nonlinear/ISAM2.cpp    | 12 ++++--------
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h
index f668e12c7..1dba44713 100644
--- a/gtsam/nonlinear/ISAM2-impl.h
+++ b/gtsam/nonlinear/ISAM2-impl.h
@@ -185,6 +185,13 @@ struct GTSAM_EXPORT UpdateImpl {
                         std::inserter(*unusedKeys, unusedKeys->end()));
   }
 
+  // Calculate nonlinear error
+  void error(const NonlinearFactorGraph& nonlinearFactors,
+             const Values& estimate, boost::optional<double>* error) const {
+    gttic(error);
+    error->reset(nonlinearFactors.error(estimate));
+  }
+
   // Mark linear update
   void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
                           const NonlinearFactorGraph& nonlinearFactors,
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 4e4a507bf..9394b3201 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -367,6 +367,8 @@ void ISAM2::addVariables(const Values& newTheta,
 
 /* ************************************************************************* */
 void ISAM2::removeVariables(const KeySet& unusedKeys) {
+  gttic(removeVariables);
+
   variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
   for (Key key : unusedKeys) {
     delta_.erase(key);
@@ -431,10 +433,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
   // \Theta:=\Theta\cup\Theta_{new}.
   addVariables(newTheta, result.detail.get_ptr());
 
-  gttic(evaluate_error_before);
   if (params_.evaluateNonlinearError)
-    result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
-  gttoc(evaluate_error_before);
+    update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore);
 
   // 3. Mark linear update
   update.gatherInvolvedKeys(newFactors, nonlinearFactors_,
@@ -490,16 +490,12 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
 
   // Update data structures to remove unused keys
   if (!result.unusedKeys.empty()) {
-    gttic(remove_variables);
     removeVariables(result.unusedKeys);
-    gttoc(remove_variables);
   }
   result.cliques = this->nodes().size();
 
-  gttic(evaluate_error_after);
   if (params_.evaluateNonlinearError)
-    result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
-  gttoc(evaluate_error_after);
+    update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter);
 
   return result;
 }