Merged in fix/ILS (pull request #423)

Diagnosing and fixing ILS error

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();
+  }
 }
 
 /* ************************************************************************* */

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<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
+    auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector));
-      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<BayesNetType> result = boost::make_shared<BayesNetType>();
+    auto result = boost::make_shared<BayesNetType>();
 
     // Run tree elimination algorithm
     FastVector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
 
     // Add remaining factors that were not involved with eliminated variables
-    boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
+    auto allRemainingFactors = boost::make_shared<FactorGraphType>();
     allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
     allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
 

gtsam/linear/HessianFactor.cpp

@@ -487,6 +487,11 @@ boost::shared_ptr<GaussianConditional> 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());
   }
 

tests/testNonlinearFactorGraph.cpp

@@ -17,16 +17,6 @@
  * @author  Christian Potthast
  */
 
-/*STL/C++*/
-#include <iostream>
-using namespace std;
-
-#include <boost/assign/std/list.hpp>
-#include <boost/assign/std/set.hpp>
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
 #include <tests/smallExample.h>
@@ -34,7 +24,21 @@ using namespace boost::assign;
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/sam/RangeFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
 
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/list.hpp>
+#include <boost/assign/std/set.hpp>
+using namespace boost::assign;
+
+/*STL/C++*/
+#include <iostream>
+
+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<Pose2>(11, T11, prior));
+  graph.add(PriorFactor<Pose2>(21, T21, prior));
+
+  // Odometry
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3));
+  graph.add(BetweenFactor<Pose2>(11, 12, T11.between(T12), model));
+  graph.add(BetweenFactor<Pose2>(21, 22, T21.between(T22), model));
+
+  // Range factor
+  auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01);
+  graph.add(RangeFactor<Pose2>(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); }
 /* ************************************************************************* */