Adding filtering interfaces for Values to isoloate points and poses

gtsam.h

@@ -897,6 +897,8 @@ class Values {
   void print(string s) const;
   gtsam::Pose3 pose(size_t i);
   gtsam::Point3 point(size_t j);
+  visualSLAM::Values allPoses() const;
+  visualSLAM::Values allPoints() const;
   bool exists(size_t key);
   Vector xs() const;
   Vector ys() const;

gtsam/slam/tests/testVisualSLAM.cpp

@@ -222,6 +222,21 @@ TEST( VisualSLAM, dataAssociation) {
   //  std::pair<visualSLAM::Values,GaussianBayesNet> actualChowLiu = isam.chowLiuPredictions(); // 2 times 2*2
 }
 
+/* ************************************************************************* */
+TEST( VisualSLAM, filteredValues) {
+	visualSLAM::Values full;
+	full.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3)));
+	full.insert(L(1), Point3(1.0, 2.0, 3.0));
+
+	visualSLAM::Values actPoses(full.allPoses());
+	visualSLAM::Values expPoses; expPoses.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3)));
+  EXPECT(assert_equal(expPoses, actPoses));
+
+  visualSLAM::Values actPoints(full.allPoints());
+  visualSLAM::Values expPoints; expPoints.insert(L(1), Point3(1.0, 2.0, 3.0));
+  EXPECT(assert_equal(expPoints, actPoints));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/visualSLAM.h

@@ -51,6 +51,8 @@ namespace visualSLAM {
   struct Values: public gtsam::Values {
 
     typedef boost::shared_ptr<Values> shared_ptr;
+    typedef gtsam::Values::ConstFiltered<Pose3> PoseFiltered;
+    typedef gtsam::Values::ConstFiltered<Point3> PointFiltered;
 
     /// Default constructor
     Values() {}
@@ -60,6 +62,16 @@ namespace visualSLAM {
         gtsam::Values(values) {
     }
 
+    /// Constructor from filtered values view of poses
+    Values(const PoseFiltered& view) :
+    		gtsam::Values(view) {
+    }
+
+    /// Constructor from filtered values view of points
+    Values(const PointFiltered& view) :
+    	gtsam::Values(view) {
+    }
+
     /// insert a pose
     void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
 
@@ -84,6 +96,12 @@ namespace visualSLAM {
     /// get a point
     Point3 point(Key j) const { return at<Point3>(j); }
 
+    /// get a const view containing only poses
+    PoseFiltered allPoses() const { return this->filter<Pose3>(); }
+
+    /// get a const view containing only points
+    PointFiltered allPoints() const { return this->filter<Point3>(); }
+
     /// check if value with specified key exists
     bool exists(Key i) const { return gtsam::Values::exists(i); }