From 9caf04dccda7aa0cc726237d68daf45b985406a8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 22 May 2012 04:30:30 +0000
Subject: [PATCH] addConstraint

---
 gtsam/slam/pose2SLAM.cpp |  6 ++++++
 gtsam/slam/pose2SLAM.h   | 13 ++++++++-----
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp
index e8cbcd7fe..dd80b821b 100644
--- a/gtsam/slam/pose2SLAM.cpp
+++ b/gtsam/slam/pose2SLAM.cpp
@@ -16,6 +16,7 @@
  **/
 
 #include <gtsam/slam/pose2SLAM.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 // Use pose2SLAM namespace for specific SLAM instance
 
@@ -51,5 +52,10 @@ namespace pose2SLAM {
   }
 
   /* ************************************************************************* */
+  Values Graph::optimize(const Values& initialEstimate) const {
+    return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
+  }
+
+  /* ************************************************************************* */
 
 } // pose2SLAM
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index 6dc682f9b..42e22a85d 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -17,14 +17,13 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/geometry/Pose2.h>
 
 // Use pose2SLAM namespace for specific SLAM instance
 namespace pose2SLAM {
@@ -93,10 +92,14 @@ namespace pose2SLAM {
     void addOdometry(Index i, Index j, const Pose2& z,
         const SharedNoiseModel& model);
 
-    /// Optimize
-    Values optimize(const Values& initialEstimate) const {
-      return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
+    /// AddConstraint adds a soft constraint between factor between keys i and j
+    void addConstraint(Index i, Index j, const Pose2& z,
+        const SharedNoiseModel& model) {
+    	addOdometry(i,j,z,model); // same for now
     }
+
+    /// Optimize
+    Values optimize(const Values& initialEstimate) const;
   };
 
 } // pose2SLAM