diff --git a/gtsam.h b/gtsam.h index e728de125..ee7f5dd01 100644 --- a/gtsam.h +++ b/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; diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index d01d402ee..d9abc67ed 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/gtsam/slam/tests/testVisualSLAM.cpp @@ -222,6 +222,21 @@ TEST( VisualSLAM, dataAssociation) { // std::pair 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); } /* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 5f2d70905..d577ee527 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -51,6 +51,8 @@ namespace visualSLAM { struct Values: public gtsam::Values { typedef boost::shared_ptr shared_ptr; + typedef gtsam::Values::ConstFiltered PoseFiltered; + typedef gtsam::Values::ConstFiltered 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(j); } + /// get a const view containing only poses + PoseFiltered allPoses() const { return this->filter(); } + + /// get a const view containing only points + PointFiltered allPoints() const { return this->filter(); } + /// check if value with specified key exists bool exists(Key i) const { return gtsam::Values::exists(i); }