From 83a853f6f1d152e517b9d202e6f5874821136a27 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sat, 4 Aug 2012 19:48:52 +0000
Subject: [PATCH] Removed use of EasyFactorGraph

---
 .../dynamics/tests/testIMUSystem.cpp          | 20 +++++++------------
 1 file changed, 7 insertions(+), 13 deletions(-)

diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
index 2f6b8b201..86301f417 100644
--- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
+++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
@@ -12,7 +12,8 @@
 #include <gtsam/slam/RangeFactor.h>
 #include <gtsam/slam/PartialPriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/nonlinear/EasyFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 #include <gtsam_unstable/dynamics/IMUFactor.h>
 #include <gtsam_unstable/dynamics/FullIMUFactor.h>
@@ -27,13 +28,6 @@ const double tol=1e-5;
 static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4;
 static const Vector g = delta(3, 2, -9.81);
 
-//typedef gtsam::IMUFactor<gtsam::PoseRTV> IMUFactor<PoseRTV>; // IMU between measurements
-//typedef gtsam::FullIMUFactor<gtsam::PoseRTV> IMUFactor<PoseRTV>; // Full-state IMU between measurements
-//typedef gtsam::BetweenFactor<gtsam::PoseRTV> Between;   // full odometry (including velocity)
-//typedef gtsam::NonlinearEquality<gtsam::PoseRTV> Constraint;
-//typedef gtsam::PriorFactor<gtsam::PoseRTV> Prior;
-//typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> Range;
-
 /* ************************************************************************* */
 TEST(testIMUSystem, instantiations) {
 	// just checking for compilation
@@ -74,7 +68,7 @@ TEST( testIMUSystem, optimize_chain ) {
 	imu34 = pose3.imuPrediction(pose4, dt);
 
 	// assemble simple graph with IMU measurements and velocity constraints
-	EasyFactorGraph graph;
+	NonlinearFactorGraph graph;
 	graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
 	graph.add(IMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
 	graph.add(IMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
@@ -100,7 +94,7 @@ TEST( testIMUSystem, optimize_chain ) {
 	values.insert(x3, PoseRTV());
 	values.insert(x4, PoseRTV());
 
-	Values actual = graph.optimize(values);
+	Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
 	EXPECT(assert_equal(true_values, actual, tol));
 }
 
@@ -121,7 +115,7 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
 	imu34 = pose3.imuPrediction(pose4, dt);
 
 	// assemble simple graph with IMU measurements and velocity constraints
-	EasyFactorGraph graph;
+	NonlinearFactorGraph graph;
 	graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
 	graph.add(FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
 	graph.add(FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
@@ -146,7 +140,7 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
 
 	cout << "Initial Error: " << graph.error(values) << endl; // Initial error is 0.5 - need better prediction model
 
-	Values actual = graph.optimize(values);
+	Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
 //	EXPECT(assert_equal(true_values, actual, tol)); // FAIL
 }
 
@@ -162,7 +156,7 @@ TEST( testIMUSystem, linear_trajectory) {
 	SharedDiagonal model = noiseModel::Unit::Create(9);
 
 	Values true_traj, init_traj;
-	EasyFactorGraph graph;
+	NonlinearFactorGraph graph;
 
 	graph.add(NonlinearEquality<gtsam::PoseRTV>(x0, start));
 	true_traj.insert(x0, start);