12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Sanitized view methods

release/4.3a0
Frank Dellaert 2012-06-24 14:33:02 +00:00
parent ce767762e9
commit 151743a96b
7 changed files with 143 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam.h
View File

@ -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;
};

7
gtsam/nonlinear/Values-inl.h
View File

@ -156,6 +156,13 @@ namespace gtsam {
return i;
}
FastList<Key> keys() const {
FastList<Key> result;
for(const_iterator it = begin(); it != end(); ++it)
result.push_back(it->key);
return result;
}
private:
friend class Values;
const_iterator begin_;

22
gtsam/slam/planarSLAM.h
View File

@ -39,6 +39,10 @@ namespace planarSLAM {
*/
struct Values: public pose2SLAM::Values {
typedef boost::shared_ptr<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<Pose2> PoseFiltered;
typedef gtsam::Values::ConstFiltered<Point2> PointFiltered;
/// Default constructor
Values() {}
@ -47,8 +51,19 @@ namespace planarSLAM {
pose2SLAM::Values(values) {
}
/// get a point
Point2 point(Key j) const { return at<Point2>(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<Pose2>(); } ///< 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<Point2>(); } ///< 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<Point2>(j); }
/// get all [x,y] coordinates in a 2*n matrix
Matrix points() const;
};

30
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -180,6 +180,36 @@ TEST( planarSLAM, constructor )
EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast<NoiseModelFactor>(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<Key> expected, actual;
expected += j3, i2, i3;
actual = c.keys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += i2, i3;
actual = c.poseKeys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += j3;
actual = c.pointKeys();
CHECK(expected == actual);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;

62
gtsam/slam/tests/testVisualSLAM.cpp
View File

@ -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<Key> expected, actual;
expected += L(2), X(1), X(2);
actual = c.keys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += X(1), X(2);
actual = c.poseKeys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += L(2);
actual = c.pointKeys();
CHECK(expected == actual);
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

16
gtsam/slam/visualSLAM.cpp
View File

@ -24,24 +24,10 @@ 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();
}
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point3> points = filter<Point3>();
ConstFiltered<Point3> points = allPoints();
Matrix result(points.size(),3);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();

28
gtsam/slam/visualSLAM.h
View File

@ -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<Pose3>(); } ///< 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<Point3>(); } ///< 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<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>(); }
Matrix points() const; ///< get all point coordinates in a matrix
};