From b4f5413989e047969c7ac38d006363edb6e816fd Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 21 Feb 2013 15:59:50 +0000
Subject: [PATCH] Added an argument to ISAM2::update to not change the
 linearization point of a given set of variables

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

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 256a3485a..92b23f232 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -548,7 +548,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
 /* ************************************************************************* */
 ISAM2Result ISAM2::update(
     const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
-    const boost::optional<FastMap<Key,int> >& constrainedKeys, bool force_relinearize) {
+    const boost::optional<FastMap<Key,int> >& constrainedKeys,
+    const boost::optional<FastList<Key> >& noRelinKeys, bool force_relinearize) {
 
   const bool debug = ISDEBUG("ISAM2 update");
   const bool verbose = ISDEBUG("ISAM2 update verbose");
@@ -665,7 +666,6 @@ ISAM2Result ISAM2::update(
   FastSet<Index> relinKeys;
   if (relinearizeThisStep) {
     gttic(gather_relinearize_keys);
-    vector<bool> markedRelinMask(ordering_.size(), false);
     // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
     if(params_.enablePartialRelinearizationCheck)
       relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
@@ -673,6 +673,13 @@ ISAM2Result ISAM2::update(
       relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold);
     if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging
 
+    // Remove from relinKeys any keys whose linearization points are fixed
+    if(noRelinKeys) {
+      BOOST_FOREACH(Key key, *noRelinKeys) {
+        relinKeys.erase(ordering_[key]);
+      }
+    }
+
     // Above relin threshold keys for detailed results
     if(params_.enableDetailedResults) {
       BOOST_FOREACH(Index index, relinKeys) {
@@ -680,6 +687,7 @@ ISAM2Result ISAM2::update(
         result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
 
     // Add the variables being relinearized to the marked keys
+    vector<bool> markedRelinMask(ordering_.size(), false);
     BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
     markedKeys.insert(relinKeys.begin(), relinKeys.end());
     gttoc(gather_relinearize_keys);
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 3d363ed9a..7e31fd8b9 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -544,6 +544,7 @@ public:
   ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
       const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
       const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
+      const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
       bool force_relinearize = false);
 
   /** Access the current linearization point */