Cleaned up cpplint errors - no functionality change

gtsam/nonlinear/ISAM2.cpp

@@ -247,7 +247,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
 /* ************************************************************************* */
 // find intermediate (linearized) factors from cache that are passed into the affected area
 
-GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
+GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) {
   GaussianFactorGraph cachedBoundary;
 
   for(sharedClique orphan: orphans) {

gtsam/nonlinear/ISAM2.h

@@ -11,7 +11,8 @@
 
 /**
  * @file    ISAM2.h
- * @brief   Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
+ * @brief   Incremental update functionality (ISAM2) for BayesTree, with fluid
+ * relinearization.
  * @author  Michael Kaess, Richard Roberts
  */
 
@@ -19,9 +20,12 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <string>
-#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
+#include <vector>
+
 #include <gtsam/linear/GaussianBayesTree.h>
+#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 #include <boost/variant.hpp>
 
@@ -34,21 +38,29 @@ namespace gtsam {
  * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&).
  */
 struct GTSAM_EXPORT ISAM2GaussNewtonParams {
-  double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001)
+  double
+      wildfireThreshold;  ///< Continue updating the linear delta only when
+                          ///< changes are above this threshold (default: 0.001)
 
   /** Specify parameters as constructor arguments */
   ISAM2GaussNewtonParams(
-      double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold
+      double _wildfireThreshold =
-  ) : wildfireThreshold(_wildfireThreshold) {}
+          0.001  ///< see ISAM2GaussNewtonParams public variables,
+                 ///< ISAM2GaussNewtonParams::wildfireThreshold
+      )
+      : wildfireThreshold(_wildfireThreshold) {}
 
   void print(const std::string str = "") const {
-    std::cout << str << "type:              ISAM2GaussNewtonParams\n";
+    using std::cout;
-    std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
+    cout << str << "type:              ISAM2GaussNewtonParams\n";
-    std::cout.flush();
+    cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
+    cout.flush();
   }
 
   double getWildfireThreshold() const { return wildfireThreshold; }
-  void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
+  void setWildfireThreshold(double wildfireThreshold) {
+    this->wildfireThreshold = wildfireThreshold;
+  }
 };
 
 /**
@@ -58,51 +70,81 @@ struct GTSAM_EXPORT ISAM2GaussNewtonParams {
  * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&).
  */
 struct GTSAM_EXPORT ISAM2DoglegParams {
-  double initialDelta; ///< The initial trust region radius for Dogleg
+  double initialDelta;  ///< The initial trust region radius for Dogleg
-  double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
+  double
-  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode
+      wildfireThreshold;  ///< Continue updating the linear delta only when
-  bool verbose; ///< Whether Dogleg prints iteration and convergence information
+                          ///< changes are above this threshold (default: 1e-5)
+  DoglegOptimizerImpl::TrustRegionAdaptationMode
+      adaptationMode;  ///< See description in
+                       ///< DoglegOptimizerImpl::TrustRegionAdaptationMode
+  bool
+      verbose;  ///< Whether Dogleg prints iteration and convergence information
 
   /** Specify parameters as constructor arguments */
   ISAM2DoglegParams(
-      double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta
+      double _initialDelta = 1.0,  ///< see ISAM2DoglegParams::initialDelta
-      double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold
+      double _wildfireThreshold =
-      DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode
+          1e-5,  ///< see ISAM2DoglegParams::wildfireThreshold
-      bool _verbose = false ///< see ISAM2DoglegParams::verbose
+      DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode =
-  ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold),
+          DoglegOptimizerImpl::
-  adaptationMode(_adaptationMode), verbose(_verbose) {}
+              SEARCH_EACH_ITERATION,  ///< see ISAM2DoglegParams::adaptationMode
+      bool _verbose = false           ///< see ISAM2DoglegParams::verbose
+      )
+      : initialDelta(_initialDelta),
+        wildfireThreshold(_wildfireThreshold),
+        adaptationMode(_adaptationMode),
+        verbose(_verbose) {}
 
   void print(const std::string str = "") const {
-    std::cout << str << "type:              ISAM2DoglegParams\n";
+    using std::cout;
-    std::cout << str << "initialDelta:      " << initialDelta << "\n";
+    cout << str << "type:              ISAM2DoglegParams\n";
-    std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
+    cout << str << "initialDelta:      " << initialDelta << "\n";
-    std::cout << str << "adaptationMode:    " << adaptationModeTranslator(adaptationMode) << "\n";
+    cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
-    std::cout.flush();
+    cout << str
+         << "adaptationMode:    " << adaptationModeTranslator(adaptationMode)
+         << "\n";
+    cout.flush();
   }
 
   double getInitialDelta() const { return initialDelta; }
   double getWildfireThreshold() const { return wildfireThreshold; }
-  std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); };
+  std::string getAdaptationMode() const {
-  bool isVerbose() const { return verbose; };
+    return adaptationModeTranslator(adaptationMode);
+  }
+  bool isVerbose() const { return verbose; }
+  void setInitialDelta(double initialDelta) {
+    this->initialDelta = initialDelta;
+  }
+  void setWildfireThreshold(double wildfireThreshold) {
+    this->wildfireThreshold = wildfireThreshold;
+  }
+  void setAdaptationMode(const std::string& adaptationMode) {
+    this->adaptationMode = adaptationModeTranslator(adaptationMode);
+  }
+  void setVerbose(bool verbose) { this->verbose = verbose; }
 
-  void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; }
+  std::string adaptationModeTranslator(
-  void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
+      const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode)
-  void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); }
+      const;
-  void setVerbose(bool verbose) { this->verbose = verbose; };
+  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(
-
+      const std::string& adaptationMode) const;
-  std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const;
-  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const;
 };
 
 /**
  * @addtogroup ISAM2
- * Parameters for the ISAM2 algorithm.  Default parameter values are listed below.
+ * Parameters for the ISAM2 algorithm.  Default parameter values are listed
+ * below.
  */
-typedef FastMap<char,Vector> ISAM2ThresholdMap;
+typedef FastMap<char, Vector> ISAM2ThresholdMap;
 typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
 struct GTSAM_EXPORT ISAM2Params {
-  typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams
+  typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
-  typedef boost::variant<double, FastMap<char,Vector> > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds
+      OptimizationParams;  ///< Either ISAM2GaussNewtonParams or
+                           ///< ISAM2DoglegParams
+  typedef boost::variant<double, FastMap<char, Vector> >
+      RelinearizationThreshold;  ///< Either a constant relinearization
+                                 ///< threshold or a per-variable-type set of
+                                 ///< thresholds
 
   /** Optimization parameters, this both selects the nonlinear optimization
    * method and specifies its parameters, either ISAM2GaussNewtonParams or
@@ -122,27 +164,36 @@ struct GTSAM_EXPORT ISAM2Params {
    * entries would be added with:
    * \code
      FastMap<char,Vector> thresholds;
-     thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold
+     thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished();
-     thresholds['l'] = Vector3(1.0, 1.0, 1.0);                // 1.0 m landmark position threshold
+   // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] =
+   Vector3(1.0, 1.0, 1.0);                // 1.0 m landmark position threshold
      params.relinearizeThreshold = thresholds;
      \endcode
    */
   RelinearizationThreshold relinearizeThreshold;
 
-  int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
+  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 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()
+  bool evaluateNonlinearError;  ///< Whether to evaluate the nonlinear error
+                                ///< before and after the update, to return in
+                                ///< ISAM2Result from update()
 
   enum Factorization { CHOLESKY, QR };
-  /** Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
+  /** Specifies whether to use QR or CHOESKY numerical factorization (default:
-   * Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when
+   * CHOLESKY). Cholesky is faster but potentially numerically unstable for
-   * uncertainty is very low in some variables (or dimensions of variables) and very high in others.  QR is
+   * poorly-conditioned problems, which can occur when uncertainty is very low
-   * slower but more numerically stable in poorly-conditioned problems.  We suggest using the default of Cholesky
+   * in some variables (or dimensions of variables) and very high in others.  QR
-   * unless gtsam sometimes throws IndefiniteLinearSystemException when your problem's Hessian is actually positive
+   * is slower but more numerically stable in poorly-conditioned problems.  We
-   * definite.  For positive definite problems, numerical error accumulation can cause the problem to become
+   * suggest using the default of Cholesky unless gtsam sometimes throws
-   * numerically negative or indefinite as solving proceeds, especially when using Cholesky.
+   * IndefiniteLinearSystemException when your problem's Hessian is actually
+   * positive definite.  For positive definite problems, numerical error
+   * accumulation can cause the problem to become numerically negative or
+   * indefinite as solving proceeds, especially when using Cholesky.
    */
   Factorization factorization;
 
@@ -153,97 +204,156 @@ struct GTSAM_EXPORT ISAM2Params {
    */
   bool cacheLinearizedFactors;
 
-  KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
+  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)
+  bool enableDetailedResults;  ///< Whether to compute and return
+                               ///< ISAM2Result::detailedResults, this can
+                               ///< increase running time (default: false)
 
-  /** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false).
+  /** Check variables for relinearization in tree-order, stopping the check once
-   * This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate
+   * a variable does not need to be relinearized (default: false). This can
-   * significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance
+   * improve speed by only checking a small part of the top of the tree.
-   * is desired over correctness. Use with caution.
+   * However, variables below the check cut-off can accumulate significant
+   * deltas without triggering relinearization. This is particularly useful in
+   * exploration scenarios where real-time performance is desired over
+   * correctness. Use with caution.
    */
   bool enablePartialRelinearizationCheck;
 
-  /// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to
+  /// When you will be removing many factors, e.g. when using ISAM2 as a
-  /// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of
+  /// fixed-lag smoother, enable this option to add factors in the first
-  /// having to search for slots every time a factor is added.
+  /// available factor slots, to avoid accumulating NULL factor slots, at the
+  /// cost of having to search for slots every time a factor is added.
   bool findUnusedFactorSlots;
 
-  /** Specify parameters as constructor arguments */
+  /**
-  ISAM2Params(
+   * Specify parameters as constructor arguments
-      OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
+   * See the documentation of member variables above.
-      RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold
+   */
-      int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
+  ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(),
-      bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
+              RelinearizationThreshold _relinearizeThreshold = 0.1,
-      bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
+              int _relinearizeSkip = 10, bool _enableRelinearization = true,
-      Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization
+              bool _evaluateNonlinearError = false,
-      bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors
+              Factorization _factorization = ISAM2Params::CHOLESKY,
-      const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
+              bool _cacheLinearizedFactors = true,
-  ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
+              const KeyFormatter& _keyFormatter =
-      relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
+                  DefaultKeyFormatter  ///< see ISAM2::Params::keyFormatter
-      evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
+              )
-      cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
+      : optimizationParams(_optimizationParams),
-      enableDetailedResults(false), enablePartialRelinearizationCheck(false),
+        relinearizeThreshold(_relinearizeThreshold),
-      findUnusedFactorSlots(false) {}
+        relinearizeSkip(_relinearizeSkip),
+        enableRelinearization(_enableRelinearization),
+        evaluateNonlinearError(_evaluateNonlinearError),
+        factorization(_factorization),
+        cacheLinearizedFactors(_cacheLinearizedFactors),
+        keyFormatter(_keyFormatter),
+        enableDetailedResults(false),
+        enablePartialRelinearizationCheck(false),
+        findUnusedFactorSlots(false) {}
 
   /// print iSAM2 parameters
   void print(const std::string& str = "") const {
-    std::cout << str << "\n";
+    using std::cout;
-    if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
+    cout << str << "\n";
-      boost::get<ISAM2GaussNewtonParams>(optimizationParams).print("optimizationParams:                ");
+
-    else if(optimizationParams.type() == typeid(ISAM2DoglegParams))
+    static const std::string kStr("optimizationParams:                ");
-      boost::get<ISAM2DoglegParams>(optimizationParams).print("optimizationParams:                ");
+    if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
+      boost::get<ISAM2GaussNewtonParams>(optimizationParams).print();
+    else if (optimizationParams.type() == typeid(ISAM2DoglegParams))
+      boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
     else
-      std::cout << "optimizationParams:                " << "{unknown type}" << "\n";
+      cout << kStr << "{unknown type}\n";
-    if(relinearizeThreshold.type() == typeid(double))
+
-      std::cout << "relinearizeThreshold:              " << boost::get<double>(relinearizeThreshold) << "\n";
+    cout << "relinearizeThreshold:              ";
-    else
+    if (relinearizeThreshold.type() == typeid(double)) {
-    {
+      cout << boost::get<double>(relinearizeThreshold) << "\n";
-      std::cout << "relinearizeThreshold:              " << "{mapped}" << "\n";
+    } else {
-      for(const ISAM2ThresholdMapValue& value: boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
+      cout << "{mapped}\n";
-        std::cout << "                                   '" << value.first << "' -> [" << value.second.transpose() << " ]\n";
+      for (const ISAM2ThresholdMapValue& value :
+           boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
+        cout << "                                   '" << value.first
+             << "' -> [" << value.second.transpose() << " ]\n";
       }
     }
-    std::cout << "relinearizeSkip:                   " << relinearizeSkip << "\n";
+
-    std::cout << "enableRelinearization:             " << enableRelinearization << "\n";
+    cout << "relinearizeSkip:                   " << relinearizeSkip << "\n";
-    std::cout << "evaluateNonlinearError:            " << evaluateNonlinearError << "\n";
+    cout << "enableRelinearization:             " << enableRelinearization
-    std::cout << "factorization:                     " << factorizationTranslator(factorization) << "\n";
+         << "\n";
-    std::cout << "cacheLinearizedFactors:            " << cacheLinearizedFactors << "\n";
+    cout << "evaluateNonlinearError:            " << evaluateNonlinearError
-    std::cout << "enableDetailedResults:             " << enableDetailedResults << "\n";
+         << "\n";
-    std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n";
+    cout << "factorization:                     "
-    std::cout << "findUnusedFactorSlots:             " << findUnusedFactorSlots << "\n";
+         << factorizationTranslator(factorization) << "\n";
-    std::cout.flush();
+    cout << "cacheLinearizedFactors:            " << cacheLinearizedFactors
+         << "\n";
+    cout << "enableDetailedResults:             " << enableDetailedResults
+         << "\n";
+    cout << "enablePartialRelinearizationCheck: "
+         << enablePartialRelinearizationCheck << "\n";
+    cout << "findUnusedFactorSlots:             " << findUnusedFactorSlots
+         << "\n";
+    cout.flush();
   }
 
   /// @name Getters and Setters for all properties
   /// @{
 
-  OptimizationParams getOptimizationParams() const { return this->optimizationParams; }
+  OptimizationParams getOptimizationParams() const {
-  RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; }
+    return this->optimizationParams;
+  }
+  RelinearizationThreshold getRelinearizeThreshold() const {
+    return relinearizeThreshold;
+  }
   int getRelinearizeSkip() const { return relinearizeSkip; }
   bool isEnableRelinearization() const { return enableRelinearization; }
   bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
-  std::string getFactorization() const { return factorizationTranslator(factorization); }
+  std::string getFactorization() const {
+    return factorizationTranslator(factorization);
+  }
   bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
   KeyFormatter getKeyFormatter() const { return keyFormatter; }
   bool isEnableDetailedResults() const { return enableDetailedResults; }
-  bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; }
+  bool isEnablePartialRelinearizationCheck() const {
+    return enablePartialRelinearizationCheck;
+  }
 
-  void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; }
+  void setOptimizationParams(OptimizationParams optimizationParams) {
-  void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; }
+    this->optimizationParams = optimizationParams;
-  void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; }
+  }
-  void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; }
+  void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
-  void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; }
+    this->relinearizeThreshold = relinearizeThreshold;
-  void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); }
+  }
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; }
+  void setRelinearizeSkip(int relinearizeSkip) {
-  void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; }
+    this->relinearizeSkip = relinearizeSkip;
-  void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; }
+  }
-  void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; }
+  void setEnableRelinearization(bool enableRelinearization) {
+    this->enableRelinearization = enableRelinearization;
+  }
+  void setEvaluateNonlinearError(bool evaluateNonlinearError) {
+    this->evaluateNonlinearError = evaluateNonlinearError;
+  }
+  void setFactorization(const std::string& factorization) {
+    this->factorization = factorizationTranslator(factorization);
+  }
+  void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
+    this->cacheLinearizedFactors = cacheLinearizedFactors;
+  }
+  void setKeyFormatter(KeyFormatter keyFormatter) {
+    this->keyFormatter = keyFormatter;
+  }
+  void setEnableDetailedResults(bool enableDetailedResults) {
+    this->enableDetailedResults = enableDetailedResults;
+  }
+  void setEnablePartialRelinearizationCheck(
+      bool enablePartialRelinearizationCheck) {
+    this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
+  }
 
   GaussianFactorGraph::Eliminate getEliminationFunction() const {
     return factorization == CHOLESKY
-      ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
+               ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
-      : (GaussianFactorGraph::Eliminate)EliminateQR;
+               : (GaussianFactorGraph::Eliminate)EliminateQR;
   }
 
   /// @}
@@ -275,8 +385,9 @@ struct GTSAM_EXPORT ISAM2Result {
    * 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
+   * \par Note: This will only be computed if
-   * is set to \c true, because there is some cost to this computation.
+   * ISAM2Params::evaluateNonlinearError is set to \c true, because there is
+   * some cost to this computation.
    */
   boost::optional<double> errorBefore;
 
@@ -286,8 +397,9 @@ struct GTSAM_EXPORT ISAM2Result {
    * 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
+   * \par Note: This will only be computed if
-   * is set to \c true, because there is some cost to this computation.
+   * ISAM2Params::evaluateNonlinearError is set to \c true, because there is
+   * some cost to this computation.
    */
   boost::optional<double> errorAfter;
 
@@ -309,7 +421,8 @@ struct GTSAM_EXPORT ISAM2Result {
    */
   size_t variablesReeliminated;
 
-  /** The number of factors that were included in reelimination of the Bayes' tree. */
+  /** The number of factors that were included in reelimination of the Bayes'
+   * tree. */
   size_t factorsRecalculated;
 
   /** The number of cliques in the Bayes' Tree */
@@ -332,14 +445,29 @@ struct GTSAM_EXPORT ISAM2Result {
        * 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 isAboveRelinThreshold;  ///< Whether the variable was just
-      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
+                                   ///< relinearized due to being above the
-      bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement.
+                                   ///< relinearization threshold
-      bool isObserved; ///< Whether the variable was just involved in new factors
+      bool isRelinearizeInvolved;  ///< Whether the variable was below the
-      bool isNew; ///< Whether the variable itself was just added
+                                   ///< relinearization threshold but was
-      bool inRootClique; ///< Whether the variable is in the root clique
+                                   ///< relinearized by being involved in a
-      VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false),
+                                   ///< factor with a variable above the
-          isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {}
+                                   ///< 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),
+            isAboveRelinThreshold(false),
+            isRelinearizeInvolved(false),
+            isRelinearized(false),
+            isObserved(false),
+            isNew(false),
+            inRootClique(false) {}
     };
 
     /** The status of each variable during this update, see VariableStatus.
@@ -351,24 +479,27 @@ struct GTSAM_EXPORT ISAM2Result {
    * Detail for information about the results data stored here. */
   boost::optional<DetailedResults> detail;
 
-
   void print(const std::string str = "") const {
-    std::cout << str << "  Reelimintated: " << variablesReeliminated << "  Relinearized: " << variablesRelinearized << "  Cliques: " << cliques << std::endl;
+    using std::cout;
+    cout << str << "  Reelimintated: " << variablesReeliminated
+         << "  Relinearized: " << variablesRelinearized
+         << "  Cliques: " << cliques << std::endl;
   }
 
   /** Getters and Setters */
-  size_t getVariablesRelinearized() const { return variablesRelinearized; };
+  size_t getVariablesRelinearized() const { return variablesRelinearized; }
-  size_t getVariablesReeliminated() const { return variablesReeliminated; };
+  size_t getVariablesReeliminated() const { return variablesReeliminated; }
-  size_t getCliques() const { return cliques; };
+  size_t getCliques() const { return cliques; }
 };
 
 /**
- * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution
+ * Specialized Clique structure for ISAM2, incorporating caching and gradient
+ * contribution
  * TODO: more documentation
  */
-class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph>
+class GTSAM_EXPORT ISAM2Clique
-{
+    : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
-public:
+ public:
   typedef ISAM2Clique This;
   typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
   typedef boost::shared_ptr<This> shared_ptr;
@@ -383,13 +514,16 @@ public:
   /// Default constructor
   ISAM2Clique() : Base() {}
 
-  /// Copy constructor, does *not* copy solution pointers as these are invalid in different trees.
+  /// Copy constructor, does *not* copy solution pointers as these are invalid
-  ISAM2Clique(const ISAM2Clique& other) :
+  /// in different trees.
-    Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {}
+  ISAM2Clique(const ISAM2Clique& other)
+      : Base(other),
+        cachedFactor_(other.cachedFactor_),
+        gradientContribution_(other.gradientContribution_) {}
 
-  /// Assignment operator, does *not* copy solution pointers as these are invalid in different trees.
+  /// Assignment operator, does *not* copy solution pointers as these are
-  ISAM2Clique& operator=(const ISAM2Clique& other)
+  /// invalid in different trees.
-  {
+  ISAM2Clique& operator=(const ISAM2Clique& other) {
     Base::operator=(other);
     cachedFactor_ = other.cachedFactor_;
     gradientContribution_ = other.gradientContribution_;
@@ -397,7 +531,8 @@ public:
   }
 
   /// Overridden to also store the remaining factor and gradient contribution
-  void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult);
+  void setEliminationResult(
+      const FactorGraphType::EliminationResult& eliminationResult);
 
   /** Access the cached factor */
   Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
@@ -405,44 +540,45 @@ public:
   /** Access the gradient contribution */
   const Vector& gradientContribution() const { return gradientContribution_; }
 
-  bool equals(const This& other, double tol=1e-9) const;
+  bool equals(const This& other, double tol = 1e-9) const;
 
   /** print this node */
-  void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "",
-
+             const KeyFormatter& formatter = DefaultKeyFormatter) const;
-private:
 
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class ARCHIVE>
+  template <class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
+    ar& BOOST_SERIALIZATION_NVP(cachedFactor_);
-    ar & BOOST_SERIALIZATION_NVP(gradientContribution_);
+    ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
   }
-}; // \struct ISAM2Clique
+};  // \struct ISAM2Clique
 
 /**
  * @addtogroup ISAM2
- * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
+ * Implementation of the full ISAM2 algorithm for incremental nonlinear
+ * optimization.
  *
- * The typical cycle of using this class to create an instance by providing ISAM2Params
+ * The typical cycle of using this class to create an instance by providing
- * to the constructor, then add measurements and variables as they arrive using the update()
+ * ISAM2Params to the constructor, then add measurements and variables as they
- * method.  At any time, calculateEstimate() may be called to obtain the current
+ * arrive using the update() method.  At any time, calculateEstimate() may be
- * estimate of all variables.
+ * called to obtain the current estimate of all variables.
  *
  */
-class GTSAM_EXPORT ISAM2: public BayesTree<ISAM2Clique> {
+class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
-
+ protected:
-protected:
-
   /** The current linearization point */
   Values theta_;
 
-  /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
+  /** VariableIndex lets us look up factors by involved variable and keeps track
+   * of dimensions */
   VariableIndex variableIndex_;
 
-  /** The linear delta from the last linear solution, an update to the estimate in theta
+  /** The linear delta from the last linear solution, an update to the estimate
+   * in theta
    *
    * This is \c mutable because it is a "cached" variable - it is not updated
    * until either requested with getDelta() or calculateEstimate(), or needed
@@ -450,8 +586,10 @@ protected:
    */
   mutable VectorValues delta_;
 
-  mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update
+  mutable VectorValues deltaNewton_;  // Only used when using Dogleg - stores
-  mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally
+                                      // the Gauss-Newton update
+  mutable VectorValues RgProd_;  // Only used when using Dogleg - stores R*g and
+                                 // is updated incrementally
 
   /** A cumulative mask for the variables that were replaced and have not yet
    * been updated in the linear solution delta_, this is only used internally,
@@ -461,9 +599,11 @@ protected:
    * This is \c mutable because it is used internally to not update delta_
    * until it is needed.
    */
-  mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way
+  mutable KeySet
+      deltaReplacedMask_;  // TODO: Make sure accessed in the right way
 
-  /** All original nonlinear factors are stored here to use during relinearization */
+  /** All original nonlinear factors are stored here to use during
+   * relinearization */
   NonlinearFactorGraph nonlinearFactors_;
 
   /** The current linear factors, which are only updated as needed */
@@ -479,20 +619,21 @@ protected:
    * variables and thus cannot have their linearization points changed. */
   KeySet fixedVariables_;
 
-  int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization
+  int update_count_;  ///< Counter incremented every update(), used to determine
+                      ///< periodic relinearization
 
-public:
+ public:
-
+  typedef ISAM2 This;                       ///< This class
-  typedef ISAM2 This; ///< This class
+  typedef BayesTree<ISAM2Clique> Base;      ///< The BayesTree base class
-  typedef BayesTree<ISAM2Clique> Base; ///< The BayesTree base class
+  typedef Base::Clique Clique;              ///< A clique
-  typedef Base::Clique Clique; ///< A clique
+  typedef Base::sharedClique sharedClique;  ///< Shared pointer to a clique
-  typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique
+  typedef Base::Cliques Cliques;  ///< List of Clique typedef from base class
-  typedef Base::Cliques Cliques; ///< List of Clique typedef from base class
 
   /** Create an empty ISAM2 instance */
-  ISAM2(const ISAM2Params& params);
+  explicit ISAM2(const ISAM2Params& params);
 
-  /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
+  /** Create an empty ISAM2 instance using the default set of parameters (see
+   * ISAM2Params) */
   ISAM2();
 
   /** default virtual destructor */
@@ -513,26 +654,31 @@ public:
    * thresholds.
    *
    * @param newFactors The new factors to be added to the system
-   * @param newTheta Initialization points for new variables to be added to the system.
+   * @param newTheta Initialization points for new variables to be added to the
-   * You must include here all new variables occuring in newFactors (which were not already
+   * system. You must include here all new variables occuring in newFactors
-   * in the system).  There must not be any variables here that do not occur in newFactors,
+   * (which were not already in the system).  There must not be any variables
-   * and additionally, variables that were already in the system must not be included here.
+   * here that do not occur in newFactors, and additionally, variables that were
+   * already in the system must not be included here.
    * @param removeFactorIndices Indices of factors to remove from system
-   * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
+   * @param force_relinearize Relinearize any variables whose delta magnitude is
-   * large (Params::relinearizeThreshold), regardless of the relinearization interval
+   * sufficiently large (Params::relinearizeThreshold), regardless of the
-   * (Params::relinearizeSkip).
+   * relinearization interval (Params::relinearizeSkip).
-   * @param constrainedKeys is an optional map of keys to group labels, such that a variable can
+   * @param constrainedKeys is an optional map of keys to group labels, such
-   * be constrained to a particular grouping in the BayesTree
+   * that a variable can be constrained to a particular grouping in the
-   * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization
+   * BayesTree
-   * point, regardless of the size of the linear delta
+   * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will
-   * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless
+   * hold at a constant linearization point, regardless of the size of the
-   * of the size of the linear delta. This allows the provided keys to be reordered.
+   * linear delta
+   * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will
+   * re-eliminate, regardless of the size of the linear delta. This allows the
+   * provided keys to be reordered.
    * @return An ISAM2Result struct containing information about the update
    */
-  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
+  virtual ISAM2Result update(
+      const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
       const Values& newTheta = Values(),
       const FactorIndices& removeFactorIndices = FactorIndices(),
-      const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
+      const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
       const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
       const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
       bool force_relinearize = false);
@@ -542,48 +688,50 @@ public:
    * requested to be marginalized.  Marginalization leaves a linear
    * approximation of the marginal in the system, and the linearization points
    * of any variables involved in this linear marginal become fixed.  The set
-   * fixed variables will include any key involved with the marginalized variables
+   * fixed variables will include any key involved with the marginalized
-   * in the original factors, and possibly additional ones due to fill-in.
+   * variables in the original factors, and possibly additional ones due to
+   * fill-in.
    *
-   * If provided, 'marginalFactorsIndices' will be augmented with the factor graph
+   * If provided, 'marginalFactorsIndices' will be augmented with the factor
-   * indices of the marginal factors added during the 'marginalizeLeaves' call
+   * graph indices of the marginal factors added during the 'marginalizeLeaves'
+   * call
    *
-   * If provided, 'deletedFactorsIndices' will be augmented with the factor graph
+   * If provided, 'deletedFactorsIndices' will be augmented with the factor
-   * indices of any factor that was removed during the 'marginalizeLeaves' call
+   * graph indices of any factor that was removed during the 'marginalizeLeaves'
+   * call
    */
-  void marginalizeLeaves(const FastList<Key>& leafKeys,
+  void marginalizeLeaves(
-    boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
+      const FastList<Key>& leafKeys,
-    boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
+      boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
+      boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
 
   /// Access the current linearization point
-  const Values& getLinearizationPoint() const {
+  const Values& getLinearizationPoint() const { return theta_; }
-    return theta_;
-  }
 
   /// Check whether variable with given key exists in linearization point
-  bool valueExists(Key key) const {
+  bool valueExists(Key key) const { return theta_.exists(key); }
-    return theta_.exists(key);
-  }
 
-  /** Compute an estimate from the incomplete linear delta computed during the last update.
+  /** Compute an estimate from the incomplete linear delta computed during the
-   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
+   * last update. This delta is incomplete because it was not updated below
-   * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
+   * wildfire_threshold.  If only a single variable is needed, it is faster to
+   * call calculateEstimate(const KEY&).
    */
   Values calculateEstimate() const;
 
-  /** Compute an estimate for a single variable using its incomplete linear delta computed
+  /** Compute an estimate for a single variable using its incomplete linear
-   * during the last update.  This is faster than calling the no-argument version of
+   * delta computed during the last update.  This is faster than calling the
-   * calculateEstimate, which operates on all variables.
+   * no-argument version of calculateEstimate, which operates on all variables.
    * @param key
    * @return
    */
-  template<class VALUE>
+  template <class VALUE>
   VALUE calculateEstimate(Key key) const;
 
-  /** Compute an estimate for a single variable using its incomplete linear delta computed
+  /** Compute an estimate for a single variable using its incomplete linear
-   * during the last update.  This is faster than calling the no-argument version of
+   * delta computed during the last update.  This is faster than calling the
-   * calculateEstimate, which operates on all variables.  This is a non-templated version
+   * no-argument version of calculateEstimate, which operates on all variables.
-   * that returns a Value base class for use with the MATLAB wrapper.
+   * This is a non-templated version that returns a Value base class for use
+   * with the MATLAB wrapper.
    * @param key
    * @return
    */
@@ -598,7 +746,8 @@ public:
   /** Internal implementation functions */
   struct Impl;
 
-  /** Compute an estimate using a complete delta computed by a full back-substitution.
+  /** Compute an estimate using a complete delta computed by a full
+   * back-substitution.
    */
   Values calculateBestEstimate() const;
 
@@ -609,7 +758,9 @@ public:
   double error(const VectorValues& x) const;
 
   /** Access the set of nonlinear factors */
-  const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
+  const NonlinearFactorGraph& getFactorsUnsafe() const {
+    return nonlinearFactors_;
+  }
 
   /** Access the nonlinear variable index */
   const VariableIndex& getVariableIndex() const { return variableIndex_; }
@@ -628,31 +779,36 @@ public:
 
   /** prints out clique statistics */
   void printStats() const { getCliqueData().getStats().print(); }
-  
+
-  /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d
+  /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert
-   * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$.  See also
+   * \Sigma^{-1} R x - d \right\Vert^2 \f$, centered around zero. The gradient
-   * gradient(const GaussianBayesNet&, const VectorValues&).
+   * about zero is \f$ -R^T d \f$.  See also gradient(const GaussianBayesNet&,
+   * const VectorValues&).
    *
    * @return A VectorValues storing the gradient.
    */
   VectorValues gradientAtZero() const;
-  
+
   /// @}
 
-protected:
+ protected:
-
   FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
-  GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
+  GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
-  GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
+      const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
+  GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans);
 
-  virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
+  virtual boost::shared_ptr<KeySet> recalculate(
-      const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
+      const KeySet& markedKeys, const KeySet& relinKeys,
+      const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
+      const boost::optional<FastMap<Key, int> >& constrainKeys,
+      ISAM2Result& result);
   void updateDelta(bool forceFullSolve = false) const;
 
-}; // ISAM2
+};  // ISAM2
 
 /// traits
-template<> struct traits<ISAM2> : public Testable<ISAM2> {};
+template <>
+struct traits<ISAM2> : public Testable<ISAM2> {};
 
 /// Optimize the BayesTree, starting from the root.
 /// @param replaced Needs to contain
@@ -665,19 +821,21 @@ template<> struct traits<ISAM2> : public Testable<ISAM2> {};
 /// and recursive backsubstitution might eventually stop if none of the changed
 /// variables are contained in the subtree.
 /// @return The number of variables that were solved for
-template<class CLIQUE>
+template <class CLIQUE>
-size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
+size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
-    double threshold, const KeySet& replaced, VectorValues& delta);
+                        const KeySet& replaced, VectorValues& delta);
 
-template<class CLIQUE>
+template <class CLIQUE>
 size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
-    double threshold, const KeySet& replaced, VectorValues& delta);
+                                    double threshold, const KeySet& replaced,
+                                    VectorValues& delta);
 
-/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
+/// calculate the number of non-zero entries for the tree starting at clique
-template<class CLIQUE>
+/// (use root for complete matrix)
+template <class CLIQUE>
 int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
 
-} /// namespace gtsam
+}  // namespace gtsam
 
-#include <gtsam/nonlinear/ISAM2-inl.h>
 #include <gtsam/nonlinear/ISAM2-impl.h>
+#include <gtsam/nonlinear/ISAM2-inl.h>