Working on fixing ISAM

gtsam/inference/BayesTree-inst.h

@@ -36,9 +36,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  typename BayesTree<CLIQUE>::CliqueData
+  BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
-  BayesTree<CLIQUE>::getCliqueData() const {
+    BayesTreeCliqueData data;
-    CliqueData data;
     BOOST_FOREACH(const sharedClique& root, roots_)
       getCliqueData(data, root);
     return data;
@@ -46,7 +45,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  void BayesTree<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
+  void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
     data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
     data.separatorSizes.push_back(clique->conditional()->nrParents());
     BOOST_FOREACH(sharedClique c, clique->children) {
@@ -110,45 +109,6 @@ namespace gtsam {
     }
   }
 
-
-  /* ************************************************************************* */
-  template<class CLIQUE>
-  void BayesTree<CLIQUE>::CliqueStats::print(const std::string& s) const {
-    std::cout << s
-              << "avg Conditional Size: " << avgConditionalSize << std::endl
-              << "max Conditional Size: " << maxConditionalSize << std::endl
-              << "avg Separator Size: "   << avgSeparatorSize   << std::endl
-              << "max Separator Size: "   << maxSeparatorSize   << std::endl;
-  }
-
-  /* ************************************************************************* */
-  template<class CLIQUE>
-  typename BayesTree<CLIQUE>::CliqueStats
-  BayesTree<CLIQUE>::CliqueData::getStats() const
-  {
-    CliqueStats stats;
-
-    double sum = 0.0;
-    size_t max = 0;
-    BOOST_FOREACH(size_t s, conditionalSizes) {
-      sum += (double)s;
-      if(s > max) max = s;
-    }
-    stats.avgConditionalSize = sum / (double)conditionalSizes.size();
-    stats.maxConditionalSize = max;
-
-    sum = 0.0;
-    max = 1;
-    BOOST_FOREACH(size_t s, separatorSizes) {
-      sum += (double)s;
-      if(s > max) max = s;
-    }
-    stats.avgSeparatorSize = sum / (double)separatorSizes.size();
-    stats.maxSeparatorSize = max;
-
-    return stats;
-  }
-
   /* ************************************************************************* */
   template<class CLIQUE>
   size_t BayesTree<CLIQUE>::size() const {

gtsam/inference/BayesTree.cpp

@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    BayesTree.cpp
+ * @brief   Bayes Tree is a tree of cliques of a Bayes Chain
+ * @author  Frank Dellaert
+ * @author  Michael Kaess
+ * @author  Viorela Ila
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/inference/BayesTree.h>
+
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void BayesTreeCliqueStats::print(const std::string& s) const {
+  std::cout << s
+    << "avg Conditional Size: " << avgConditionalSize << std::endl
+    << "max Conditional Size: " << maxConditionalSize << std::endl
+    << "avg Separator Size: "   << avgSeparatorSize   << std::endl
+    << "max Separator Size: "   << maxSeparatorSize   << std::endl;
+}
+
+/* ************************************************************************* */
+BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
+{
+  BayesTreeCliqueStats stats;
+
+  double sum = 0.0;
+  size_t max = 0;
+  BOOST_FOREACH(size_t s, conditionalSizes) {
+    sum += (double)s;
+    if(s > max) max = s;
+  }
+  stats.avgConditionalSize = sum / (double)conditionalSizes.size();
+  stats.maxConditionalSize = max;
+
+  sum = 0.0;
+  max = 1;
+  BOOST_FOREACH(size_t s, separatorSizes) {
+    sum += (double)s;
+    if(s > max) max = s;
+  }
+  stats.avgSeparatorSize = sum / (double)separatorSizes.size();
+  stats.maxSeparatorSize = max;
+
+  return stats;
+}
+
+}

gtsam/inference/BayesTree.h

@@ -31,6 +31,23 @@ namespace gtsam {
   // Forward declarations
   template<class FACTOR> class FactorGraph;
 
+  /* ************************************************************************* */
+  /** clique statistics */
+  struct GTSAM_EXPORT BayesTreeCliqueStats {
+    double avgConditionalSize;
+    std::size_t maxConditionalSize;
+    double avgSeparatorSize;
+    std::size_t maxSeparatorSize;
+    void print(const std::string& s = "") const ;
+  };
+
+  /** store all the sizes  */
+  struct GTSAM_EXPORT BayesTreeCliqueData {
+    std::vector<std::size_t> conditionalSizes;
+    std::vector<std::size_t> separatorSizes;
+    BayesTreeCliqueStats getStats() const;
+  };
+
   /* ************************************************************************* */
   /**
    * Bayes tree
@@ -68,22 +85,6 @@ namespace gtsam {
     /** A convenience class for a list of shared cliques */
     typedef FastList<sharedClique> Cliques;
 
-    /** clique statistics */
-    struct CliqueStats {
-      double avgConditionalSize;
-      std::size_t maxConditionalSize;
-      double avgSeparatorSize;
-      std::size_t maxSeparatorSize;
-      void print(const std::string& s = "") const ;
-    };
-
-    /** store all the sizes  */
-    struct CliqueData {
-      std::vector<std::size_t> conditionalSizes;
-      std::vector<std::size_t> separatorSizes;
-      CliqueStats getStats() const;
-    };
-
     /** Map from keys to Clique */
     typedef FastMap<Key, sharedClique> Nodes;
 
@@ -151,7 +152,7 @@ namespace gtsam {
     }
 
     /** Gather data on all cliques */
-    CliqueData getCliqueData() const;
+    BayesTreeCliqueData getCliqueData() const;
 
     /** Collect number of cliques with cached separator marginals */
     size_t numCachedSeparatorMarginals() const;
@@ -233,7 +234,7 @@ namespace gtsam {
         int parentnum = 0) const;
 
     /** Gather data on a single clique */
-    void getCliqueData(CliqueData& stats, sharedClique clique) const;
+    void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const;
 
     /** remove a clique: warning, can result in a forest */
     void removeClique(sharedClique clique);

gtsam/inference/ISAM-inst.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/inference/ISAM.h>
+#include <gtsam/inference/VariableIndex.h>
 
 namespace gtsam {
 
@@ -31,9 +32,6 @@ namespace gtsam {
     if (!this->empty()) {
       const FastSet<Key> newFactorKeys = newFactors.keys();
       this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
-      if (bn.empty())
-        throw std::runtime_error(
-            "ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!");
     }
 
     // Add the removed top and the new factors
@@ -46,7 +44,11 @@ namespace gtsam {
       factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
 
     // eliminate into a Bayes net
-    Base bayesTree = *factors.eliminateMultifrontal(boost::none, function);
+    const VariableIndex varIndex(factors);
+    const FastSet<Key> newFactorKeys = newFactors.keys();
+    const Ordering constrainedOrdering =
+      Ordering::COLAMDConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
+    Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
     this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
     this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
   }

gtsam/linear/GaussianBayesTree.cpp

@@ -165,6 +165,13 @@ namespace gtsam {
     return exp(logDeterminant());
   }
 
+  /* ************************************************************************* */
+  Matrix GaussianBayesTree::marginalCovariance(Key key) const
+  {
+    return marginalFactor(key)->information().inverse();
+  }
+
+
 } // \namespace gtsam
 
 

gtsam/linear/GaussianBayesTree.h

@@ -131,6 +131,10 @@ namespace gtsam {
      * multiplying we add the logarithms of the diagonal elements and take the exponent at the end
      * because this is more numerically stable. */
     double logDeterminant() const;
+
+    /** Return the marginal on the requested variable as a covariance matrix.  See also
+    *   marginalFactor(). */
+    Matrix marginalCovariance(Key key) const;
   };
 
 }

gtsam/linear/HessianFactor.cpp

@@ -402,7 +402,8 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
 /* ************************************************************************* */
 void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter)
 {
-  updateATA(HessianFactor(update), scatter);
+  if(update.rows() > 0) // Zero-row Jacobians are treated specially
+    updateATA(HessianFactor(update), scatter);
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.cpp

@@ -158,18 +158,20 @@ namespace gtsam {
           BOOST_FOREACH(const size_t sourceVarpos, slots->second) {
             if(sourceVarpos < numeric_limits<size_t>::max()) {
               const JacobianFactor& sourceFactor = *factors[sourceFactorI];
-              DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
+              if(sourceFactor.rows() > 0) {
+                DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
-              if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
+                if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
+                  varDims[jointVarpos] = vardim;
+                  n += vardim;
+                } else
+                  assert(varDims[jointVarpos] == vardim);
+#else
                 varDims[jointVarpos] = vardim;
                 n += vardim;
-              } else
+                break;
-                assert(varDims[jointVarpos] == vardim);
-#else
-              varDims[jointVarpos] = vardim;
-              n += vardim;
-              break;
 #endif
+              }
             }
             ++ sourceFactorI;
           }
@@ -280,13 +282,15 @@ namespace gtsam {
         // Slot in source factor
         const size_t sourceSlot = varslot->second[factorI];
         const DenseIndex sourceRows = jacobians[factorI]->rows();
-        JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
+        if(sourceRows > 0) {
-        // Copy if exists in source factor, otherwise set zero
+          JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
-        if(sourceSlot != numeric_limits<size_t>::max())
+          // Copy if exists in source factor, otherwise set zero
-          destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
+          if(sourceSlot != numeric_limits<size_t>::max())
-        else
+            destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
-          destBlock.setZero();
+          else
-        nextRow += sourceRows;
+            destBlock.setZero();
+          nextRow += sourceRows;
+        }
       }
       ++combinedSlot;
     }

gtsam/nonlinear/NonlinearISAM.cpp

@@ -15,13 +15,11 @@
  * @author Viorela Ila and Richard Roberts
  */
 
-#if 0
-
 #include <gtsam/nonlinear/NonlinearISAM.h>
 
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/inference/ISAM-inl.h>
+#include <gtsam/inference/ISAM-inst.h>
-#include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/inference/Ordering.h>
 
 #include <boost/foreach.hpp>
 
@@ -34,12 +32,11 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const {
-  isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter));
+  isam_.saveGraph(s, keyFormatter);
 }
 
 /* ************************************************************************* */
-void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
+void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& initialValues) {
-    const Values& initialValues) {
 
   if(newFactors.size() > 0) {
 
@@ -55,15 +52,10 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
     // TODO: optimize for whole config?
     linPoint_.insert(initialValues);
 
-    // Augment ordering
+    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_);
-    // TODO: allow for ordering constraints within the new variables
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues)
-      ordering_.insert(key_value.key, ordering_.size());
-
-    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
 
     // Update ISAM
-    isam_.update(*linearizedNewFactors);
+    isam_.update(*linearizedNewFactors, eliminationFunction_);
   }
 }
 
@@ -78,16 +70,10 @@ void NonlinearISAM::reorder_relinearize() {
 
     isam_.clear();
 
-    // Compute an ordering
-    // TODO: allow for constrained ordering here
-    ordering_ = *factors_.orderingCOLAMD(newLinPoint);
-
-    // Create a linear factor graph at the new linearization point
-    // TODO: decouple relinearization and reordering to avoid
-    boost::shared_ptr<GaussianFactorGraph> gfg = factors_.linearize(newLinPoint, ordering_);
-
     // Just recreate the whole BayesTree
-    isam_.update(*gfg);
+    // TODO: allow for constrained ordering here
+    // TODO: decouple relinearization and reordering to avoid
+    isam_.update(*factors_.linearize(newLinPoint), eliminationFunction_);
 
     // Update linearization point
     linPoint_ = newLinPoint;
@@ -97,14 +83,14 @@ void NonlinearISAM::reorder_relinearize() {
 /* ************************************************************************* */
 Values NonlinearISAM::estimate() const {
   if(isam_.size() > 0)
-    return linPoint_.retract(optimize(isam_), ordering_);
+    return linPoint_.retract(isam_.optimize());
   else
     return linPoint_;
 }
 
 /* ************************************************************************* */
 Matrix NonlinearISAM::marginalCovariance(Key key) const {
-  return isam_.marginalCovariance(ordering_[key]);
+  return isam_.marginalCovariance(key);
 }
 
 /* ************************************************************************* */
@@ -112,7 +98,6 @@ void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) con
   cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
   isam_.print("GaussianISAM:\n");
   linPoint_.print("Linearization Point:\n", keyFormatter);
-  ordering_.print("System Ordering:\n", keyFormatter);
   factors_.print("Nonlinear Graph:\n", keyFormatter);
 }
 
@@ -124,5 +109,3 @@ void NonlinearISAM::printStats() const {
 /* ************************************************************************* */
 
 }///\ namespace gtsam
-
-#endif

gtsam/nonlinear/NonlinearISAM.h

@@ -15,8 +15,6 @@
  * @author Viorela Ila and Richard Roberts
  */
 
-#if 0
-
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -35,9 +33,6 @@ protected:
   /** The current linearization point */
   Values linPoint_;
 
-  /** The ordering */
-  gtsam::Ordering ordering_;
-
   /** The original factors, used when relinearizing */
   NonlinearFactorGraph factors_;
 
@@ -45,6 +40,9 @@ protected:
   int reorderInterval_;
   int reorderCounter_;
 
+  /** The elimination function */
+  GaussianFactorGraph::Eliminate eliminationFunction_;
+
 public:
 
   /// @name Standard Constructors
@@ -57,7 +55,9 @@ public:
    *  1 (default) reorders every time, in worse case is batch every update
    *  typical values are 50 or 100
    */
-  NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
+  NonlinearISAM(int reorderInterval = 1,
+    const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
+  reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
 
   /// @}
   /// @name Standard Interface
@@ -77,9 +77,6 @@ public:
   /** Return the current linearization point */
   const Values& getLinearizationPoint() const { return linPoint_; }
 
-  /** Get the ordering */
-  const gtsam::Ordering& getOrdering() const { return ordering_; }
-
   /** get underlying nonlinear graph */
   const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
 
@@ -106,16 +103,8 @@ public:
   /** Relinearization and reordering of variables */
   void reorder_relinearize();
 
-  /** manually add a key to the end of the ordering */
-  void addKey(Key key) { ordering_.push_back(key); }
-
-  /** replace the current ordering */
-  void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
-
   /// @}
 
 };
 
 } // \namespace gtsam
-
-#endif

tests/testGaussianISAM.cpp

@@ -17,8 +17,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#if 0
-
 #include <tests/smallExample.h>
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/linear/GaussianISAM.h>
@@ -27,6 +25,8 @@
 #include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
+#include <boost/range/adaptor/map.hpp>
+namespace br { using namespace boost::adaptors; using namespace boost::range; }
 
 using namespace std;
 using namespace gtsam;
@@ -35,11 +35,6 @@ using namespace example;
 using symbol_shorthand::X;
 using symbol_shorthand::L;
 
-/* ************************************************************************* */
-// Some numbers that should be consistent among all smoother tests
-
-static const double tol = 1e-4;
-
 /* ************************************************************************* */
 TEST( ISAM, iSAM_smoother )
 {
@@ -47,7 +42,7 @@ TEST( ISAM, iSAM_smoother )
   for (int t = 1; t <= 7; t++) ordering += X(t);
 
   // Create smoother with 7 nodes
-  GaussianFactorGraph smoother = createSmoother(7, ordering).first;
+  GaussianFactorGraph smoother = createSmoother(7);
 
   // run iSAM for every factor
   GaussianISAM actual;
@@ -58,27 +53,26 @@ TEST( ISAM, iSAM_smoother )
   }
 
   // Create expected Bayes Tree by solving smoother with "natural" ordering
-  BayesTree<GaussianConditional>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate();
+  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
-  GaussianISAM expected(*bayesTree);
+  GaussianISAM expected;
+  expected.insertRoot(bayesTree.roots().front());
 
   // Verify sigmas in the bayes tree
-  BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree->nodes()) {
+  BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree.nodes() | br::map_values) {
     GaussianConditional::shared_ptr conditional = clique->conditional();
-    size_t dim = conditional->dim();
+    EXPECT(!conditional->get_model());
-    EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
   }
 
   // Check whether BayesTree is correct
   EXPECT(assert_equal(expected, actual));
 
   // obtain solution
-  VectorValues e(VectorValues::Zero(7,2)); // expected solution
+  VectorValues e; // expected solution
-  VectorValues optimized = optimize(actual); // actual solution
+  for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
+  VectorValues optimized = actual.optimize(); // actual solution
   EXPECT(assert_equal(e, optimized));
 }
 
-#endif
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

tests/testNonlinearISAM.cpp

@@ -5,8 +5,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#if 0
-
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearISAM.h>
@@ -19,14 +17,13 @@
 
 using namespace gtsam;
 
-typedef NonlinearISAM PlanarISAM;
-
 const double tol=1e-5;
 
 /* ************************************************************************* */
 TEST(testNonlinearISAM, markov_chain ) {
   int reorder_interval = 2;
-  PlanarISAM isam(reorder_interval); // create an ISAM object
+  NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
+  NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
 
   SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5));
   Sampler sampler(model, 42u);
@@ -34,52 +31,41 @@ TEST(testNonlinearISAM, markov_chain ) {
   // create initial graph
   Pose2 cur_pose; // start at origin
   NonlinearFactorGraph start_factors;
-  start_factors.add(NonlinearEquality<Pose2>(0, cur_pose));
+  start_factors += NonlinearEquality<Pose2>(0, cur_pose);
 
   Values init;
   Values expected;
   init.insert(0, cur_pose);
   expected.insert(0, cur_pose);
-  isam.update(start_factors, init);
+  isamChol.update(start_factors, init);
+  isamQR.update(start_factors, init);
 
   // loop for a period of time to verify memory usage
   size_t nrPoses = 21;
   Pose2 z(1.0, 2.0, 0.1);
   for (size_t i=1; i<=nrPoses; ++i) {
     NonlinearFactorGraph new_factors;
-    new_factors.add(BetweenFactor<Pose2>(i-1, i, z, model));
+    new_factors += BetweenFactor<Pose2>(i-1, i, z, model);
     Values new_init;
 
-    // perform a check on changing orderings
-    if (i == 5) {
-      Ordering ordering = isam.getOrdering();
-
-      // swap last two elements
-      Key last = ordering.pop_back().first;
-      Key secondLast = ordering.pop_back().first;
-      ordering.push_back(last);
-      ordering.push_back(secondLast);
-      isam.setOrdering(ordering);
-
-      Ordering expected; expected += (0), (1), (2), (4), (3);
-      EXPECT(assert_equal(expected, isam.getOrdering()));
-    }
-
     cur_pose = cur_pose.compose(z);
     new_init.insert(i, cur_pose.retract(sampler.sample()));
     expected.insert(i, cur_pose);
-    isam.update(new_factors, new_init);
+    isamChol.update(new_factors, new_init);
+    isamQR.update(new_factors, new_init);
   }
 
   // verify values - all but the last one should be very close
-  Values actual = isam.estimate();
+  Values actualChol = isamChol.estimate();
   for (size_t i=0; i<nrPoses; ++i) {
-    EXPECT(assert_equal(expected.at<Pose2>(i), actual.at<Pose2>(i), tol));
+    EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
+  }
+  Values actualQR = isamQR.estimate();
+  for (size_t i=0; i<nrPoses; ++i) {
+    EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
   }
 }
 
-#endif
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */