From e01a320efae44cd07b99821975e8af2d35382512 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Tue, 5 Jun 2012 16:24:04 +0000
Subject: [PATCH] Matlab convenience function for plotting points

---
 gtsam/slam/visualSLAM.cpp | 9 +++++++++
 gtsam/slam/visualSLAM.h   | 8 +++++---
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp
index be5a1c1ca..b8721b875 100644
--- a/gtsam/slam/visualSLAM.cpp
+++ b/gtsam/slam/visualSLAM.cpp
@@ -50,6 +50,15 @@ namespace visualSLAM {
     return result;
   }
 
+  Matrix Values::points() const {
+    size_t j=0;
+    ConstFiltered<Point3> points = filter<Point3>();
+    Matrix result(points.size(),3);
+    BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points)
+      result.row(j++) = keyValue.value.vector();
+    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 9f9224c9d..b0e034653 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -81,9 +81,11 @@ 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
+    Vector xs() const; ///< get all pose x coordinates in a matrix
+    Vector ys() const; ///< get all pose y coordinates in a matrix
+    Vector zs() const; ///< get all pose z coordinates in a matrix
+
+    Matrix points() const; ///< get all point coordinates in a matrix
 
   };