diff --git a/matlab.h b/matlab.h
index dfc0117e7..17f96bcd6 100644
--- a/matlab.h
+++ b/matlab.h
@@ -45,9 +45,9 @@ namespace gtsam {
 
     /// Extract all Point3 values into a single matrix [x y z]
     Matrix extractPoint3(const Values& values) {
-      size_t j=0;
       Values::ConstFiltered<Point3> points = values.filter<Point3>();
       Matrix result(points.size(),3);
+      size_t j=0;
       BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points)
         result.row(j++) = key_value.value.vector();
       return result;
@@ -55,19 +55,24 @@ namespace gtsam {
 
     /// Extract all Pose2 values into a single matrix [x y theta]
     Matrix extractPose2(const Values& values) {
-      size_t j=0;
       Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
       Matrix result(poses.size(),3);
+      size_t j=0;
       BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses)
         result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
       return result;
     }
 
+    /// Extract all Pose3 values
+    Values allPose3s(const Values& values) {
+      return values.filter<Pose3>();
+    }
+
     /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z]
     Matrix extractPose3(const Values& values) {
-      size_t j=0;
       Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
       Matrix result(poses.size(),12);
+      size_t j=0;
       BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) {
         result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0);
         result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1);
@@ -78,7 +83,6 @@ namespace gtsam {
       return result;
     }
 
-
     /// perturb all Point2 using normally distributed noise
     void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
       noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma);