From f13b3049a84fef401cfd58e6cd18ee0548724d86 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Tue, 5 Jun 2012 15:49:15 +0000
Subject: [PATCH] added Matlab convenience functions

---
 gtsam/slam/visualSLAM.cpp | 30 ++++++++++++++++++++++++++++++
 gtsam/slam/visualSLAM.h   |  4 ++++
 2 files changed, 34 insertions(+)

diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp
index f4cdb0af3..be5a1c1ca 100644
--- a/gtsam/slam/visualSLAM.cpp
+++ b/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) {
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index 96a1e037c..9f9224c9d 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/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
+
   };
 
   /**