From 22c38eb41a1ac225c0e5a6e42697a5d3f4845d0d Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Sat, 28 Jan 2012 03:01:54 +0000
Subject: [PATCH] added Matlab convenience function: optimize

---
 gtsam/slam/pose2SLAM.h | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 6f1b35386..e283c7911 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -94,6 +94,13 @@ namespace pose2SLAM {
     /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
     void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
         const SharedNoiseModel& model);
+
+    /// Optimize
+    Values optimize(const Values& initialEstimate) {
+      typedef NonlinearOptimizer<Graph, Values> Optimizer;
+      return *Optimizer::optimizeLM(*this, initialEstimate,
+                  NonlinearOptimizationParameters::LAMBDA);
+    }
   };
 
   /// The sequential optimizer