Added matlab interface to get indices out of Values structures. Added more matlab symbol functions.

gtsam.h

@@ -643,8 +643,11 @@ class KalmanFilter {
 
 class Symbol {
 	Symbol(char c, size_t j);
+	Symbol(size_t k);
 	void print(string s) const;
 	size_t key() const;
+	size_t index() const;
+	char chr() const;
 };
 
 class Ordering {
@@ -665,6 +668,27 @@ class Values {
 	bool exists(size_t j) const;
 };
 
+// Actually a FastList<Key>
+#include <gtsam/nonlinear/Values.h>
+class KeyList {
+	KeyList();
+	KeyList(const gtsam::KeyList& other);
+
+	// Note: no print function
+	size_t size() const;
+};
+
+// Actually a KeyVector<Key>
+#include <gtsam/nonlinear/Values.h>
+class KeyVector {
+	KeyVector();
+	KeyVector(const gtsam::KeyVector& other);
+
+	// Note: no print function
+	size_t size() const;
+	size_t at(size_t i) const;
+};
+
 class Marginals {
 	Marginals(const gtsam::NonlinearFactorGraph& graph,
 			const gtsam::Values& solution);
@@ -899,6 +923,7 @@ class Values {
   gtsam::Point3 point(size_t j);
   visualSLAM::Values allPoses() const;
   visualSLAM::Values allPoints() const;
+  gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList
   bool exists(size_t key);
   Vector xs() const;
   Vector ys() const;

gtsam/base/FastVector.h

@@ -23,6 +23,8 @@
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
 
+#include <gtsam/base/FastList.h>
+
 namespace gtsam {
 
 /**
@@ -58,9 +60,15 @@ public:
       Base::assign(first, last);
   }
 
-  /** Copy constructor from another FastSet */
+  /** Copy constructor from another FastVector */
   FastVector(const FastVector<VALUE>& x) : Base(x) {}
 
+  /** Copy constructor from a FastList */
+  FastVector(const FastList<VALUE>& x) {
+    if(x.size() > 0)
+      Base::assign(x.begin(), x.end());
+  }
+
   /** Copy constructor from the base class */
   FastVector(const Base& x) : Base(x) {}
 

gtsam/nonlinear/Values.h

@@ -26,6 +26,7 @@
 
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/FastVector.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/Ordering.h>
 
@@ -43,12 +44,16 @@
 
 namespace gtsam {
 
+	// Useful typedefs for operations with Values - allow for matlab interfaces
+	typedef FastList<Key> KeyList;
+	typedef FastVector<Key> KeyVector;
+
   // Forward declarations / utilities
   class ValueCloneAllocator;
   class ValueAutomaticCasting;
   template<typename T> static bool _truePredicate(const T&) { return true; }
 
-/**
+  /**
   * A non-templated config holding any types of Manifold-group elements.  A
   * values structure is a map from keys to values. It is used to specify the
   * value of a bunch of variables in a factor graph. A Values is a values
@@ -238,7 +243,7 @@ namespace gtsam {
      * Returns a set of keys in the config
      * Note: by construction, the list is ordered
      */
-    FastList<Key> keys() const;
+    KeyList keys() const;
 
     /** Replace all keys and variables */
     Values& operator=(const Values& rhs);

matlab/VisualISAMPlot.m

@@ -10,15 +10,13 @@ cla(h);
 hold on;
 
 %% Plot points
-for j=1:N
+pointKeys = result.allPoints().keys();
-    %% TODO: use the actual set of keys present
+for j=0:N-1  % NOTE: uses indexing directly from a C++ vector, so zero-indexed
-    jj = symbol('l',j);
+    jj = pointKeys.at(j);
-    if result.exists(jj)
+    point_j = result.point(jj);
-        point_j = result.point(jj);
+    plot3(point_j.x, point_j.y, point_j.z,'marker','o');
-        plot3(point_j.x, point_j.y, point_j.z,'marker','o');
+    P = isam.marginalCovariance(jj);
-        P = isam.marginalCovariance(jj);
+    covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
-        covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
-    end
 end
 
 %% Plot cameras

matlab/symbolChr.m

@@ -0,0 +1,4 @@
+function c = symbolChr(key)
+% generate the chr from a key 
+s = gtsamSymbol(key);
+c = s.chr();

matlab/symbolIndex.m

@@ -0,0 +1,4 @@
+function i = symbolIndex(key)
+% generate the index from a key 
+s = gtsamSymbol(key);
+i = s.index();