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

ostream operator

release/4.3a0
Frank Dellaert 2016-01-26 20:42:48 -08:00
parent ae867e8d6e
commit 3a891b2a2c
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/NavState.cpp
View File

@ -74,11 +74,17 @@ Matrix7 NavState::matrix() const {
return T;
}
//------------------------------------------------------------------------------
ostream& operator<<(ostream& os, const NavState& state) {
os << "R:" << state.attitude();
os << "p:" << state.position() << endl;
os << "v:" << Point3(state.velocity()) << endl;
return os;
}
//------------------------------------------------------------------------------
void NavState::print(const string& s) const {
attitude().print(s + ".R");
position().print(s + ".p");
gtsam::print((Vector) v_, s + ".v");
cout << s << *this << endl;
}
//------------------------------------------------------------------------------

5
gtsam/navigation/NavState.h
View File

@ -124,6 +124,9 @@ public:
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state);
/// print
void print(const std::string& s = "") const;
@ -229,6 +232,8 @@ public:
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
/// @}
private:
/// @{
/// serialization