Merge pull request #25 from jlblancoc/fix_issue_10_iSAM2

Fix iSAM2 support for smart factors

gtsam/inference/VariableIndex.cpp

@@ -55,4 +55,17 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
   os << flush;
 }
 
+/* ************************************************************************* */
+void VariableIndex::augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys)
+{
+  gttic(VariableIndex_augmentExistingFactor);
+
+  for(const Key key: newKeys) {
+    index_[key].push_back(factorIndex);
+    ++nEntries_;
+  }
+
+  gttoc(VariableIndex_augmentExistingFactor);
+}
+
 }

gtsam/inference/VariableIndex.h

@@ -125,6 +125,13 @@ public:
   template<class FG>
   void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
 
+  /**
+   * Augment the variable index after an existing factor now affects to more
+   * variable Keys. This can be used when solving problems incrementally, with
+   * smart factors or in general with factors with a dynamic number of Keys.
+   */
+  void augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys);
+
   /**
    * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement
    * nFactors_ because the factor indices need to remain consistent.  Removing factors from a factor

gtsam/nonlinear/ISAM2.cpp

@@ -128,7 +128,7 @@ FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const {
 GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
     const FastList<Key>& affectedKeys, const KeySet& relinKeys) const {
   gttic(getAffectedFactors);
-  KeySet candidates = getAffectedFactors(affectedKeys);
+  FactorIndexSet candidates = getAffectedFactors(affectedKeys);
   gttoc(getAffectedFactors);
 
   gttic(affectedKeysSet);
@@ -139,7 +139,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
 
   gttic(check_candidates_and_linearize);
   auto linearized = boost::make_shared<GaussianFactorGraph>();
-  for (Key idx : candidates) {
+  for (const FactorIndex idx : candidates) {
     bool inside = true;
     bool useCachedLinear = params_.cacheLinearizedFactors;
     for (Key key : nonlinearFactors_[idx]->keys()) {
@@ -544,6 +544,21 @@ ISAM2Result ISAM2::update(
     const boost::optional<FastList<Key> >& noRelinKeys,
     const boost::optional<FastList<Key> >& extraReelimKeys,
     bool force_relinearize) {
+
+  ISAM2UpdateParams params;
+  params.constrainedKeys = constrainedKeys;
+  params.extraReelimKeys = extraReelimKeys;
+  params.force_relinearize = force_relinearize;
+  params.noRelinKeys = noRelinKeys;
+  params.removeFactorIndices = removeFactorIndices;
+
+  return update(newFactors, newTheta, params);
+}
+
+/* ************************************************************************* */
+ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
+    const Values& newTheta,
+    const ISAM2UpdateParams& updateParams) {
   const bool debug = ISDEBUG("ISAM2 update");
   const bool verbose = ISDEBUG("ISAM2 update verbose");
 
@@ -561,7 +576,7 @@ ISAM2Result ISAM2::update(
   if (params_.enableDetailedResults)
     result.detail = ISAM2Result::DetailedResults();
   const bool relinearizeThisStep =
-      force_relinearize || (params_.enableRelinearization &&
+      updateParams.force_relinearize || (params_.enableRelinearization &&
                             update_count_ % params_.relinearizeSkip == 0);
 
   if (verbose) {
@@ -585,8 +600,8 @@ ISAM2Result ISAM2::update(
 
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
-  removeFactors.reserve(removeFactorIndices.size());
+  removeFactors.reserve(updateParams.removeFactorIndices.size());
-  for (const auto index : removeFactorIndices) {
+  for (const auto index : updateParams.removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@@ -594,7 +609,8 @@ ISAM2Result ISAM2::update(
 
   // Remove removed factors from the variable index so we do not attempt to
   // relinearize them
-  variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(),
+  variableIndex_.remove(updateParams.removeFactorIndices.begin(),
+                        updateParams.removeFactorIndices.end(),
                         removeFactors);
 
   // Compute unused keys and indices
@@ -649,11 +665,20 @@ ISAM2Result ISAM2::update(
         markedRemoveKeys.end());  // Add to the overall set of marked keys
   }
   // Also mark any provided extra re-eliminate keys
-  if (extraReelimKeys) {
+  if (updateParams.extraReelimKeys) {
-    for (Key key : *extraReelimKeys) {
+    for (Key key : *updateParams.extraReelimKeys) {
       markedKeys.insert(key);
     }
   }
+  // Also, keys that were not observed in existing factors, but whose affected
+  // keys have been extended now (e.g. smart factors)
+  if (updateParams.newAffectedKeys) {
+    for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) {
+      const auto factorIdx = factorAddedKeys.first;
+      const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys();
+      markedKeys.insert(affectedKeys.begin(),affectedKeys.end());
+    }
+  }
 
   // Observed keys for detailed results
   if (params_.enableDetailedResults) {
@@ -661,16 +686,13 @@ ISAM2Result ISAM2::update(
       result.detail->variableStatus[key].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, Key value) instead of the iterator constructor.
   KeyVector observedKeys;
-  observedKeys.reserve(markedKeys.size());
   for (Key index : markedKeys) {
-    if (unusedIndices.find(index) ==
+    // Only add if not unused
-        unusedIndices.end())  // Only add if not unused
+    if (unusedIndices.find(index) == unusedIndices.end())
-      observedKeys.push_back(
+      // Make a copy of these, as we'll soon add to them
-          index);  // Make a copy of these, as we'll soon add to them
+      observedKeys.push_back(index);
   }
   gttoc(gather_involved_keys);
 
@@ -695,8 +717,8 @@ ISAM2Result ISAM2::update(
     for (Key key : fixedVariables_) {
       relinKeys.erase(key);
     }
-    if (noRelinKeys) {
+    if (updateParams.noRelinKeys) {
-      for (Key key : *noRelinKeys) {
+      for (Key key : *updateParams.noRelinKeys) {
         relinKeys.erase(key);
       }
     }
@@ -773,14 +795,24 @@ ISAM2Result ISAM2::update(
     variableIndex_.augment(newFactors, result.newFactorsIndices);
   else
     variableIndex_.augment(newFactors);
+
+  // Augment it with existing factors which now affect to more variables:
+  if (updateParams.newAffectedKeys) {
+    for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) {
+      const auto factorIdx = factorAddedKeys.first;
+      variableIndex_.augmentExistingFactor(
+            factorIdx, factorAddedKeys.second);
+    }
+  }
   gttoc(augment_VI);
 
   gttic(recalculate);
   // 8. Redo top of Bayes tree
   boost::shared_ptr<KeySet> replacedKeys;
   if (!markedKeys.empty() || !observedKeys.empty())
-    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys,
+    replacedKeys = recalculate(
-                               unusedIndices, constrainedKeys, &result);
+          markedKeys, relinKeys, observedKeys, unusedIndices,
+          updateParams.constrainedKeys, &result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if (replacedKeys)

gtsam/nonlinear/ISAM2.h

@@ -23,6 +23,7 @@
 #include <gtsam/nonlinear/ISAM2Params.h>
 #include <gtsam/nonlinear/ISAM2Result.h>
 #include <gtsam/nonlinear/ISAM2Clique.h>
+#include <gtsam/nonlinear/ISAM2UpdateParams.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 
@@ -156,6 +157,28 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
       const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
       bool force_relinearize = false);
 
+  /**
+   * Add new factors, updating the solution and relinearizing as needed.
+   *
+   * Alternative signature of update() (see its documentation above), with all
+   * additional parameters in one structure. This form makes easier to keep
+   * future API/ABI compatibility if parameters change.
+   *
+   * @param newFactors The new factors to be added to the system
+   * @param newTheta Initialization points for new variables to be added to the
+   * system. You must include here all new variables occuring in newFactors
+   * (which were not already in the system).  There must not be any variables
+   * here that do not occur in newFactors, and additionally, variables that were
+   * already in the system must not be included here.
+   * @param updateParams Additional parameters to control relinearization,
+   * constrained keys, etc.
+   * @return An ISAM2Result struct containing information about the update
+   * @note No default parameters to avoid ambiguous call errors.
+   */
+  virtual ISAM2Result update(
+      const NonlinearFactorGraph& newFactors, const Values& newTheta,
+      const ISAM2UpdateParams& updateParams);
+
   /** Marginalize out variables listed in leafKeys.  These keys must be leaves
    * in the BayesTree.  Throws MarginalizeNonleafException if non-leaves are
    * requested to be marginalized.  Marginalization leaves a linear

gtsam/nonlinear/ISAM2UpdateParams.h

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    ISAM2UpdateParams.h
+ * @brief   Class that stores extra params for ISAM2::update()
+ * @author  Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <boost/optional.hpp>
+#include <gtsam/base/FastList.h>
+#include <gtsam/dllexport.h>             // GTSAM_EXPORT
+#include <gtsam/inference/Key.h>         // Key, KeySet
+#include <gtsam/nonlinear/ISAM2Result.h> //FactorIndices
+
+namespace gtsam {
+
+/**
+ * @addtogroup ISAM2
+ * This struct is used by ISAM2::update() to pass additional parameters to
+ * give the user a fine-grained control on how factors and relinearized, etc.
+ */
+struct GTSAM_EXPORT ISAM2UpdateParams {
+  ISAM2UpdateParams() = default;
+
+  /** Indices of factors to remove from system (default: empty) */
+  FactorIndices removeFactorIndices;
+
+  /** An optional map of keys to group labels, such that a variable can be
+   * constrained to a particular grouping in the BayesTree */
+  boost::optional<FastMap<Key, int>> constrainedKeys{boost::none};
+
+  /** An optional set of nonlinear keys that iSAM2 will hold at a constant
+   * linearization point, regardless of the size of the linear delta */
+  boost::optional<FastList<Key>> noRelinKeys{boost::none};
+
+  /** 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. */
+  boost::optional<FastList<Key>> extraReelimKeys{boost::none};
+
+  /** Relinearize any variables whose delta magnitude is sufficiently large
+   * (Params::relinearizeThreshold), regardless of the relinearization
+   * interval (Params::relinearizeSkip). */
+  bool force_relinearize{false};
+
+  /** An optional set of new Keys that are now affected by factors,
+   * indexed by factor indices (as returned by ISAM2::update()).
+   * Use when working with smart factors. For example:
+   *  - Timestamp `i`: ISAM2::update() called with a new smart factor depending
+   *    on Keys `X(0)` and `X(1)`. It returns that the factor index for the new
+   *    smart factor (inside ISAM2) is `13`.
+   *  - Timestamp `i+1`: The same smart factor has been augmented to now also
+   *    depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include
+   *    its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`.
+   */
+  boost::optional<FastMap<FactorIndex,KeySet>> newAffectedKeys{boost::none};
+
+};
+
+} // namespace gtsam

gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp

@@ -0,0 +1,370 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testSmartStereoFactor_iSAM2.cpp
+ *  @brief Unit tests for ProjectionFactor Class
+ *  @author Jose Luis Blanco-Claraco
+ *  @date   May 2019
+ *
+ *  @note Originally based on ISAM2_SmartFactorStereo.cpp by Nghia Ho
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/debug.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
+
+#include <array>
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+// Set to 1 to enable verbose output of intermediary results
+#define TEST_VERBOSE_OUTPUT 0
+
+#if TEST_VERBOSE_OUTPUT
+#define TEST_COUT(ARGS_) std::cout << ARGS_
+#else
+#define TEST_COUT(ARGS_) void(0)
+#endif
+
+// Tolerance for ground-truth pose comparison:
+static const double tol = 1e-3;
+
+// Synthetic dataset generated with rwt
+// (https://github.com/jlblancoc/recursive-world-toolkit)
+// Camera parameters
+const double fx = 200.0;
+const double fy = 150.0;
+const double cx = 512.0;
+const double cy = 384.0;
+const double baseline = 0.2; // meters
+
+using timestep_t = std::size_t;
+using lm_id_t = int;
+
+struct stereo_meas_t {
+  stereo_meas_t(lm_id_t id, double lu, double ru, double v_lr)
+      : lm_id{id}, left_u{lu}, right_u{ru}, v{v_lr} {}
+
+  lm_id_t lm_id{-1}; // landmark id
+  double left_u{0}, right_u{0}, v{0};
+};
+
+static std::map<timestep_t, std::vector<stereo_meas_t>> dataset = {
+    {0,
+     {{0, 911.99993896, 712.00000000, 384.0},
+      {159, 311.99996948, 211.99996948, 384.0},
+      {3, 378.66665649, 312.00000000, 384.0},
+      {2, 645.33331299, 578.66662598, 384.0},
+      {157, 111.99994659, 11.99993896, 384.0},
+      {4, 578.66662598, 545.33331299, 384.0},
+      {5, 445.33331299, 412.00000000, 384.0},
+      {6, 562.00000000, 537.00000000, 384.0}}},
+    {1,
+     {{0, 1022.06353760, 762.57519531, 384.0},
+      {159, 288.30487061, 177.80273438, 384.0},
+      {2, 655.30645752, 583.12127686, 384.0},
+      {3, 368.60937500, 297.43176270, 384.0},
+      {4, 581.82666016, 547.16766357, 384.0},
+      {5, 443.66183472, 409.23681641, 384.0},
+      {6, 564.35980225, 538.62115479, 384.0},
+      {7, 461.66418457, 436.05477905, 384.0},
+      {8, 550.32220459, 531.75256348, 384.0},
+      {9, 476.17767334, 457.67541504, 384.0}}},
+    {2,
+     {{159, 257.97128296, 134.26287842, 384.0},
+      {2, 666.87255859, 588.07275391, 384.0},
+      {3, 356.53823853, 280.10061646, 384.0},
+      {4, 585.10949707, 548.99212646, 384.0},
+      {5, 441.66403198, 406.05108643, 384.0},
+      {6, 566.75402832, 540.21868896, 384.0},
+      {7, 461.16207886, 434.90002441, 384.0},
+      {8, 552.28387451, 533.30230713, 384.0},
+      {9, 476.63549805, 457.79418945, 384.0},
+      {10, 546.48394775, 530.53009033, 384.0}}},
+    {3,
+     {{159, 218.10592651, 77.30914307, 384.0},
+      {2, 680.54644775, 593.68103027, 384.0},
+      {3, 341.92507935, 259.28231812, 384.0},
+      {4, 588.53289795, 550.80499268, 384.0},
+      {5, 439.29989624, 402.39105225, 384.0},
+      {6, 569.18627930, 541.78991699, 384.0},
+      {7, 460.47863770, 433.51678467, 384.0},
+      {8, 554.24902344, 534.82952881, 384.0},
+      {9, 477.00451660, 457.80438232, 384.0},
+      {10, 548.33770752, 532.07501221, 384.0},
+      {11, 483.58688354, 467.47830200, 384.0},
+      {12, 542.36785889, 529.29321289, 384.0}}},
+    {4,
+     {{2, 697.09454346, 600.18432617, 384.0},
+      {3, 324.03643799, 233.97094727, 384.0},
+      {4, 592.11877441, 552.60449219, 384.0},
+      {5, 436.52197266, 398.19531250, 384.0},
+      {6, 571.66101074, 543.33209229, 384.0},
+      {7, 459.59658813, 431.88333130, 384.0},
+      {8, 556.21801758, 536.33258057, 384.0},
+      {9, 477.27893066, 457.69882202, 384.0},
+      {10, 550.18920898, 533.60003662, 384.0},
+      {11, 484.24472046, 467.86862183, 384.0},
+      {12, 544.14727783, 530.86157227, 384.0},
+      {13, 491.26141357, 478.11267090, 384.0},
+      {14, 541.29949951, 529.57086182, 384.0},
+      {15, 494.58111572, 482.95935059, 384.0}}}};
+
+// clang-format off
+/*
+% Ground truth path of the SENSOR, and the ROBOT
+% STEP     X       Y        Z        QR        QX      QY      QZ     |    X Y Z        QR        QX      QY      QZ
+ ----------------------------------------------------------------------------------------------------------------------------------------
+     0 0.000000  0.000000 0.000000 0.500000 -0.500000 0.500000 -0.500000 0.000000  0.000000 0.000000 1.000000 0.000000 0.000000 0.000000
+     1 0.042019 -0.008403 0.000000 0.502446 -0.502446 0.497542 -0.497542 0.042019 -0.008403 0.000000 0.999988 0.000000 0.000000 0.004905
+     2 0.084783 -0.016953 0.000000 0.504879 -0.504879 0.495073 -0.495073 0.084783 -0.016953 0.000000 0.999952 0.000000 0.000000 0.009806
+     3 0.128305 -0.025648 0.000000 0.507299 -0.507299 0.492592 -0.492592 0.128305 -0.025648 0.000000 0.999892 0.000000 0.000000 0.014707
+     4 0.172605 -0.034490 0.000000 0.509709 -0.509709 0.490098 -0.490098 0.172605 -0.034490 0.000000 0.999808 0.000000 0.000000 0.019611
+*/
+// clang-format on
+
+// Ground truth using camera pose = vehicle frame
+// The table above uses:
+//   camera +x = vehicle -y
+//   camera +y = vehicle -z
+//   camera +z = vehicle +x
+static const std::map<timestep_t, gtsam::Point3> gt_positions = {
+    {0, {0.000000, 0.000000, 0.0}},
+    {1, {0.042019, -0.008403, 0.0}},
+    {2, {0.084783, -0.016953, 0.0}},
+    {3, {0.128305, -0.025648, 0.0}},
+    {4, {0.172605, -0.034490, 0.0}}};
+
+// Batch version, to compare against iSAM2 solution.
+TEST(testISAM2SmartFactor, Stereo_Batch) {
+  TEST_COUT("============ Running: Batch ============\n");
+
+  using namespace gtsam;
+  using symbol_shorthand::V;
+  using symbol_shorthand::X;
+
+  const auto K =
+      boost::make_shared<Cal3_S2Stereo>(fx, fy, .0, cx, cy, baseline);
+
+  // Pose prior - at identity
+  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.2), Vector3::Constant(0.2)).finished());
+
+  // Map: landmark_id => smart_factor_index inside iSAM2
+  std::map<lm_id_t, FactorIndex> lm2factor;
+
+  // Storage of smart factors:
+  std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
+
+  NonlinearFactorGraph batch_graph;
+  Values batch_values;
+
+  // Run one timestep at once:
+  for (const auto &entries : dataset) {
+    // 1) Process new observations:
+    // ------------------------------
+    const auto kf_id = entries.first;
+    const std::vector<stereo_meas_t> &obs = entries.second;
+
+    for (const stereo_meas_t &stObs : obs) {
+      if (smartFactors.count(stObs.lm_id) == 0) {
+        auto noise = noiseModel::Isotropic::Sigma(3, 0.1);
+        SmartProjectionParams parm(HESSIAN, ZERO_ON_DEGENERACY);
+
+        smartFactors[stObs.lm_id] =
+            boost::make_shared<SmartStereoProjectionPoseFactor>(noise, parm);
+
+        batch_graph.push_back(smartFactors[stObs.lm_id]);
+      }
+
+      TEST_COUT("Adding stereo observation from KF #" << kf_id << " for LM #"
+                                                      << stObs.lm_id << "\n");
+
+      smartFactors[stObs.lm_id]->add(
+          StereoPoint2(stObs.left_u, stObs.right_u, stObs.v), X(kf_id), K);
+    }
+
+    // prior, for the first keyframe:
+    if (kf_id == 0) {
+      const auto prior = boost::make_shared<PriorFactor<Pose3>>(
+          X(kf_id), Pose3::identity(), priorPoseNoise);
+      batch_graph.push_back(prior);
+    }
+
+    batch_values.insert(X(kf_id), Pose3::identity());
+  }
+
+  LevenbergMarquardtParams parameters;
+#if TEST_VERBOSE_OUTPUT
+  parameters.verbosity = NonlinearOptimizerParams::LINEAR;
+  parameters.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
+#endif
+
+  LevenbergMarquardtOptimizer lm(batch_graph, batch_values, parameters);
+
+  Values finalEstimate = lm.optimize();
+#if TEST_VERBOSE_OUTPUT
+  finalEstimate.print("LevMarq estimate:");
+#endif
+
+  // GT:
+  //   camera +x = vehicle -y
+  //   camera +y = vehicle -z
+  //   camera +z = vehicle +x
+  for (const auto &gt : gt_positions) {
+    const Pose3 p = finalEstimate.at<Pose3>(X(gt.first));
+    EXPECT(assert_equal(p.x(), -gt.second.y(), tol));
+    EXPECT(assert_equal(p.y(), -gt.second.z(), tol));
+    EXPECT(assert_equal(p.z(), gt.second.x(), tol));
+  }
+}
+
+TEST(testISAM2SmartFactor, Stereo_iSAM2) {
+  TEST_COUT("======= Running: iSAM2 ==========\n");
+
+#if TEST_VERBOSE_OUTPUT
+  SETDEBUG("ISAM2 update", true);
+  // SETDEBUG("ISAM2 update verbose",true);
+#endif
+
+  using namespace gtsam;
+  using symbol_shorthand::V;
+  using symbol_shorthand::X;
+
+  const auto K =
+      boost::make_shared<Cal3_S2Stereo>(fx, fy, .0, cx, cy, baseline);
+
+  ISAM2Params parameters;
+  parameters.relinearizeThreshold = 0.01;
+  parameters.evaluateNonlinearError = true;
+
+  // Do not cache smart factors:
+  parameters.cacheLinearizedFactors = false;
+
+  // Important: must set relinearizeSkip=1 to additional calls to update() to
+  // have a real effect.
+  parameters.relinearizeSkip = 1;
+
+  ISAM2 isam(parameters);
+
+  // Pose prior - at identity
+  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.2), Vector3::Constant(0.2)).finished());
+
+  // Map: landmark_id => smart_factor_index inside iSAM2
+  std::map<lm_id_t, FactorIndex> lm2factor;
+
+  // Storage of smart factors:
+  std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
+
+  Pose3 lastKeyframePose = Pose3::identity();
+
+  // Run one timestep at once:
+  for (const auto &entries : dataset) {
+    // 1) Process new observations:
+    // ------------------------------
+    const auto kf_id = entries.first;
+    const std::vector<stereo_meas_t> &obs = entries.second;
+
+    // Special instructions for using iSAM2 + smart factors:
+    // Must fill factorNewAffectedKeys:
+    FastMap<FactorIndex, KeySet> factorNewAffectedKeys;
+    NonlinearFactorGraph newFactors;
+
+    // Map: factor index in the internal graph of iSAM2 => landmark_id
+    std::map<FactorIndex, lm_id_t> newFactor2lm;
+
+    for (const stereo_meas_t &stObs : obs) {
+      if (smartFactors.count(stObs.lm_id) == 0) {
+        auto noise = noiseModel::Isotropic::Sigma(3, 0.1);
+        SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
+
+        smartFactors[stObs.lm_id] =
+            boost::make_shared<SmartStereoProjectionPoseFactor>(noise, params);
+        newFactor2lm[newFactors.size()] = stObs.lm_id;
+        newFactors.push_back(smartFactors[stObs.lm_id]);
+      } else {
+        // Only if the factor *already* existed:
+        factorNewAffectedKeys[lm2factor.at(stObs.lm_id)].insert(X(kf_id));
+      }
+
+      TEST_COUT("Adding stereo observation from KF #" << kf_id << " for LM #"
+                                                      << stObs.lm_id << "\n");
+
+      smartFactors[stObs.lm_id]->add(
+          StereoPoint2(stObs.left_u, stObs.right_u, stObs.v), X(kf_id), K);
+    }
+
+    // prior, for the first keyframe:
+    if (kf_id == 0) {
+      const auto prior = boost::make_shared<PriorFactor<Pose3>>(
+          X(kf_id), Pose3::identity(), priorPoseNoise);
+      newFactors.push_back(prior);
+    }
+
+    // 2) Run iSAM2:
+    // ------------------------------
+    Values newValues;
+    newValues.insert(X(kf_id), lastKeyframePose);
+
+    TEST_COUT("Running iSAM2 for frame: " << kf_id << "\n");
+
+    ISAM2UpdateParams updateParams;
+    updateParams.newAffectedKeys = std::move(factorNewAffectedKeys);
+
+    ISAM2Result res = isam.update(newFactors, newValues, updateParams);
+
+    for (const auto &f2l : newFactor2lm)
+      lm2factor[f2l.second] = res.newFactorsIndices.at(f2l.first);
+
+    TEST_COUT("error before1: " << res.errorBefore.value() << "\n");
+    TEST_COUT("error after1 : " << res.errorAfter.value() << "\n");
+
+    // Additional refining steps:
+    ISAM2Result res2 = isam.update();
+    TEST_COUT("error before2: " << res2.errorBefore.value() << "\n");
+    TEST_COUT("error after2 : " << res2.errorAfter.value() << "\n");
+
+    Values currentEstimate = isam.calculateEstimate();
+#if TEST_VERBOSE_OUTPUT
+    currentEstimate.print("currentEstimate:");
+#endif
+
+    // Keep last KF pose as initial pose of the next one, to reduce the need
+    // to run more non-linear iterations:
+    lastKeyframePose = currentEstimate.at(X(kf_id)).cast<Pose3>();
+
+  } // end for each timestep
+
+  Values finalEstimate = isam.calculateEstimate();
+
+  // GT:
+  //   camera +x = vehicle -y
+  //   camera +y = vehicle -z
+  //   camera +z = vehicle +x
+  for (const auto &gt : gt_positions) {
+    const Pose3 p = finalEstimate.at<Pose3>(X(gt.first));
+    EXPECT(assert_equal(p.x(), -gt.second.y(), tol));
+    EXPECT(assert_equal(p.y(), -gt.second.z(), tol));
+    EXPECT(assert_equal(p.z(), gt.second.x(), tol));
+  }
+}
+
+/* *************************************************************************
+ */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}