From cdd89a43f56ffdfd4648ac58e556fe5e649c16ca Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 23 Mar 2012 03:43:28 +0000
Subject: [PATCH] DoglegOptimizer unit tests

---
 tests/testNonlinearOptimizer.cpp | 39 ++++++++++++++------------------
 1 file changed, 17 insertions(+), 22 deletions(-)

diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 5f228f3e7..cb97a6ae7 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -31,10 +31,9 @@ using namespace boost;
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/NoiseModel.h>
-
-// template definitions
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/DoglegOptimizer.h>
 
 using namespace gtsam;
 
@@ -100,6 +99,10 @@ TEST( NonlinearOptimizer, optimize )
 	// Levenberg-Marquardt
 	NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimize();
 	DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
+
+  // Dogleg
+  NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, DoglegParams(), ord).optimize();
+  DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
 }
 
 /* ************************************************************************* */
@@ -116,19 +119,6 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
 	DOUBLES_EQUAL(0,fg->error(*actual),tol);
 }
 
-/* ************************************************************************* */
-TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared )
-{
-	example::Graph fg = example::createReallyNonlinearFactorGraph();
-
-	Point2 x0(3,3);
-	Values c0;
-	c0.insert(simulated2D::PoseKey(1), x0);
-
-  Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimize()->values();
-	DOUBLES_EQUAL(0,fg.error(*actual),tol);
-}
-
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, SimpleGNOptimizer )
 {
@@ -144,16 +134,17 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
 }
 
 /* ************************************************************************* */
-TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared )
+TEST( NonlinearOptimizer, SimpleDLOptimizer )
 {
-	example::Graph fg = example::createReallyNonlinearFactorGraph();
+  shared_ptr<example::Graph> fg(new example::Graph(
+      example::createReallyNonlinearFactorGraph()));
 
-	Point2 x0(3,3);
-	Values c0;
-	c0.insert(simulated2D::PoseKey(1), x0);
+  Point2 x0(3,3);
+  boost::shared_ptr<Values> c0(new Values);
+  c0->insert(simulated2D::PoseKey(1), x0);
 
-  Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimize()->values();
-	DOUBLES_EQUAL(0,fg.error(*actual),tol);
+  Values::const_shared_ptr actual = DoglegOptimizer(fg, c0).optimize()->values();
+  DOUBLES_EQUAL(0,fg->error(*actual),tol);
 }
 
 /* ************************************************************************* */
@@ -231,6 +222,10 @@ TEST(NonlinearOptimizer, NullFactor) {
   // Levenberg-Marquardt
   NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
   DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
+
+  // Dogleg
+  NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, ord).optimize();
+  DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
 }
 
 /* ************************************************************************* */