new Pose3SLAM examples in C++ and MATLAB

examples/Pose2SLAMExample.cpp

@@ -26,7 +26,7 @@ using namespace gtsam;
 int main(int argc, char** argv) {
 
 	// 1. Create graph container and add factors to it
-	pose2SLAM::Graph graph ;
+	pose2SLAM::Graph graph;
 
 	// 2a. Add Gaussian prior
 	Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
@@ -35,25 +35,25 @@ int main(int argc, char** argv) {
 
 	// 2b. Add odometry factors
 	SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
-	graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0   ), odometryNoise);
+	graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
 	graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 	graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 	graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 
 	// 2c. Add pose constraint
-	SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1));
+	SharedDiagonal model(Vector_(3, 0.2, 0.2, 0.1));
-	graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
+	graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model);
 
 	// print
 	graph.print("\nFactor graph:\n");
 
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+	// 3. Create the data structure to hold the initialEstimate estimate to the solution
 	pose2SLAM::Values initialEstimate;
-	Pose2 x1(0.5, 0.0, 0.2   ); initialEstimate.insertPose(1, x1);
+	initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
-	Pose2 x2(2.3, 0.1,-0.2   ); initialEstimate.insertPose(2, x2);
+	initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2));
-	Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3);
+	initialEstimate.insertPose(3, Pose2(4.1, 0.1, M_PI_2));
-	Pose2 x4(4.0, 2.0, M_PI  ); initialEstimate.insertPose(4, x4);
+	initialEstimate.insertPose(4, Pose2(4.0, 2.0, M_PI));
-	Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5);
+	initialEstimate.insertPose(5, Pose2(2.1, 2.1, -M_PI_2));
 	initialEstimate.print("\nInitial estimate:\n");
 
 	// 4. Single Step Optimization using Levenberg-Marquardt

examples/matlab/Pose2SLAMExample.m

@@ -36,19 +36,19 @@ graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
 graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
 
 %% Add pose constraint
-constraintUncertainty = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
+model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
-graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), constraintUncertainty);
+graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
 
 % print
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
 initialEstimate = pose2SLAMValues;
-x1 = gtsamPose2(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
+initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
-x2 = gtsamPose2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
+initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
-x3 = gtsamPose2(4.1, 0.1, pi/2); initialEstimate.insertPose(3, x3);
+initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
-x4 = gtsamPose2(4.0, 2.0, pi  ); initialEstimate.insertPose(4, x4);
+initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi  ));
-x5 = gtsamPose2(2.1, 2.1,-pi/2); initialEstimate.insertPose(5, x5);
+initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd

examples/matlab/Pose2SLAMExample_circle.m

@@ -0,0 +1,48 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Read graph from file and perform GraphSLAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Create a hexagon of poses
+hexagon = pose2SLAMValues_Circle(6,1.0);
+p0 = hexagon.pose(0);
+p1 = hexagon.pose(1);
+
+%% create a Pose graph with one equality constraint and one measurement
+fg = pose2SLAMGraph;
+fg.addPoseConstraint(0, p0);
+delta = p0.between(p1);
+covariance = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]);
+fg.addOdometry(0,1, delta, covariance);
+fg.addOdometry(1,2, delta, covariance);
+fg.addOdometry(2,3, delta, covariance);
+fg.addOdometry(3,4, delta, covariance);
+fg.addOdometry(4,5, delta, covariance);
+fg.addOdometry(5,0, delta, covariance);
+
+%% Create initial config
+initial = pose2SLAMValues;
+initial.insertPose(0, p0);
+initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]'));
+initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]'));
+initial.insertPose(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]'));
+initial.insertPose(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]'));
+initial.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]'));
+
+%% Plot Initial Estimate
+figure(1);clf
+plot(initial.xs(),initial.ys(),'g-*'); axis equal
+
+%% optimize
+result = fg.optimize(initial);
+
+%% Show Result
+hold on; plot(result.xs(),result.ys(),'b-*')
+result.print(sprintf('\nFinal result:\n'));

examples/matlab/Pose3SLAMExample_circle.m

@@ -0,0 +1,49 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Read graph from file and perform GraphSLAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Create a hexagon of poses
+hexagon = pose3SLAMValues_Circle(6,1.0);
+p0 = hexagon.pose(0);
+p1 = hexagon.pose(1);
+
+%% create a Pose graph with one equality constraint and one measurement
+fg = pose3SLAMGraph;
+fg.addHardConstraint(0, p0);
+delta = p0.between(p1);
+covariance = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+fg.addConstraint(0,1, delta, covariance);
+fg.addConstraint(1,2, delta, covariance);
+fg.addConstraint(2,3, delta, covariance);
+fg.addConstraint(3,4, delta, covariance);
+fg.addConstraint(4,5, delta, covariance);
+fg.addConstraint(5,0, delta, covariance);
+
+%% Create initial config
+initial = pose3SLAMValues;
+s = 0.10;
+initial.insertPose(0, p0);
+initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));
+initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1)));
+initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1)));
+initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1)));
+initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
+
+%% Plot Initial Estimate
+figure(1);clf
+plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); axis equal
+
+%% optimize
+result = fg.optimize(initial);
+
+%% Show Result
+hold on; plot3(result.xs(),result.ys(),result.zs(),'b-*')
+result.print(sprintf('\nFinal result:\n'));

examples/matlab/Pose3SLAMExample_graph.m

@@ -0,0 +1,28 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Read graph from file and perform GraphSLAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Initialize graph, initial estimate, and odometry noise
+model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+[graph,initial]=load3D('../Data/sphere_smallnoise.graph',model,100);
+
+%% Fix first pose
+first = initial.pose(0);
+graph.addHardConstraint(0, first); % add directly to graph
+
+%% Plot Initial Estimate
+figure(1);clf
+plot3(initial.xs(),initial.ys(),initial.zs(),'g-'); hold on
+plot3(first.x(),first.y(),first.z(),'r*'); 
+
+%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+result = graph.optimize(initial);
+hold on; plot3(result.xs(),result.ys(),result.zs(),'b-');axis equal; 

examples/matlab/load3D.m

@@ -0,0 +1,38 @@
+function [graph,initial] = load3D(filename,model,N)
+% load3D: read TORO 3D pose graph
+% cannot read noise model from file yet, uses specified model
+fid = fopen(filename);
+if fid < 0
+    error(['load2D: Cannot open file ' filename]);
+end
+
+% scan all lines into a cell array
+columns=textscan(fid,'%s','delimiter','\n');
+fclose(fid);
+lines=columns{1};
+
+% loop over lines and add vertices
+graph = pose3SLAMGraph;
+initial = pose3SLAMValues;
+n=size(lines,1);
+if nargin<3, N=n;end
+
+for i=1:n
+    line_i=lines{i};
+    if strcmp('VERTEX3',line_i(1:7))
+        v = textscan(line_i,'%s %d %f %f %f %f %f %f',1);
+        if v{2}<N
+            t = gtsamPoint3(v{3}, v{4}, v{5});
+            R = gtsamRot3_ypr(v{6}, v{7}, v{8});
+            initial.insertPose(v{2}, gtsamPose3(R,t));
+        end
+    elseif strcmp('EDGE3',line_i(1:5))
+        e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
+        if e{2}<N & e{3}<N
+            t = gtsamPoint3(e{4}, e{5}, e{6});
+            R = gtsamRot3_ypr(e{7}, e{8}, e{9});
+            graph.addConstraint(e{2}, e{3}, gtsamPose3(R,t), model);
+        end
+    end
+end
+

gtsam.h

@@ -447,9 +447,10 @@ namespace pose2SLAM {
 
 class Values {
 	Values();
-	void insertPose(size_t key, const gtsam::Pose2& pose);
 	size_t size() const;
 	void print(string s) const;
+	static pose2SLAM::Values Circle(size_t n, double R);
+	void insertPose(size_t key, const gtsam::Pose2& pose);
 	gtsam::Pose2 pose(size_t i);
   Vector xs() const;
   Vector ys() const;
@@ -459,26 +460,79 @@ class Values {
 class Graph {
 	Graph();
 
+	// FactorGraph
 	void print(string s) const;
+	bool equals(const pose2SLAM::Graph& fg, double tol) const;
+	size_t size() const;
+	bool empty() const;
+	void remove(size_t i);
+	size_t nrFactors() const;
 
+	// NonlinearFactorGraph
 	double error(const pose2SLAM::Values& values) const;
+	double probPrime(const pose2SLAM::Values& values) const;
 	gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
 	gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
 			const gtsam::Ordering& ordering) const;
 
-	void addPrior(size_t key, const gtsam::Pose2& pose,
+	// pose2SLAM-specific
-			const gtsam::SharedNoiseModel& noiseModel);
+	void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
 	void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
-	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry,
+	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
-			const gtsam::SharedNoiseModel& noiseModel);
+	void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
-	void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry,
-			const gtsam::SharedNoiseModel& noiseModel);
 	pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
 	gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
 };
 
 }///\namespace pose2SLAM
 
+//*************************************************************************
+// Pose3SLAM
+//*************************************************************************
+
+#include <gtsam/slam/pose3SLAM.h>
+namespace pose3SLAM {
+
+class Values {
+	Values();
+	size_t size() const;
+	void print(string s) const;
+	static pose3SLAM::Values Circle(size_t n, double R);
+	void insertPose(size_t key, const gtsam::Pose3& pose);
+	gtsam::Pose3 pose(size_t i);
+  Vector xs() const;
+  Vector ys() const;
+  Vector zs() const;
+};
+
+class Graph {
+	Graph();
+
+	// FactorGraph
+	void print(string s) const;
+	bool equals(const pose3SLAM::Graph& fg, double tol) const;
+	size_t size() const;
+	bool empty() const;
+	void remove(size_t i);
+	size_t nrFactors() const;
+
+	// NonlinearFactorGraph
+	double error(const pose3SLAM::Values& values) const;
+	double probPrime(const pose3SLAM::Values& values) const;
+	gtsam::Ordering* orderingCOLAMD(const pose3SLAM::Values& values) const;
+	gtsam::GaussianFactorGraph* linearize(const pose3SLAM::Values& values,
+			const gtsam::Ordering& ordering) const;
+
+	// pose3SLAM-specific
+	void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
+	void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::SharedNoiseModel& model);
+	void addHardConstraint(size_t i, const gtsam::Pose3& p);
+	pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const;
+	gtsam::Marginals marginals(const pose3SLAM::Values& solution) const;
+};
+
+}///\namespace pose3SLAM
+
 //*************************************************************************
 // planarSLAM
 //*************************************************************************
@@ -488,9 +542,10 @@ namespace planarSLAM {
 
 class Values {
 	Values();
+	size_t size() const;
+	void print(string s) const;
 	void insertPose(size_t key, const gtsam::Pose2& pose);
 	void insertPoint(size_t key, const gtsam::Point2& point);
-	void print(string s) const;
 	gtsam::Pose2 pose(size_t key) const;
 	gtsam::Point2 point(size_t key) const;
 };
@@ -498,25 +553,30 @@ class Values {
 class Graph {
 	Graph();
 
+	// FactorGraph
 	void print(string s) const;
+	bool equals(const planarSLAM::Graph& fg, double tol) const;
+	size_t size() const;
+	bool empty() const;
+	void remove(size_t i);
+	size_t nrFactors() const;
 
+	// NonlinearFactorGraph
 	double error(const planarSLAM::Values& values) const;
+	double probPrime(const planarSLAM::Values& values) const;
 	gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
 	gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
 			const gtsam::Ordering& ordering) const;
 
-	void addPrior(size_t key, const gtsam::Pose2& pose,
+	// planarSLAM-specific
-			const gtsam::SharedNoiseModel& noiseModel);
+	void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
 	void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
-	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry,
+	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
-			const gtsam::SharedNoiseModel& noiseModel);
+	void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel);
-	void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,
+	void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
-			const gtsam::SharedNoiseModel& noiseModel);
+	void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::SharedNoiseModel& noiseModel);
-	void addRange(size_t poseKey, size_t pointKey, double range,
-			const gtsam::SharedNoiseModel& noiseModel);
-	void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,
-			double range, const gtsam::SharedNoiseModel& noiseModel);
 	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
+	gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
 };
 
 class Odometry {

gtsam/slam/planarSLAM.h

@@ -24,8 +24,13 @@
 #include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose2.h>
 
+/**
+ * @defgroup SLAM
+ */
+
 // Use planarSLAM namespace for specific SLAM instance
 namespace planarSLAM {
 
@@ -48,9 +53,9 @@ namespace planarSLAM {
   /// A factor between a pose and a point to express difference in rotation and location
   typedef BearingRangeFactor<Pose2, Point2> BearingRange;
 
-  /**
+  /*
-   * Values class, using specific poses and points
+   * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
-   * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
+   * @ingroup SLAM
    */
   struct Values: public gtsam::Values {
 
@@ -75,7 +80,10 @@ namespace planarSLAM {
     void insertPoint(Key j, const Point2& point) { insert(j, point); }
   };
 
-  /// Creates a NonlinearFactorGraph with the Values type
+  /**
+   * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
+   * @ingroup SLAM
+   */
   struct Graph: public NonlinearFactorGraph {
 
     /// Default constructor for a NonlinearFactorGraph
@@ -104,6 +112,11 @@ namespace planarSLAM {
 
     /// Optimize
     Values optimize(const Values& initialEstimate) const;
+
+    /// Return a Marginals object
+    Marginals marginals(const Values& solution) const {
+    	return Marginals(*this,solution);
+    }
   };
 
 } // planarSLAM

gtsam/slam/pose2SLAM.cpp

@@ -23,7 +23,7 @@
 namespace pose2SLAM {
 
   /* ************************************************************************* */
-  Values circle(size_t n, double R) {
+  Values Values::Circle(size_t n, double R) {
     Values x;
     double theta = 0, dtheta = 2 * M_PI / n;
     for (size_t i = 0; i < n; i++, theta += dtheta)

gtsam/slam/pose2SLAM.h

@@ -30,7 +30,10 @@ namespace pose2SLAM {
 
   using namespace gtsam;
 
-  /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
+  /*
+   * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
+   * @ingroup SLAM
+   */
   struct Values: public gtsam::Values {
 
 	  typedef boost::shared_ptr<Values> shared_ptr;
@@ -43,6 +46,15 @@ namespace pose2SLAM {
         gtsam::Values(values) {
     }
 
+    /**
+     * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
+     * @param n number of poses
+     * @param R radius of circle
+     * @param c character to use for keys
+     * @return circle of n 2D poses
+     */
+    static Values Circle(size_t n, double R);
+
     /// insert a pose
     void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
 
@@ -54,15 +66,6 @@ namespace pose2SLAM {
     Vector thetas() const; ///< get all orientations in a matrix
   };
 
-  /**
-   * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
-   * @param n number of poses
-   * @param R radius of circle
-   * @param c character to use for keys
-   * @return circle of n 2D poses
-   */
-  Values circle(size_t n, double R);
-
   /**
    * List of typedefs for factors
    */
@@ -74,7 +77,10 @@ namespace pose2SLAM {
   /// A factor to add an odometry measurement between two poses.
   typedef BetweenFactor<Pose2> Odometry;
 
-  /// Graph
+  /**
+   * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
+   * @ingroup SLAM
+   */
   struct Graph: public NonlinearFactorGraph {
 
 	  typedef boost::shared_ptr<Graph> shared_ptr;

gtsam/slam/pose3SLAM.cpp

@@ -22,7 +22,7 @@
 namespace pose3SLAM {
 
   /* ************************************************************************* */
-  Values circle(size_t n, double radius) {
+  Values Values::Circle(size_t n, double radius) {
     Values x;
     double theta = 0, dtheta = 2 * M_PI / n;
     // We use aerospace/navlab convention, X forward, Y right, Z down
@@ -41,6 +41,36 @@ namespace pose3SLAM {
     return x;
   }
 
+  /* ************************************************************************* */
+	Vector Values::xs() const {
+		size_t j=0;
+		Vector result(size());
+		ConstFiltered<Pose3> poses = filter<Pose3>();
+		BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
+			result(j++) = keyValue.value.x();
+		return result;
+	}
+
+  /* ************************************************************************* */
+	Vector Values::ys() const {
+		size_t j=0;
+		Vector result(size());
+		ConstFiltered<Pose3> poses = filter<Pose3>();
+		BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
+			result(j++) = keyValue.value.y();
+		return result;
+	}
+
+  /* ************************************************************************* */
+	Vector Values::zs() const {
+		size_t j=0;
+		Vector result(size());
+		ConstFiltered<Pose3> poses = filter<Pose3>();
+		BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
+			result(j++) = keyValue.value.z();
+		return result;
+	}
+
   /* ************************************************************************* */
   void Graph::addPrior(Key i, const Pose3& p, const SharedNoiseModel& model) {
     sharedFactor factor(new Prior(i, p, model));

gtsam/slam/pose3SLAM.h

@@ -17,25 +17,53 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/geometry/Pose3.h>
 
 /// Use pose3SLAM namespace for specific SLAM instance
 namespace pose3SLAM {
 
   using namespace gtsam;
 
-  /**
+  /*
-   * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
+   * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
-   * @param n number of poses
+   * @ingroup SLAM
-   * @param R radius of circle
-   * @return circle of n 3D poses
    */
-  Values circle(size_t n, double R);
+  struct Values: public gtsam::Values {
+
+	  typedef boost::shared_ptr<Values> shared_ptr;
+
+	  /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) :
+        gtsam::Values(values) {
+    }
+
+    /**
+     * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
+     * @param n number of poses
+     * @param R radius of circle
+     * @return circle of n 3D poses
+     */
+    static Values Circle(size_t n, double R);
+
+    /// insert a pose
+    void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
+
+    /// get a pose
+    Pose3 pose(Key i) const { return at<Pose3>(i); }
+
+    Vector xs() const; ///< get all x coordinates in a matrix
+    Vector ys() const; ///< get all y coordinates in a matrix
+    Vector zs() const; ///< get all z coordinates in a matrix
+  };
 
   /// A prior factor on Key with Pose3 data type.
   typedef PriorFactor<Pose3> Prior;
@@ -44,7 +72,10 @@ namespace pose3SLAM {
   /// A hard constraint would enforce that the given key would have the input value in the results.
   typedef NonlinearEquality<Pose3> HardConstraint;
 
-  /// Graph
+  /**
+   * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
+   * @ingroup SLAM
+   */
   struct Graph: public NonlinearFactorGraph {
 
     /// Adds a factor between keys of the same type
@@ -61,6 +92,10 @@ namespace pose3SLAM {
       return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
     }
 
+    /// Return a Marginals object
+    Marginals marginals(const Values& solution) const {
+    	return Marginals(*this,solution);
+    }
   };
 
 } // pose3SLAM

gtsam/slam/tests/testPose2SLAM.cpp

@@ -195,7 +195,7 @@ TEST_UNSAFE(Pose2SLAM, optimize) {
 TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
 
 	// Create a hexagon of poses
-	pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
+	pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(3,1.0);
   Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
 
 	// create a Pose graph with one equality constraint and one measurement
@@ -231,7 +231,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
 TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
 
 	// Create a hexagon of poses
-	pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
+	pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(6,1.0);
   Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
 
 	// create a Pose graph with one equality constraint and one measurement
@@ -366,7 +366,7 @@ TEST_UNSAFE(Pose2Values, pose2Circle )
 	expected.insert(2, Pose2(-1,  0, - M_PI_2));
 	expected.insert(3, Pose2( 0, -1,   0     ));
 
-	pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
+	pose2SLAM::Values actual = pose2SLAM::Values::Circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -381,7 +381,7 @@ TEST_UNSAFE(Pose2SLAM, expmap )
 	expected.insert(3, Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
-	pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
+	pose2SLAM::Values circle = pose2SLAM::Values::Circle(4,1.0);
   Ordering ordering(*circle.orderingArbitrary());
 	VectorValues delta(circle.dims(ordering));
 	delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0);

gtsam/slam/tests/testPose3SLAM.cpp

@@ -43,7 +43,7 @@ TEST(Pose3Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
 	double radius = 10;
-	Values hexagon = pose3SLAM::circle(6,radius);
+	Values hexagon = pose3SLAM::Values::Circle(6,radius);
   Pose3 gT0 = hexagon.at<Pose3>(0), gT1 = hexagon.at<Pose3>(1);
 
 	// create a Pose graph with one equality constraint and one measurement
@@ -179,7 +179,7 @@ TEST( Values, pose3Circle )
 	expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
 	expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
 
-	Values actual = pose3SLAM::circle(4,1.0);
+	Values actual = pose3SLAM::Values::Circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -199,7 +199,7 @@ TEST( Values, expmap )
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0);
-	Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
+	Values actual = pose3SLAM::Values::Circle(4,1.0).retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }