diff --git a/gtsam.h b/gtsam.h index b4fac0eee..9609adb03 100644 --- a/gtsam.h +++ b/gtsam.h @@ -985,6 +985,9 @@ class Values { Values(); size_t size() const; void print(string s) const; + bool exists(size_t key); + gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList + static pose2SLAM::Values Circle(size_t n, double R); void insertPose(size_t key, const gtsam::Pose2& pose); void updatePose(size_t key, const gtsam::Pose2& pose); @@ -1033,6 +1036,8 @@ class Values { Values(); size_t size() const; void print(string s) const; + bool exists(size_t key); + gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList static pose3SLAM::Values Circle(size_t n, double R); void insertPose(size_t key, const gtsam::Pose3& pose); @@ -1082,13 +1087,26 @@ class Values { Values(); size_t size() const; void print(string s) const; + bool exists(size_t key); + gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - static planarSLAM::Values Circle(size_t n, double R); + // inherited from pose2SLAM + static planarSLAM::Values Circle(size_t n, double R); void insertPose(size_t key, const gtsam::Pose2& pose); void updatePose(size_t key, const gtsam::Pose2& pose); gtsam::Pose2 pose(size_t i); Matrix poses() const; + // Access to poses + planarSLAM::Values allPoses() const; + size_t nrPoses() const; + gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList + + // Access to points + planarSLAM::Values allPoints() const; + size_t nrPoints() const; + gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList + void insertPoint(size_t key, const gtsam::Point2& point); void updatePoint(size_t key, const gtsam::Point2& point); gtsam::Point2 point(size_t key) const; @@ -1161,14 +1179,19 @@ class Values { gtsam::Pose3 pose(size_t i); Matrix translations() const; - // visualSLAM specific + // Access to poses + visualSLAM::Values allPoses() const; + size_t nrPoses() const; + gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList + + // Access to points + visualSLAM::Values allPoints() const; + size_t nrPoints() const; + gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList + void insertPoint(size_t key, const gtsam::Point3& pose); void updatePoint(size_t key, const gtsam::Point3& pose); - size_t nrPoses() const; - size_t nrPoints() const; gtsam::Point3 point(size_t j); - visualSLAM::Values allPoses() const; - visualSLAM::Values allPoints() const; Matrix points() const; }; diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index c38f14177..3aec3e7b2 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -156,6 +156,13 @@ namespace gtsam { return i; } + FastList keys() const { + FastList result; + for(const_iterator it = begin(); it != end(); ++it) + result.push_back(it->key); + return result; + } + private: friend class Values; const_iterator begin_; diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index aeb38794e..3ab841892 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -39,6 +39,10 @@ namespace planarSLAM { */ struct Values: public pose2SLAM::Values { + typedef boost::shared_ptr shared_ptr; + typedef gtsam::Values::ConstFiltered PoseFiltered; + typedef gtsam::Values::ConstFiltered PointFiltered; + /// Default constructor Values() {} @@ -47,8 +51,19 @@ namespace planarSLAM { pose2SLAM::Values(values) { } - /// get a point - Point2 point(Key j) const { return at(j); } + /// Constructor from filtered values view of poses + Values(const PoseFiltered& view) : pose2SLAM::Values(view) {} + + /// Constructor from filtered values view of points + Values(const PointFiltered& view) : pose2SLAM::Values(view) {} + + PoseFiltered allPoses() const { return this->filter(); } ///< pose view + size_t nrPoses() const { return allPoses().size(); } ///< get number of poses + KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only + + PointFiltered allPoints() const { return this->filter(); } ///< point view + size_t nrPoints() const { return allPoints().size(); } ///< get number of points + KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only /// insert a point void insertPoint(Key j, const Point2& point) { insert(j, point); } @@ -56,6 +71,9 @@ namespace planarSLAM { /// update a point void updatePoint(Key j, const Point2& point) { update(j, point); } + /// get a point + Point2 point(Key j) const { return at(j); } + /// get all [x,y] coordinates in a 2*n matrix Matrix points() const; }; diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index cbe33e1da..5652b71b4 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -180,6 +180,36 @@ TEST( planarSLAM, constructor ) EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast(G[3])->unwhitenedError(c))); } +/* ************************************************************************* */ +TEST( planarSLAM, keys_and_view ) +{ + // create config + planarSLAM::Values c; + c.insert(i2, x2); + c.insert(i3, x3); + c.insert(j3, l3); + LONGS_EQUAL(2,c.nrPoses()); + LONGS_EQUAL(1,c.nrPoints()); + { + FastList expected, actual; + expected += j3, i2, i3; + actual = c.keys(); + CHECK(expected == actual); + } + { + FastList expected, actual; + expected += i2, i3; + actual = c.poseKeys(); + CHECK(expected == actual); + } + { + FastList expected, actual; + expected += j3; + actual = c.pointKeys(); + CHECK(expected == actual); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index 081142038..631ecc365 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/gtsam/slam/tests/testVisualSLAM.cpp @@ -73,10 +73,10 @@ visualSLAM::Graph testGraph() { g.addMeasurement(z12, sigma, X(1), L(2), sK); g.addMeasurement(z13, sigma, X(1), L(3), sK); g.addMeasurement(z14, sigma, X(1), L(4), sK); - g.addMeasurement(z21, sigma, X(2), L(1), sK); - g.addMeasurement(z22, sigma, X(2), L(2), sK); - g.addMeasurement(z23, sigma, X(2), L(3), sK); - g.addMeasurement(z24, sigma, X(2), L(4), sK); + g.addMeasurement(z21, sigma, X(1), L(1), sK); + g.addMeasurement(z22, sigma, X(1), L(2), sK); + g.addMeasurement(z23, sigma, X(1), L(3), sK); + g.addMeasurement(z24, sigma, X(1), L(4), sK); return g; } @@ -93,7 +93,7 @@ TEST( VisualSLAM, optimizeLM) // Create an initial values structure corresponding to the ground truth Values initialEstimate; initialEstimate.insert(X(1), camera1); - initialEstimate.insert(X(2), camera2); + initialEstimate.insert(X(1), camera2); initialEstimate.insert(L(1), landmark1); initialEstimate.insert(L(2), landmark2); initialEstimate.insert(L(3), landmark3); @@ -101,7 +101,7 @@ TEST( VisualSLAM, optimizeLM) // Create an ordering of the variables Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); + ordering += L(1),L(2),L(3),L(4),X(1),X(1); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -125,12 +125,12 @@ TEST( VisualSLAM, optimizeLM2) visualSLAM::Graph graph(testGraph()); // add 2 camera constraints graph.addPoseConstraint(X(1), camera1); - graph.addPoseConstraint(X(2), camera2); + graph.addPoseConstraint(X(1), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; initialEstimate.insert(X(1), camera1); - initialEstimate.insert(X(2), camera2); + initialEstimate.insert(X(1), camera2); initialEstimate.insert(L(1), landmark1); initialEstimate.insert(L(2), landmark2); initialEstimate.insert(L(3), landmark3); @@ -138,7 +138,7 @@ TEST( VisualSLAM, optimizeLM2) // Create an ordering of the variables Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); + ordering += L(1),L(2),L(3),L(4),X(1),X(1); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -161,12 +161,12 @@ TEST( VisualSLAM, LMoptimizer) visualSLAM::Graph graph(testGraph()); // add 2 camera constraints graph.addPoseConstraint(X(1), camera1); - graph.addPoseConstraint(X(2), camera2); + graph.addPoseConstraint(X(1), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; initialEstimate.insert(X(1), camera1); - initialEstimate.insert(X(2), camera2); + initialEstimate.insert(X(1), camera2); initialEstimate.insert(L(1), landmark1); initialEstimate.insert(L(2), landmark2); initialEstimate.insert(L(3), landmark3); @@ -194,12 +194,12 @@ TEST( VisualSLAM, CHECK_ORDERING) visualSLAM::Graph graph = testGraph(); // add 2 camera constraints graph.addPoseConstraint(X(1), camera1); - graph.addPoseConstraint(X(2), camera2); + graph.addPoseConstraint(X(1), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; initialEstimate.insert(X(1), camera1); - initialEstimate.insert(X(2), camera2); + initialEstimate.insert(X(1), camera2); initialEstimate.insert(L(1), landmark1); initialEstimate.insert(L(2), landmark2); initialEstimate.insert(L(3), landmark3); @@ -233,12 +233,12 @@ TEST( VisualSLAM, update_with_large_delta) { Ordering largeOrdering; Values largeValues = init; - largeValues.insert(X(2), Pose3()); - largeOrdering += X(1),L(1),X(2); + largeValues.insert(X(1), Pose3()); + largeOrdering += X(1),L(1),X(1); VectorValues delta(largeValues.dims(largeOrdering)); delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering[X(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); + delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); Values actual = init.retract(delta, largeOrdering); CHECK(assert_equal(expected,actual)); @@ -269,6 +269,36 @@ TEST( VisualSLAM, filteredValues) { EXPECT(assert_equal(expPoints, actPoints)); } +/* ************************************************************************* */ +TEST( VisualSLAM, keys_and_view ) +{ + // create config + visualSLAM::Values c; + c.insert(X(1), camera1); + c.insert(X(2), camera2); + c.insert(L(2), landmark2); + LONGS_EQUAL(2,c.nrPoses()); + LONGS_EQUAL(1,c.nrPoints()); + { + FastList expected, actual; + expected += L(2), X(1), X(2); + actual = c.keys(); + CHECK(expected == actual); + } + { + FastList expected, actual; + expected += X(1), X(2); + actual = c.poseKeys(); + CHECK(expected == actual); + } + { + FastList expected, actual; + expected += L(2); + actual = c.pointKeys(); + CHECK(expected == actual); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 793051370..6f31c345c 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -24,24 +24,10 @@ 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(); - } - /* ************************************************************************* */ Matrix Values::points() const { size_t j=0; - ConstFiltered points = filter(); + ConstFiltered points = allPoints(); Matrix result(points.size(),3); BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) result.row(j++) = keyValue.value.vector(); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index dce603a03..da87622fd 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -47,14 +47,18 @@ namespace visualSLAM { } /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : - pose3SLAM::Values(view) { - } + Values(const PoseFiltered& view) : pose3SLAM::Values(view) {} /// Constructor from filtered values view of points - Values(const PointFiltered& view) : - pose3SLAM::Values(view) { - } + Values(const PointFiltered& view) : pose3SLAM::Values(view) {} + + PoseFiltered allPoses() const { return this->filter(); } ///< pose view + size_t nrPoses() const { return allPoses().size(); } ///< get number of poses + KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only + + PointFiltered allPoints() const { return this->filter(); } ///< point view + size_t nrPoints() const { return allPoints().size(); } ///< get number of points + KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only /// insert a point void insertPoint(Key j, const Point3& point) { insert(j, point); } @@ -62,21 +66,9 @@ 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 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(); } - Matrix points() const; ///< get all point coordinates in a matrix };