From 754e289737ad398a1b71084e1eecdf92c0b95bb2 Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Mon, 14 May 2012 22:31:42 +0000
Subject: [PATCH] Fixed errors in unit tests from updated NonlinearOptimizers

---
 gtsam/nonlinear/DoglegOptimizer.h             |  4 ++--
 gtsam/nonlinear/GaussNewtonOptimizer.h        |  2 +-
 gtsam/nonlinear/LevenbergMarquardtOptimizer.h |  4 ++--
 tests/testNonlinearOptimizer.cpp              | 23 +++++++++++++++----
 4 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h
index 44813befa..69d0503d7 100644
--- a/gtsam/nonlinear/DoglegOptimizer.h
+++ b/gtsam/nonlinear/DoglegOptimizer.h
@@ -65,7 +65,7 @@ public:
   virtual ~DoglegState() {}
 
 protected:
-  DoglegState(const NonlinearFactorGraph& graph, const Values& values, const DoglegParams& params, unsigned int interations = 0) :
+  DoglegState(const NonlinearFactorGraph& graph, const Values& values, const DoglegParams& params, unsigned int iterations = 0) :
     NonlinearOptimizerState(graph, values, iterations), Delta(params.deltaInitial) {}
 
   friend class DoglegOptimizer;
@@ -105,7 +105,7 @@ public:
    */
   DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
         NonlinearOptimizer(graph) {
-    *params_.ordering = ordering;
+    params_.ordering = ordering;
     state_ = DoglegState(graph, initialValues, params_); }
 
   /// @}
diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h
index f7c57c3e9..39b64b667 100644
--- a/gtsam/nonlinear/GaussNewtonOptimizer.h
+++ b/gtsam/nonlinear/GaussNewtonOptimizer.h
@@ -70,7 +70,7 @@ public:
    */
   GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
         NonlinearOptimizer(graph), state_(graph, initialValues) {
-    *params_.ordering = ordering; }
+    params_.ordering = ordering; }
 
   /// @}
 
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
index f2e2e4fc7..a03140166 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
@@ -74,7 +74,7 @@ public:
 
 protected:
   LevenbergMarquardtState(const NonlinearFactorGraph& graph, const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) :
-    NonlinearOptimizerState(graph, values, iterations), lambda(params.lambdaInitial) {}
+    NonlinearOptimizerState(graph, initialValues, iterations), lambda(params.lambdaInitial) {}
 
   friend class LevenbergMarquardtOptimizer;
 };
@@ -114,7 +114,7 @@ public:
    */
   LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
         NonlinearOptimizer(graph), dimensions_(initialValues.dims(ordering)) {
-    *params_.ordering = ordering;
+    params_.ordering = ordering;
     state_ = LevenbergMarquardtState(graph, initialValues, params_); }
 
   /** Access the current damping value */
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index e1088683e..b8d80d645 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -54,11 +54,14 @@ TEST( NonlinearOptimizer, iterateLM )
 	config.insert(simulated2D::PoseKey(1), x0);
 
 	// normal iterate
-	GaussNewtonOptimizer gnOptimizer(fg, config);
+	GaussNewtonParams gnParams;
+	GaussNewtonOptimizer gnOptimizer(fg, config, gnParams);
 	gnOptimizer.iterate();
 
 	// LM iterate with lambda 0 should be the same
-	LevenbergMarquardtOptimizer lmOptimizer(fg, config);
+	LevenbergMarquardtParams lmParams;
+	lmParams.lambdaInitial = 0.0;
+	LevenbergMarquardtOptimizer lmOptimizer(fg, config, lmParams);
 	lmOptimizer.iterate();
 
 	CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
@@ -81,16 +84,26 @@ TEST( NonlinearOptimizer, optimize )
 	c0.insert(simulated2D::PoseKey(1), x0);
 	DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
 
+	// optimize parameters
+	Ordering ord;
+	ord.push_back(kx(1));
+
 	// Gauss-Newton
-	Values actual1 = GaussNewtonOptimizer(fg, c0).optimize();
+	GaussNewtonParams gnParams;
+	gnParams.ordering = ord;
+	Values actual1 = GaussNewtonOptimizer(fg, c0, gnParams).optimize();
 	DOUBLES_EQUAL(0,fg.error(actual1),tol);
 
 	// Levenberg-Marquardt
-  Values actual2 = LevenbergMarquardtOptimizer(fg, c0).optimize();
+	LevenbergMarquardtParams lmParams;
+	lmParams.ordering = ord;
+  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
   DOUBLES_EQUAL(0,fg.error(actual2),tol);
 
   // Dogleg
-  Values actual3 = DoglegOptimizer(fg, c0).optimize();
+  DoglegParams dlParams;
+  dlParams.ordering = ord;
+  Values actual3 = DoglegOptimizer(fg, c0, dlParams).optimize();
   DOUBLES_EQUAL(0,fg.error(actual3),tol);
 }