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

added Matlab convenience functions

release/4.3a0
Chris Beall 2012-06-05 15:49:15 +00:00
parent fd592dd689
commit f13b3049a8
2 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/slam/visualSLAM.cpp
View File

@ -20,6 +20,36 @@
namespace visualSLAM {
/* ************************************************************************* */
Vector Values::xs() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Vector result(poses.size());
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.x();
return result;
}
/* ************************************************************************* */
Vector Values::ys() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Vector result(poses.size());
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.y();
return result;
}
/* ************************************************************************* */
Vector Values::zs() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Vector result(poses.size());
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.z();
return result;
}
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) {

4
gtsam/slam/visualSLAM.h
View File

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