iSAM2 factorization parameter to choose LDL or QR

gtsam/nonlinear/ISAM2-impl.cpp

@@ -163,7 +163,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& del
 /* ************************************************************************* */
 ISAM2::Impl::PartialSolveResult
 ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
-    const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
+    const FastSet<Index>& keys, const ReorderingMode& reorderingMode, bool useQR) {
 
   const bool debug = ISDEBUG("ISAM2 recalculate");
 
@@ -245,7 +245,10 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
   // eliminate into a Bayes net
   tic(7,"eliminate");
   JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex);
+  if(!useQR)
     result.bayesTree = jt.eliminate(EliminatePreferLDL);
+  else
+    result.bayesTree = jt.eliminate(EliminateQR);
   toc(7,"eliminate");
 
   tic(8,"permute eliminated");

gtsam/nonlinear/ISAM2-impl.h

@@ -118,11 +118,12 @@ struct ISAM2::Impl {
    * problem.  This is permuted during use and so is cleared when this function
    * returns in order to invalidate it.
    * \param keys The set of indices used in \c factors.
+   * \param useQR Whether to use QR (if true), or LDL (if false).
    * \return The eliminated BayesTree and the permutation to be applied to the
    * rest of the ISAM2 data.
    */
   static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
-      const ReorderingMode& reorderingMode);
+      const ReorderingMode& reorderingMode, bool useQR);
 
   static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
 

gtsam/nonlinear/ISAM2.cpp

@@ -123,12 +123,11 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
 
 /* ************************************************************************* */
 // find intermediate (linearized) factors from cache that are passed into the affected area
-FactorGraph<ISAM2::CacheFactor>
+GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
-ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
 
   static const bool debug = false;
 
-  FactorGraph<CacheFactor> cachedBoundary;
+  GaussianFactorGraph cachedBoundary;
 
   BOOST_FOREACH(sharedClique orphan, orphans) {
     // find the last variable that was eliminated
@@ -141,7 +140,7 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
 //    assert(key == lastKey);
 #endif
     // retrieve the cached factor and add to boundary
-    cachedBoundary.push_back(boost::dynamic_pointer_cast<CacheFactor>(orphan->cachedFactor()));
+    cachedBoundary.push_back(orphan->cachedFactor());
     if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }
   }
 
@@ -266,7 +265,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
 
     tic(5,"eliminate");
     JunctionTree<GaussianFactorGraph, Base::Clique> jt(factors, variableIndex_);
-    sharedClique newRoot = jt.eliminate(EliminatePreferLDL);
+    sharedClique newRoot;
+    if(params_.factorization == ISAM2Params::LDL)
+      newRoot = jt.eliminate(EliminatePreferLDL);
+    else if(params_.factorization == ISAM2Params::QR)
+      newRoot = jt.eliminate(EliminateQR);
+    else assert(false);
     if(debug) newRoot->print("Eliminated: ");
     toc(5,"eliminate");
 
@@ -314,12 +318,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
 
     tic(2,"cached");
     // add the cached intermediate results from the boundary of the orphans ...
-    FactorGraph<CacheFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
+    GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans);
     if(debug) cachedBoundary.print("Boundary factors: ");
     factors.reserve(factors.size() + cachedBoundary.size());
     // Copy so that we can later permute factors
-    BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) {
+    BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) {
-      factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached)));
+      factors.push_back(cached->clone());
     }
     toc(2,"cached");
 
@@ -348,7 +352,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
       BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
     }
     Impl::PartialSolveResult partialSolveResult =
-        Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode);
+        Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
     toc(2,"PartialSolve");
 
     // We now need to permute everything according this partial reordering: the

gtsam/nonlinear/ISAM2.h

@@ -107,6 +107,17 @@ struct ISAM2Params {
 
   bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
 
+  enum Factorization { LDL, QR };
+  /** Specifies whether to use QR or LDL numerical factorization (default: LDL).
+   * LDL is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when
+   * uncertainty is very low in some variables (or dimensions of variables) and very high in others.  QR is
+   * slower but more numerically stable in poorly-conditioned problems.  We suggest using the default of LDL
+   * unless gtsam sometimes throws NegativeMatrixException 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 LDL.
+   */
+  Factorization factorization;
+
   KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
 
   /** Specify parameters as constructor arguments */
@@ -116,10 +127,12 @@ struct ISAM2Params {
       int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
       bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
       bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
+      Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization
       const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
   ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
       relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
-      evaluateNonlinearError(_evaluateNonlinearError), keyFormatter(_keyFormatter) {}
+      evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
+      keyFormatter(_keyFormatter) {}
 };
 
 /**
@@ -340,8 +353,6 @@ private:
   std::vector<bool> lastRelinVariables_;
 #endif
 
-  typedef HessianFactor CacheFactor;
-
 public:
 
   typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
@@ -460,7 +471,7 @@ private:
 
   FastList<size_t> getAffectedFactors(const FastList<Index>& keys) const;
   FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const;
-  FactorGraph<CacheFactor> getCachedBoundaryFactors(Cliques& orphans);
+  GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
 
   boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys,
       const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,

tests/testGaussianISAM2.cpp

@@ -509,6 +509,272 @@ TEST(ISAM2, slamlike_solution_dogleg)
   EXPECT(assert_equal(expectedGradient, actualGradient));
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, slamlike_solution_gaussnewton_qr)
+{
+
+//  SETDEBUG("ISAM2 update", true);
+//  SETDEBUG("ISAM2 update verbose", true);
+//  SETDEBUG("ISAM2 recalculate", true);
+
+  // Pose and landmark key types from planarSLAM
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
+
+  // Set up parameters
+  SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
+  SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
+
+  // These variables will be reused and accumulate factors and values
+  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
+  Values fullinit;
+  planarSLAM::Graph fullgraph;
+
+  // i keeps track of the time step
+  size_t i = 0;
+
+  // Add a prior at time 0 and update isam
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+    fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Add odometry from time 0 to time 5
+  for( ; i<5; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 5 to 6 and landmark measurement at time 5
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+    fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Add odometry from time 6 to time 10
+  for( ; i<10; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 10 to 11 and landmark measurement at time 10
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Compare solutions
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Check gradient at each node
+  typedef ISAM2::sharedClique sharedClique;
+  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
+    // Compute expected gradient
+    FactorGraph<JacobianFactor> jfg;
+    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
+    VectorValues expectedGradient(*allocateVectorValues(isam));
+    gradientAtZero(jfg, expectedGradient);
+    // Compare with actual gradients
+    int variablePosition = 0;
+    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
+      const int dim = clique->conditional()->dim(jit);
+      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
+      EXPECT(assert_equal(expectedGradient[*jit], actual));
+      variablePosition += dim;
+    }
+    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
+  }
+
+  // Check gradient
+  VectorValues expectedGradient(*allocateVectorValues(isam));
+  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
+  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
+  VectorValues actualGradient(*allocateVectorValues(isam));
+  gradientAtZero(isam, actualGradient);
+  EXPECT(assert_equal(expectedGradient2, expectedGradient));
+  EXPECT(assert_equal(expectedGradient, actualGradient));
+}
+
+/* ************************************************************************* */
+TEST(ISAM2, slamlike_solution_dogleg_qr)
+{
+
+//  SETDEBUG("ISAM2 update", true);
+//  SETDEBUG("ISAM2 update verbose", true);
+//  SETDEBUG("ISAM2 recalculate", true);
+
+  // Pose and landmark key types from planarSLAM
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
+
+  // Set up parameters
+  SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
+  SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
+
+  // These variables will be reused and accumulate factors and values
+  ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
+  Values fullinit;
+  planarSLAM::Graph fullgraph;
+
+  // i keeps track of the time step
+  size_t i = 0;
+
+  // Add a prior at time 0 and update isam
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+    fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Add odometry from time 0 to time 5
+  for( ; i<5; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 5 to 6 and landmark measurement at time 5
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+    fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Add odometry from time 6 to time 10
+  for( ; i<10; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 10 to 11 and landmark measurement at time 10
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    fullgraph.push_back(newfactors);
+
+    Values init;
+    init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Compare solutions
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Check gradient at each node
+  typedef ISAM2::sharedClique sharedClique;
+  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
+    // Compute expected gradient
+    FactorGraph<JacobianFactor> jfg;
+    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
+    VectorValues expectedGradient(*allocateVectorValues(isam));
+    gradientAtZero(jfg, expectedGradient);
+    // Compare with actual gradients
+    int variablePosition = 0;
+    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
+      const int dim = clique->conditional()->dim(jit);
+      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
+      EXPECT(assert_equal(expectedGradient[*jit], actual));
+      variablePosition += dim;
+    }
+    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
+  }
+
+  // Check gradient
+  VectorValues expectedGradient(*allocateVectorValues(isam));
+  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
+  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
+  VectorValues actualGradient(*allocateVectorValues(isam));
+  gradientAtZero(isam, actualGradient);
+  EXPECT(assert_equal(expectedGradient2, expectedGradient));
+  EXPECT(assert_equal(expectedGradient, actualGradient));
+}
+
 /* ************************************************************************* */
 TEST(ISAM2, clone) {