Fixed bugs, all unit tests pass

.cproject

@@ -2053,6 +2053,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testNonlinearOptimizer.run" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j7</buildArguments>
+				<buildTarget>testNonlinearOptimizer.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/nonlinear/GaussNewtonOptimizer.h

@@ -210,7 +210,7 @@ private:
   // error, and increments the iteration count.
   GaussNewtonOptimizer(const GaussNewtonOptimizer& original,
       const SharedValues& newValues, double newError) :
-        NonlinearOptimizer(graph_, newValues, params_, newError, iterations_+1),
+        NonlinearOptimizer(original.graph_, newValues, original.params_, newError, original.iterations_+1),
         gnParams_(original.gnParams_),
         colamdOrdering_(original.colamdOrdering_),
         ordering_(original.ordering_) {}

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -46,7 +46,7 @@ NonlinearOptimizer::auto_ptr LevenbergMarquardtOptimizer::iterate() const {
   const double lambdaFactor = lmParams_->lambdaFactor;
 
   // Variables to update during try_lambda loop
-  double lambda = lmParams_->lambdaInitial;
+  double lambda = lambda_;
   double next_error = error();
   SharedValues next_values = values();
 

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -263,7 +263,7 @@ private:
   // error, and increments the iteration count.
   LevenbergMarquardtOptimizer(const LevenbergMarquardtOptimizer& original,
       const SharedValues& newValues, double newError, double newLambda) :
-        NonlinearOptimizer(graph_, newValues, params_, newError, iterations_+1),
+        NonlinearOptimizer(original.graph_, newValues, original.params_, newError, original.iterations_+1),
         lmParams_(original.lmParams_),
         colamdOrdering_(original.colamdOrdering_),
         ordering_(original.ordering_),

tests/testNonlinearOptimizer.cpp

@@ -163,7 +163,6 @@ TEST( NonlinearOptimizer, optimization_method )
   paramsQR.factorization = LevenbergMarquardtParams::QR;
   LevenbergMarquardtParams paramsLDL;
   paramsLDL.factorization = LevenbergMarquardtParams::LDL;
-  EXPECT(false);
 
 	example::Graph fg = example::createReallyNonlinearFactorGraph();
 
@@ -202,7 +201,7 @@ TEST( NonlinearOptimizer, Factorization )
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(NonlinearOptimizer, NullFactor) {
+TEST(NonlinearOptimizer, NullFactor) {
 
   example::Graph fg = example::createReallyNonlinearFactorGraph();
 
@@ -234,66 +233,6 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) {
   DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( NonlinearOptimizer, SubgraphSolver )
-//{
-//	using namespace pose2SLAM;
-//	typedef SubgraphSolver<Graph, Values> Solver;
-//	typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, Solver> Optimizer;
-//
-//	// Create a graph
-//	boost::shared_ptr<Graph> graph(new Graph);
-//	graph->addPrior(1, Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1e-10));
-//	graph->addConstraint(1, 2, Pose2(1., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
-//
-//	// Create an initial config
-//	boost::shared_ptr<Values> config(new Values);
-//	config->insert(1, Pose2(0., 0., 0.));
-//	config->insert(2, Pose2(1.5, 0., 0.));
-//
-//	// Create solver and optimizer
-//	Optimizer::shared_solver solver
-//		(new SubgraphSolver<Graph, Values> (*graph, *config));
-//	Optimizer optimizer(graph, config, solver);
-//
-//	// Optimize !!!!
-//	double relativeThreshold = 1e-5;
-//	double absoluteThreshold = 1e-5;
-//	Optimizer optimized = optimizer.gaussNewton(relativeThreshold,
-//			absoluteThreshold, Optimizer::SILENT);
-//
-//	// Check solution
-//	Values expected;
-//	expected.insert(1, Pose2(0., 0., 0.));
-//	expected.insert(2, Pose2(1., 0., 0.));
-//	CHECK(assert_equal(expected, *optimized.values(), 1e-5));
-//}
-
-/* ************************************************************************* */
-// SL-FIX TEST( NonlinearOptimizer, MultiFrontalSolver )
-//{
-//	shared_ptr<example::Graph> fg(new example::Graph(
-//			example::createNonlinearFactorGraph()));
-//	Optimizer::shared_values initial = example::sharedNoisyValues();
-//
-//	Values expected;
-//	expected.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0));
-//	expected.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0));
-//	expected.insert(simulated2D::PointKey(1), Point2(0.0, -1.0));
-//
-//	Optimizer::shared_solver solver;
-//
-//	// Check one ordering
-//	shared_ptr<Ordering> ord1(new Ordering());
-//	*ord1 += kx(2),kl(1),kx(1);
-//	solver = Optimizer::shared_solver(new Optimizer::solver(ord1));
-//	Optimizer optimizer1(fg, initial, solver);
-//
-//	Values actual = optimizer1.levenbergMarquardt();
-//	CHECK(assert_equal(actual,expected));
-//}
-
-
 /* ************************************************************************* */
 int main() {
 	TestResult tr;