From 033807321d139dc1a47e2494557a6f0f08cbf138 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Sat, 28 Jan 2012 02:49:43 +0000
Subject: [PATCH] fixes in Pose2SLAM

---
 gtsam.h | 69 +++++++++++++++++++++++++--------------------------------
 1 file changed, 30 insertions(+), 39 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 92616574d..9399cc6d4 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -427,6 +427,36 @@ class Odometry {
 
 }///\namespace planarSLAM
 
+// Pose2SLAM example domain
+#include <gtsam/slam/pose2SLAM.h>
+namespace pose2SLAM {
+
+class Values {
+  Values();
+  void print(string s) const;
+  void insertPose(int key, const Pose2& pose);
+  Pose2 pose(int i);
+};
+
+class Graph {
+  Graph();
+
+  void print(string s) const;
+
+  double error(const pose2SLAM::Values& values) const;
+  Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
+  GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
+      const Ordering& ordering) const;
+
+  void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel);
+  void addConstraint(int key, const Pose2& pose);
+  void addOdometry(int key1, int key2, const Pose2& odometry, const SharedNoiseModel& noiseModel);
+  pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate);
+};
+
+}///\namespace pose2SLAM
+
+
 // Simulated2D Example Domain
 #include <gtsam/slam/simulated2D.h>
 namespace simulated2D {
@@ -471,45 +501,6 @@ class Graph {
 
 }///\namespace simulated2DOriented
 
-
-
-
-
-
-
-
-// Planar SLAM example domain
-#include <gtsam/slam/pose2SLAM.h>
-namespace pose2SLAM {
-
-class Values {
-	Values();
-	void print(string s) const;
-	Pose2 pose(int key) const;
-	Point2 point(int key) const;
-	void insertPose(int key, const Pose2& pose);
-	void insertPoint(int key, const Point2& point);
-};
-
-class Graph {
-	Graph();
-
-	void print(string s) const;
-
-	double error(const planarSLAM::Values& values) const;
-	Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
-	GaussianFactorGraph* linearize(const planarSLAM::Values& values,
-			const Ordering& ordering) const;
-
-	void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel);
-	void addHardConstraint(int key, const Pose2& pose);
-	void addConstraint(int key1, int key2, const Pose2& odometry, const SharedNoiseModel& noiseModel);
-	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
-};
-
-}///\namespace pose2SLAM
-
-
 //// These are considered to be broken and will be added back as they start working
 //// It's assumed that there have been interface changes that might break this
 //