diff --git a/gtsam.h b/gtsam.h index ee7f5dd01..f1720c466 100644 --- a/gtsam.h +++ b/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 +#include +class KeyList { + KeyList(); + KeyList(const gtsam::KeyList& other); + + // Note: no print function + size_t size() const; +}; + +// Actually a KeyVector +#include +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; diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index ee7258ba8..e91bd1813 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -23,6 +23,8 @@ #include #include +#include + namespace gtsam { /** @@ -58,9 +60,15 @@ public: Base::assign(first, last); } - /** Copy constructor from another FastSet */ + /** Copy constructor from another FastVector */ FastVector(const FastVector& x) : Base(x) {} + /** Copy constructor from a FastList */ + FastVector(const FastList& x) { + if(x.size() > 0) + Base::assign(x.begin(), x.end()); + } + /** Copy constructor from the base class */ FastVector(const Base& x) : Base(x) {} diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 8b8fbc71c..8ade8ff06 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -43,12 +44,16 @@ namespace gtsam { + // Useful typedefs for operations with Values - allow for matlab interfaces + typedef FastList KeyList; + typedef FastVector KeyVector; + // Forward declarations / utilities class ValueCloneAllocator; class ValueAutomaticCasting; template 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 keys() const; + KeyList keys() const; /** Replace all keys and variables */ Values& operator=(const Values& rhs); diff --git a/matlab/VisualISAMPlot.m b/matlab/VisualISAMPlot.m index b23b3a475..90c542662 100644 --- a/matlab/VisualISAMPlot.m +++ b/matlab/VisualISAMPlot.m @@ -10,15 +10,13 @@ cla(h); hold on; %% Plot points -for j=1:N - %% TODO: use the actual set of keys present - jj = symbol('l',j); - if result.exists(jj) - point_j = result.point(jj); - plot3(point_j.x, point_j.y, point_j.z,'marker','o'); - P = isam.marginalCovariance(jj); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); - end +pointKeys = result.allPoints().keys(); +for j=0:N-1 % NOTE: uses indexing directly from a C++ vector, so zero-indexed + jj = pointKeys.at(j); + point_j = result.point(jj); + plot3(point_j.x, point_j.y, point_j.z,'marker','o'); + P = isam.marginalCovariance(jj); + covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); end %% Plot cameras diff --git a/matlab/symbolChr.m b/matlab/symbolChr.m new file mode 100644 index 000000000..39c92ec85 --- /dev/null +++ b/matlab/symbolChr.m @@ -0,0 +1,4 @@ +function c = symbolChr(key) +% generate the chr from a key +s = gtsamSymbol(key); +c = s.chr(); \ No newline at end of file diff --git a/matlab/symbolIndex.m b/matlab/symbolIndex.m new file mode 100644 index 000000000..3046cf229 --- /dev/null +++ b/matlab/symbolIndex.m @@ -0,0 +1,4 @@ +function i = symbolIndex(key) +% generate the index from a key +s = gtsamSymbol(key); +i = s.index(); \ No newline at end of file