diff --git a/.cproject b/.cproject
index e65c713f6..0f9a11611 100644
--- a/.cproject
+++ b/.cproject
@@ -362,14 +362,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -396,6 +388,7 @@
make
+
tests/testBayesTree.run
true
false
@@ -403,6 +396,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -450,6 +444,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -457,6 +452,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -464,6 +460,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -479,11 +476,20 @@
make
+
tests/testBayesTree
true
false
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -510,7 +516,6 @@
make
-
testGraph.run
true
false
@@ -582,7 +587,6 @@
make
-
testInference.run
true
false
@@ -590,7 +594,6 @@
make
-
testGaussianFactor.run
true
false
@@ -598,7 +601,6 @@
make
-
testJunctionTree.run
true
false
@@ -606,7 +608,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -614,7 +615,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -684,22 +684,6 @@
false
true
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
make
-j2
@@ -716,6 +700,22 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
make
-j2
@@ -740,18 +740,26 @@
true
true
-
+
make
- -j5
- nonlinear.testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- nonlinear.testOrdering.run
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
true
true
true
@@ -788,26 +796,18 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ nonlinear.testValues.run
true
true
true
-
+
make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
+ -j5
+ nonlinear.testOrdering.run
true
true
true
@@ -1142,6 +1142,7 @@
make
+
testErrors.run
true
false
@@ -1597,7 +1598,6 @@
make
-
testSimulated2DOriented.run
true
false
@@ -1637,7 +1637,6 @@
make
-
testSimulated2D.run
true
false
@@ -1645,7 +1644,6 @@
make
-
testSimulated3D.run
true
false
@@ -1829,7 +1827,6 @@
make
-
tests/testGaussianISAM2
true
false
@@ -1851,102 +1848,6 @@
true
true
-
- make
- -j2
- testRot3.run
- true
- true
- true
-
-
- make
- -j2
- testRot2.run
- true
- true
- true
-
-
- make
- -j2
- testPose3.run
- true
- true
- true
-
-
- make
- -j2
- timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
- true
- true
- true
-
make
-j2
@@ -2138,42 +2039,106 @@
true
true
-
+
make
- -j5
- wrap.testSpirit.run
+ -j8
+ testNonlinearOptimizer.run
true
true
true
-
+
make
- -j5
- wrap.testWrap.run
+ -j2
+ testRot3.run
true
true
true
-
+
make
- -j5
- check.wrap
+ -j2
+ testRot2.run
true
true
true
-
+
make
- -j5
- wrap_gtsam
+ -j2
+ testPose3.run
true
true
true
-
+
make
- -j5
- wrap
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
true
true
true
@@ -2217,6 +2182,46 @@
false
true
+
+ make
+ -j5
+ wrap.testSpirit.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap.testWrap.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.wrap
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap
+ true
+ true
+ true
+
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 2b9865ada..5295ce9ff 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -68,9 +68,9 @@ void LevenbergMarquardtOptimizer::iterate() {
// Optimize
VectorValues delta;
if(params_.elimination == SuccessiveLinearizationParams::MULTIFRONTAL)
- delta = GaussianJunctionTree(*linear).optimize(eliminationMethod);
+ delta = GaussianJunctionTree(dampedSystem).optimize(eliminationMethod);
else if(params_.elimination == SuccessiveLinearizationParams::SEQUENTIAL)
- delta = gtsam::optimize(*EliminationTree::Create(*linear)->eliminate(eliminationMethod));
+ delta = gtsam::optimize(*EliminationTree::Create(dampedSystem)->eliminate(eliminationMethod));
else
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 7173253b8..80754ce02 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -233,6 +233,29 @@ TEST(NonlinearOptimizer, NullFactor) {
DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, MoreOptimization) {
+
+ NonlinearFactorGraph fg;
+ fg.add(PriorFactor(0, Pose2(0,0,0), sharedSigma(3,1)));
+ fg.add(BetweenFactor(0, 1, Pose2(1,0,M_PI/2), sharedSigma(3,1)));
+ fg.add(BetweenFactor(1, 2, Pose2(1,0,M_PI/2), sharedSigma(3,1)));
+
+ Values init;
+ init.insert(0, Pose2(3,4,-M_PI));
+ init.insert(1, Pose2(10,2,-M_PI));
+ init.insert(2, Pose2(11,7,-M_PI));
+
+ Values expected;
+ expected.insert(0, Pose2(0,0,0));
+ expected.insert(1, Pose2(1,0,M_PI/2));
+ expected.insert(2, Pose2(1,1,M_PI));
+
+ // Try LM and Dogleg
+ EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize()));
+ EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
+}
+
/* ************************************************************************* */
int main() {
TestResult tr;