From ed721a152a3b12c21bf9a6b4b7b1ea47e44d1a11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 20:55:16 -0400 Subject: [PATCH 1/4] Example coded, throws ILS --- tests/testNonlinearFactorGraph.cpp | 67 +++++++++++++++++++++++++----- 1 file changed, 57 insertions(+), 10 deletions(-) diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9556a80180..57d8dfb4a7 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -17,16 +17,6 @@ * @author Christian Potthast */ -/*STL/C++*/ -#include -using namespace std; - -#include -#include -using namespace boost::assign; - -#include - #include #include #include @@ -34,7 +24,21 @@ using namespace boost::assign; #include #include #include +#include +#include +#include +#include + +#include +#include +#include +using namespace boost::assign; + +/*STL/C++*/ +#include + +using namespace std; using namespace gtsam; using namespace example; @@ -197,6 +201,49 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); } +/* ************************************************************************* */ +// Example from issue #452 which threw an ILS error +TEST(testNonlinearFactorGraph, eliminate) { + // Linearization point + Pose2 T11(0, 0, 0); + Pose2 T12(1, 0, 0); + Pose2 T21(0, 1, 0); + Pose2 T22(1, 1, 0); + + // Factor graph + auto graph = NonlinearFactorGraph(); + + // Priors + auto prior = noiseModel::Isotropic::Sigma(3, 1); + graph.add(PriorFactor(11, T11, prior)); + graph.add(PriorFactor(21, T21, prior)); + + // Odometry + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 1e6)); + graph.add(BetweenFactor(11, 12, T11.between(T12), model)); + graph.add(BetweenFactor(21, 22, T21.between(T22), model)); + + // Range + auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01); + graph.add(RangeFactor(12, 22, 1.0, model_rho)); + + // Print graph + GTSAM_PRINT(graph); + + double sigma = 0.1; + Values values; + values.insert(11, T11.retract(Vector3(0.1,0.2,0.3))); + values.insert(12, T12); + values.insert(21, T21); + values.insert(22, T22); + auto linearized = graph.linearize(values); + + // Eliminate + Ordering ordering; + ordering += 11, 21, 12, 22; + auto bn = linearized->eliminateSequential(ordering); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 2527f34e75224a0441b2657117b50fc1cbecd591 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 22:32:13 -0400 Subject: [PATCH 2/4] Print extra debug information for ILS in Debug mode --- gtsam/linear/HessianFactor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index da7d7bd7bf..d16373c78e 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -487,6 +487,11 @@ boost::shared_ptr HessianFactor::eliminateCholesky(const Or // Erase the eliminated keys in this factor keys_.erase(begin(), begin() + nFrontals); } catch (const CholeskyFailed&) { +#ifndef NDEBUG + cout << "Partial Cholesky on HessianFactor failed." << endl; + keys.print("Frontal keys "); + print("HessianFactor:"); +#endif throw IndeterminantLinearSystemException(keys.front()); } From 3f487c24f790267d70bb56e7e5bdeba07263730a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 22:32:58 -0400 Subject: [PATCH 3/4] Small formatting/auto changes --- gtsam/base/SymmetricBlockMatrix.cpp | 3 ++- gtsam/inference/EliminationTree-inst.h | 7 +++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 5eaa2df1cd..7f41da137b 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -65,8 +65,9 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const { void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) { gttic(VerticalBlockMatrix_choleskyPartial); DenseIndex topleft = variableColOffsets_[blockStart_]; - if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) + if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) { throw CholeskyFailed(); + } } /* ************************************************************************* */ diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 333f898b87..33ba6e18da 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -49,8 +49,7 @@ namespace gtsam { // Do dense elimination step KeyVector keyAsVector(1); keyAsVector[0] = key; - std::pair, boost::shared_ptr > eliminationResult = - function(gatheredFactors, Ordering(keyAsVector)); + auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector)); // Add conditional to BayesNet output->push_back(eliminationResult.first); @@ -190,13 +189,13 @@ namespace gtsam { { gttic(EliminationTree_eliminate); // Allocate result - boost::shared_ptr result = boost::make_shared(); + auto result = boost::make_shared(); // Run tree elimination algorithm FastVector remainingFactors = inference::EliminateTree(result, *this, function); // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared(); + auto allRemainingFactors = boost::make_shared(); allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end()); From 70d3c9044d62cb31672adbf8e5973d00699a6738 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 22:33:22 -0400 Subject: [PATCH 4/4] Fixed ILS error in example --- tests/testNonlinearFactorGraph.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 57d8dfb4a7..290f0138e6 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -202,7 +202,8 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } /* ************************************************************************* */ -// Example from issue #452 which threw an ILS error +// Example from issue #452 which threw an ILS error. The reason was a very +// weak prior on heading, which was tightened, and the ILS disappeared. TEST(testNonlinearFactorGraph, eliminate) { // Linearization point Pose2 T11(0, 0, 0); @@ -219,18 +220,14 @@ TEST(testNonlinearFactorGraph, eliminate) { graph.add(PriorFactor(21, T21, prior)); // Odometry - auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 1e6)); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3)); graph.add(BetweenFactor(11, 12, T11.between(T12), model)); graph.add(BetweenFactor(21, 22, T21.between(T22), model)); - // Range + // Range factor auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01); graph.add(RangeFactor(12, 22, 1.0, model_rho)); - // Print graph - GTSAM_PRINT(graph); - - double sigma = 0.1; Values values; values.insert(11, T11.retract(Vector3(0.1,0.2,0.3))); values.insert(12, T12); @@ -242,6 +239,7 @@ TEST(testNonlinearFactorGraph, eliminate) { Ordering ordering; ordering += 11, 21, 12, 22; auto bn = linearized->eliminateSequential(ordering); + EXPECT_LONGS_EQUAL(4, bn->size()); } /* ************************************************************************* */