From d0ce5d36da1bc15a2043e5f442e3acc90e1d4515 Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Fri, 19 Feb 2010 02:48:47 +0000
Subject: [PATCH] created addPrior

---
 cpp/SymbolicFactorGraph.cpp | 7 +++----
 cpp/planarSLAM.cpp          | 6 ++++++
 cpp/planarSLAM.h            | 3 +++
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp
index 70c893f2c..026c8f01f 100644
--- a/cpp/SymbolicFactorGraph.cpp
+++ b/cpp/SymbolicFactorGraph.cpp
@@ -52,7 +52,8 @@ namespace gtsam {
 		Point2 pt;
 		float scale = 100;
 
-		ofstream of(s.c_str());
+		string dotfile = s + ".dot";
+		ofstream of(dotfile.c_str());
 		of << "graph G{" << endl;
 		of << "bgcolor=\"transparent\";" << endl;
 
@@ -74,9 +75,7 @@ namespace gtsam {
 		of<<"}";
 		of.close();
 
-		char filename[100];
-		sscanf(s.c_str(), "%s.dot", filename);
-		string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % s % filename);
+		string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % dotfile % s);
 		system(cmd.c_str());
 	}
 
diff --git a/cpp/planarSLAM.cpp b/cpp/planarSLAM.cpp
index ba89cbf3e..928b213e9 100644
--- a/cpp/planarSLAM.cpp
+++ b/cpp/planarSLAM.cpp
@@ -20,6 +20,12 @@ namespace gtsam {
 
 	namespace planarSLAM {
 
+		void Graph::addPrior(const PoseKey& i, const Pose2& p,
+				const SharedGaussian& model) {
+			sharedFactor factor(new Prior(i, p, model));
+			push_back(factor);
+		}
+
 	  void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
 	  	sharedFactor factor(new Constraint(i, p));
 			push_back(factor);
diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h
index 2411e7364..d86d659d9 100644
--- a/cpp/planarSLAM.h
+++ b/cpp/planarSLAM.h
@@ -12,6 +12,7 @@
 #include "NonlinearFactor.h"
 #include "TupleConfig.h"
 #include "NonlinearEquality.h"
+#include "PriorFactor.h"
 #include "BetweenFactor.h"
 #include "NonlinearFactorGraph.h"
 #include "NonlinearOptimizer.h"
@@ -86,12 +87,14 @@ namespace gtsam {
 
 		// Factors
 		typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;
+		typedef PriorFactor<Config, PoseKey, Pose2> Prior;
 		typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
 		typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
 		typedef RangeFactor<Config, PoseKey, PointKey> Range;
 
 		// Graph
 		struct Graph: public NonlinearFactorGraph<Config> {
+			void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model);
 			void addPoseConstraint(const PoseKey& i, const Pose2& p);
 			void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
 					const SharedGaussian& model);