Added MATLAB utility functions to pose2SLAM

examples/matlab/Pose2SLAMExample_graph.m

@@ -12,7 +12,7 @@
 
 %% Create graph container and add factors to it
 graph = pose2SLAMGraph;
-initialEstimate = pose2SLAMValues;
+initial = pose2SLAMValues;
 odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
 constraint=gtsamPose2; % identity
 
@@ -27,35 +27,33 @@ if fid < 0
     error('Cannot open the file ');
 end
 
-disp('Reading data from file...')
-lines=textscan(fid,'%s','delimiter','\n');
+columns=textscan(fid,'%s','delimiter','\n');
 fclose(fid);
-n=size(lines{1},1);
+lines=columns{1};
+n=size(lines,1);
 
 for i=1:n
-    text=cell2mat(lines{1}(i));
-    if strcmp('VERTEX2',text(1:7))
-        v = textscan(text,'%s %d %f %f %f',1);
-        initialEstimate.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5}));
-    elseif strcmp('EDGE2',text(1:5))
-        e = textscan(text,'%s %d %d %f %f %f',1);
+    line_i=lines{i};
+    if strcmp('VERTEX2',line_i(1:7))
+        v = textscan(line_i,'%s %d %f %f %f',1);
+        initial.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5}));
+    elseif strcmp('EDGE2',line_i(1:5))
+        e = textscan(line_i,'%s %d %d %f %f %f',1);
         graph.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), odometryNoise);
     end
 end
 
 %% Plot Initial Estimate
-figure(1)
-clf
-plotTrajectory(initialEstimate,'r-*')
-hold on
+figure(1);clf
+plot(initial.xs(),initial.ys(),'r-*'); axis equal
 
 addEquivalences=0;
 if addEquivalences
     %% Add equivalence constraints
     for i=1:n
-        text=cell2mat(lines{1}(i));
-        if strcmp('EQUIV',text(1:5))
-            equiv = textscan(text,'%s %d %d',1);
+        line_i=cell2mat(lines(i));
+        if strcmp('EQUIV',line_i(1:5))
+            equiv = textscan(line_i,'%s %d %d',1);
             graph.addOdometry(e{2}, e{3}, constraint, odometryNoise);
         end
     end
@@ -63,8 +61,8 @@ if addEquivalences
 end
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimize(initialEstimate);
-plotTrajectory(result,'g-*')
+result = graph.optimize(initial);
+hold on; plot(result.xs(),result.ys(),'g-*')
 
 %% Plot Covariance Ellipses
 % marginals = graph.marginals(result);

examples/matlab/plotTrajectory.m

@@ -1,10 +0,0 @@
-function plotTrajectory(values,color)
-% plotTrajectory: plot the poses in a values object
-% assumes keys are 0 to N-1, where N is values.size()
-X=[];Y=[];
-for i=0:values.size()-1
-    pose_i = values.pose(i);
-    X=[X;pose_i.x];
-    Y=[Y;pose_i.y];
-end
-plot(X,Y,color);

gtsam.h

@@ -448,9 +448,12 @@ namespace pose2SLAM {
 class Values {
 	Values();
 	void insertPose(size_t key, const gtsam::Pose2& pose);
-    size_t size() const;
+	size_t size() const;
 	void print(string s) const;
 	gtsam::Pose2 pose(size_t i);
+  Vector xs() const;
+  Vector ys() const;
+  Vector thetas() const;
 };
 
 class Graph {

gtsam/slam/pose2SLAM.cpp

@@ -31,6 +31,36 @@ namespace pose2SLAM {
     return x;
   }
 
+  /* ************************************************************************* */
+	Vector Values::xs() const {
+		size_t j=0;
+		Vector result(size());
+		ConstFiltered<Pose2> poses = filter<Pose2>();
+		BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
+			result(j++) = keyValue.value.x();
+		return result;
+	}
+
+  /* ************************************************************************* */
+	Vector Values::ys() const {
+		size_t j=0;
+		Vector result(size());
+		ConstFiltered<Pose2> poses = filter<Pose2>();
+		BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
+			result(j++) = keyValue.value.y();
+		return result;
+	}
+
+  /* ************************************************************************* */
+	Vector Values::thetas() const {
+		size_t j=0;
+		Vector result(size());
+		ConstFiltered<Pose2> poses = filter<Pose2>();
+		BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
+			result(j++) = keyValue.value.theta	();
+		return result;
+	}
+
   /* ************************************************************************* */
   void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
     sharedFactor factor(new Prior(i, p, model));

gtsam/slam/pose2SLAM.h

@@ -41,13 +41,15 @@ namespace pose2SLAM {
         gtsam::Values(values) {
     }
 
-    // Convenience for MATLAB wrapper, which does not allow for identically named methods
+    /// insert a pose
+    void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
 
     /// get a pose
     Pose2 pose(Key i) const { return at<Pose2>(i); }
 
-    /// insert a pose
-    void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
+    Vector xs() const; ///< get all x coordinates in a matrix
+    Vector ys() const; ///< get all y coordinates in a matrix
+    Vector thetas() const; ///< get all orientations in a matrix
   };
 
   /**

gtsam/slam/tests/testPose2SLAM.cpp

@@ -41,7 +41,17 @@ static Matrix cov(Matrix_(3, 3,
 	0.0, 0.0, st*st
 	));
 static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov));
-//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
+
+/* ************************************************************************* */
+TEST_UNSAFE( Pose2SLAM, XYT )
+{
+	pose2SLAM::Values values;
+	values.insertPose(1,Pose2(1,2,3));
+	values.insertPose(2,Pose2(4,5,6));
+	EXPECT(assert_equal(Vector_(2,1.0,4.0),values.xs()));
+	EXPECT(assert_equal(Vector_(2,2.0,5.0),values.ys()));
+	EXPECT(assert_equal(Vector_(2,3.0,6.0-2*M_PI),values.thetas()));
+}
 
 /* ************************************************************************* */
 // Test constraint, small displacement