From 87c386d77fd67f6b6b4f18173513ae587167162a Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 25 May 2014 02:03:33 -0400
Subject: [PATCH] Fast creation of large key sets from within MATLAB

---
 gtsam.h                  | 1 +
 matlab.h                 | 8 ++++++++
 matlab/+gtsam/Contents.m | 1 +
 3 files changed, 10 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 2d181d350..71fcaeb17 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2375,6 +2375,7 @@ namespace utilities {
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
+  gtsam::KeySet createKeySet(string s, Vector I);
 
 } //\namespace utilities
 
diff --git a/matlab.h b/matlab.h
index 100f5a242..261fd2d48 100644
--- a/matlab.h
+++ b/matlab.h
@@ -151,6 +151,14 @@ namespace gtsam {
       return errors;
     }
 
+    // Create a Keyset from indices
+    FastSet<Key> createKeySet(string s, const Vector& I) {
+      FastSet<Key> set;
+      char c = s[0];
+      for(int i=0;i<I.size();i++)
+        set.insert(symbol(c,I[i]));
+      return set;
+    }
   }
 
 }
diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m
index 5fec7721c..ac6842e8e 100644
--- a/matlab/+gtsam/Contents.m
+++ b/matlab/+gtsam/Contents.m
@@ -186,3 +186,4 @@
 %   utilities.insertBackprojections   - Insert a number of initial point values by backprojecting
 %   utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key
 %   utilities.reprojectionErrors      - Calculate the errors of all projection factors in a graph
+%   utilities.createKeySet            - Create keys from indices