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()); 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()); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9556a80180..290f0138e6 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,47 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); } +/* ************************************************************************* */ +// 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); + 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, 0.3)); + graph.add(BetweenFactor(11, 12, T11.between(T12), model)); + graph.add(BetweenFactor(21, 22, T21.between(T22), model)); + + // Range factor + auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01); + graph.add(RangeFactor(12, 22, 1.0, model_rho)); + + 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); + EXPECT_LONGS_EQUAL(4, bn->size()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */