created addPrior

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];
+		string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % dotfile % s);
-		sscanf(s.c_str(), "%s.dot", filename);
-		string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % s % filename);
 		system(cmd.c_str());
 	}
 

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);

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);