diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 9f2d84a90..e4cb2abd3 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -163,7 +163,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& del /* ************************************************************************* */ ISAM2::Impl::PartialSolveResult ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, - const FastSet& keys, const ReorderingMode& reorderingMode) { + const FastSet& 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 jt(factors, affectedFactorsIndex); - result.bayesTree = jt.eliminate(EliminatePreferLDL); + if(!useQR) + result.bayesTree = jt.eliminate(EliminatePreferLDL); + else + result.bayesTree = jt.eliminate(EliminateQR); toc(7,"eliminate"); tic(8,"permute eliminated"); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 9c1f28043..e9046b3e3 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/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& keys, - const ReorderingMode& reorderingMode); + const ReorderingMode& reorderingMode, bool useQR); static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 8888a8259..a59bc97dc 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -123,12 +123,11 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -FactorGraph -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; - FactorGraph 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(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 > ISAM2::recalculate( tic(5,"eliminate"); JunctionTree 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 > ISAM2::recalculate( tic(2,"cached"); // add the cached intermediate results from the boundary of the orphans ... - FactorGraph 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) { - factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); + BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) { + factors.push_back(cached->clone()); } toc(2,"cached"); @@ -348,7 +352,7 @@ boost::shared_ptr > 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 diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 64408b520..f37d02312 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/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 lastRelinVariables_; #endif - typedef HessianFactor CacheFactor; - public: typedef BayesTree Base; ///< The BayesTree base class @@ -460,7 +471,7 @@ private: FastList getAffectedFactors(const FastList& keys) const; FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys) const; - FactorGraph getCachedBoundaryFactors(Cliques& orphans); + GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1c2290e8c..f30574d0b 100644 --- a/tests/testGaussianISAM2.cpp +++ b/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 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(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(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 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(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); + gradientAtZero(isam, actualGradient); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + /* ************************************************************************* */ TEST(ISAM2, clone) {