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 createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for(int i=0;i