diff --git a/examples/matlab/Pose3SLAMExample_graph.m b/examples/matlab/Pose3SLAMExample_graph.m
index a42484484..b1f0afb1a 100644
--- a/examples/matlab/Pose3SLAMExample_graph.m
+++ b/examples/matlab/Pose3SLAMExample_graph.m
@@ -10,19 +10,24 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+N = 2500;
+% filename = '../Data/sphere_smallnoise.graph';
+% filename = '../Data/sphere2500_groundtruth.txt';
+filename = '../Data/sphere2500.txt';
+
 %% 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
+[graph,initial]=load3D(filename,model,true,N);
 first = initial.pose(0);
-graph.addHardConstraint(0, first); % add directly to graph
+graph.addHardConstraint(0, first);
 
 %% Plot Initial Estimate
 figure(1);clf
-plot3(initial.xs(),initial.ys(),initial.zs(),'g-'); hold on
-plot3(first.x(),first.y(),first.z(),'r*'); 
+plot3(first.x(),first.y(),first.z(),'r*'); hold on
+plot3DTrajectory(initial,'g-',false);
 
-%% 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; 
\ No newline at end of file
+%% Read again, now all constraints
+[graph,discard]=load3D(filename,model,false,N);
+graph.addHardConstraint(0, first);
+result = graph.optimize(initial); % start from old result
+plot3DTrajectory(result,'r-',false); axis equal;
diff --git a/examples/matlab/load3D.m b/examples/matlab/load3D.m
index 0ed6ecd61..82ae098b9 100644
--- a/examples/matlab/load3D.m
+++ b/examples/matlab/load3D.m
@@ -1,6 +1,9 @@
-function [graph,initial] = load3D(filename,model,N)
+function [graph,initial] = load3D(filename,model,successive,N)
 % load3D: read TORO 3D pose graph
 % cannot read noise model from file yet, uses specified model
+% if [successive] is tru, constructs initial estimate from odometry
+
+if nargin<3, successive=false; end
 fid = fopen(filename);
 if fid < 0
     error(['load2D: Cannot open file ' filename]);
@@ -14,24 +17,39 @@ lines=columns{1};
 % loop over lines and add vertices
 graph = pose3SLAMGraph;
 initial = pose3SLAMValues;
+origin=gtsamPose3;
+initial.insertPose(0,origin);
 n=size(lines,1);
-if nargin<3, N=n;end
+if nargin<4, 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
+        i=v{2};
+        if (~successive & i<N | successive & i==0)
             t = gtsamPoint3(v{3}, v{4}, v{5});
-            R = gtsamRot3_ypr(v{6}, v{7}, v{8});
-            initial.insertPose(v{2}, gtsamPose3(R,t));
+            R = gtsamRot3_ypr(v{8}, -v{7}, v{6});
+            initial.insertPose(i, 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);
+        i1=e{2};
+        i2=e{3};
+        if i1<N & i2<N
+            if ~successive | abs(i2-i1)==1
+                t = gtsamPoint3(e{4}, e{5}, e{6});
+                R = gtsamRot3_ypr(e{9}, e{8}, e{7});
+                dpose = gtsamPose3(R,t);
+                graph.addConstraint(i1, i2, dpose, model);
+                if successive
+                    if i2>i1
+                        initial.insertPose(i2,initial.pose(i1).compose(dpose));
+                    else
+                        initial.insertPose(i1,initial.pose(i2).compose(dpose.inverse));
+                    end
+                end
+            end
         end
     end
 end
diff --git a/examples/matlab/plot3DTrajectory.m b/examples/matlab/plot3DTrajectory.m
new file mode 100644
index 000000000..f2c1d8f2f
--- /dev/null
+++ b/examples/matlab/plot3DTrajectory.m
@@ -0,0 +1,15 @@
+function plot3DTrajectory(values,style,frames)
+% plot3DTrajectory
+if nargin<3,frames=false;end
+
+plot3(values.xs(),values.ys(),values.zs(),style); hold on
+if frames
+    for i=0:N-1
+        pose = values.pose(i);
+        t = pose.translation;
+        R = pose.rotation.matrix;
+        quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),'r');
+        quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),'g');
+        quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),'b');
+    end
+end
\ No newline at end of file
diff --git a/gtsam.h b/gtsam.h
index 48a205e2e..2015705ae 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -197,26 +197,51 @@ class Pose2 {
 };
 
 class Pose3 {
+	// Standard Constructors
 	Pose3();
+	Pose3(const gtsam::Pose3& pose);
 	Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
-	Pose3(Matrix t);
 	Pose3(const gtsam::Pose2& pose2);
-	static gtsam::Pose3 Expmap(Vector v);
-	static Vector Logmap(const gtsam::Pose3& p);
+	Pose3(Matrix t);
+
+	// Testable
 	void print(string s) const;
 	bool equals(const gtsam::Pose3& pose, double tol) const;
+
+	// Group
+	static gtsam::Pose3 identity();
+	gtsam::Pose3 inverse();
+	gtsam::Pose3 compose(const gtsam::Pose3& p2);
+	gtsam::Pose3 between(const gtsam::Pose3& p2);
+
+	// Manifold
+	static size_t Dim();
+	size_t dim() const;
+	gtsam::Pose3 retract(Vector v);
+	gtsam::Pose3 retractFirstOrder(Vector v);
+	Vector localCoordinates(const gtsam::Pose3& T2) const;
+
+	// Lie Group
+	static gtsam::Pose3 Expmap(Vector v);
+	static Vector Logmap(const gtsam::Pose3& p);
+	Matrix adjointMap() const;
+	Vector adjoint(const Vector& xi) const;
+	static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
+
+	// Group Action on Point3
+	gtsam::Point3 transform_from(const gtsam::Point3& p) const;
+	gtsam::Point3 transform_to(const gtsam::Point3& p) const;
+
+	// Standard Interface
+	gtsam::Rot3 rotation() const;
+	gtsam::Point3 translation() const;
 	double x() const;
 	double y() const;
 	double z() const;
 	Matrix matrix() const;
-	Matrix adjointMap() const;
-	gtsam::Pose3 compose(const gtsam::Pose3& p2);
-	gtsam::Pose3 between(const gtsam::Pose3& p2);
-	gtsam::Pose3 retract(Vector v);
-	gtsam::Pose3 retractFirstOrder(Vector v);
-	Vector localCoordinates(const gtsam::Pose3& T2) const;
-	gtsam::Point3 translation() const;
-	gtsam::Rot3 rotation() const;
+	gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const;
+	double range(const gtsam::Point3& point);
+	// double range(const gtsam::Pose3& pose);
 };
 
 class Cal3_S2 {