diff --git a/gtsam.h b/gtsam.h index 315071f65..ff9bb2022 100644 --- a/gtsam.h +++ b/gtsam.h @@ -891,6 +891,8 @@ class Values { void updatePose(size_t key, const gtsam::Pose3& pose); void updatePoint(size_t key, const gtsam::Point3& pose); size_t size() const; + size_t nrPoses() const; + size_t nrPoints() const; void print(string s) const; gtsam::Pose3 pose(size_t i); gtsam::Point3 point(size_t j); diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index bb3cd19d1..26123dda6 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -23,6 +23,20 @@ using boost::make_shared; namespace visualSLAM { + /* ************************************************************************* */ + size_t Values::nrPoses() const { + // TODO, is there a better way? + ConstFiltered poses = filter(); + return poses.size(); + } + + /* ************************************************************************* */ + size_t Values::nrPoints() const { + // TODO, is there a better way? + ConstFiltered points = filter(); + return points.size(); + } + /* ************************************************************************* */ Vector Values::xs() const { size_t j=0; @@ -53,6 +67,7 @@ namespace visualSLAM { return result; } + /* ************************************************************************* */ Matrix Values::points() const { size_t j=0; ConstFiltered points = filter(); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index f1f5c3850..5f2d70905 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -72,6 +72,12 @@ namespace visualSLAM { /// update a point void updatePoint(Key j, const Point3& point) { update(j, point); } + /// get number of poses + size_t nrPoses() const; + + /// get number of points + size_t nrPoints() const; + /// get a pose Pose3 pose(Key i) const { return at(i); }