added Matlab convenience functions

gtsam/slam/visualSLAM.cpp

@@ -20,6 +20,36 @@
 
 namespace visualSLAM {
 
+  /* ************************************************************************* */
+  Vector Values::xs() const {
+    size_t j=0;
+    ConstFiltered<Pose3> poses = filter<Pose3>();
+    Vector result(poses.size());
+    BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
+      result(j++) = keyValue.value.x();
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Vector Values::ys() const {
+    size_t j=0;
+    ConstFiltered<Pose3> poses = filter<Pose3>();
+    Vector result(poses.size());
+    BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
+      result(j++) = keyValue.value.y();
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Vector Values::zs() const {
+    size_t j=0;
+    ConstFiltered<Pose3> poses = filter<Pose3>();
+    Vector result(poses.size());
+    BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
+      result(j++) = keyValue.value.z();
+    return result;
+  }
+
   /* ************************************************************************* */
   void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
        Key poseKey, Key pointKey, const shared_ptrK K) {

gtsam/slam/visualSLAM.h

@@ -81,6 +81,10 @@ namespace visualSLAM {
     /// check if value with specified key exists
     bool exists(Key i) const { return gtsam::Values::exists(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
+
   };
 
   /**