nrPoses, nrPoints

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);

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<Pose3> poses = filter<Pose3>();
+    return poses.size();
+  }
+
+  /* ************************************************************************* */
+  size_t Values::nrPoints() const {
+  	// TODO, is there a better way?
+    ConstFiltered<Point3> points = filter<Point3>();
+    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<Point3> points = filter<Point3>();

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<Pose3>(i); }