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

Matlab convenience function for plotting points

release/4.3a0
Chris Beall 2012-06-05 16:24:04 +00:00
parent f13b3049a8
commit e01a320efa
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/slam/visualSLAM.cpp
View File

@ -50,6 +50,15 @@ namespace visualSLAM {
return result; return result;
} }
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point3> points = filter<Point3>();
Matrix result(points.size(),3);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();
return result;
}
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) { Key poseKey, Key pointKey, const shared_ptrK K) {

8
gtsam/slam/visualSLAM.h
View File

@ -81,9 +81,11 @@ namespace visualSLAM {
/// check if value with specified key exists /// check if value with specified key exists
bool exists(Key i) const { return gtsam::Values::exists(i); } bool exists(Key i) const { return gtsam::Values::exists(i); }
Vector xs() const; ///< get all x coordinates in a matrix Vector xs() const; ///< get all pose x coordinates in a matrix
Vector ys() const; ///< get all y coordinates in a matrix Vector ys() const; ///< get all pose y coordinates in a matrix
Vector zs() const; ///< get all z coordinates in a matrix Vector zs() const; ///< get all pose z coordinates in a matrix
Matrix points() const; ///< get all point coordinates in a matrix
}; };