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

Cleaned up printing of NavState

release/4.3a0
Varun Agrawal 2020-06-18 11:10:44 -05:00
parent 7815a27e21
commit d3ac33ac18
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/navigation/NavState.cpp
View File

@ -88,15 +88,15 @@ Matrix7 NavState::matrix() const {
//------------------------------------------------------------------------------
ostream& operator<<(ostream& os, const NavState& state) {
os << "R:" << state.attitude();
os << "p:" << state.position() << endl;
os << "v:" << Point3(state.velocity()) << endl;
os << "R: " << state.attitude() << "\n";
os << "p: " << state.position() << "\n";
os << "v: " << Point3(state.velocity());
return os;
}
//------------------------------------------------------------------------------
void NavState::print(const string& s) const {
cout << s << *this << endl;
cout << (s.empty() ? s : s + " ") << *this << endl;
}
//------------------------------------------------------------------------------

12
gtsam/navigation/tests/testNavState.cpp
View File

@ -237,6 +237,18 @@ TEST(NavState, CorrectPIM) {
EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
}
/* ************************************************************************* */
TEST(NavState, Stream)
{
NavState state;
std::ostringstream os;
os << state;
string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: [0, 0, 0]'\nv: [0, 0, 0]'";
EXPECT(os.str() == expected);
}
/* ************************************************************************* */
int main() {
TestResult tr;