diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3c5836b5e..afacad508 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace gtsam { @@ -145,6 +146,34 @@ public: /// @} + /// @name Streaming operators + /// @{ + + // stream to stream + friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E) { + Rot3 R = E.rotation(); + Sphere2 d = E.direction(); + os.precision(10); + os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + return os; + } + + /// stream from stream + friend std::istream& operator >>(std::istream& is, EssentialMatrix& E) { + double rx, ry, rz, dx, dy, dz; + is >> rx >> ry >> rz; // Read the rotation rxyz + is >> dx >> dy >> dz; // Read the translation dxyz + + // Create EssentialMatrix from rotation and translation + Rot3 rot = Rot3::RzRyRx(rx, ry, rz); + Sphere2 dt = Sphere2(dx, dy, dz); + E = EssentialMatrix(rot, dt); + + return is; + } + + /// @} + }; } // gtsam diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 1e94f12f3..59cc15581 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -122,32 +122,6 @@ TEST (EssentialMatrix, FromPose3_b) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -ostream& operator <<(ostream& os, const gtsam::EssentialMatrix& E) { - gtsam::Rot3 R = E.rotation(); - gtsam::Sphere2 d = E.direction(); - os.precision(10); - os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; - return os; -} - -/* ************************************************************************* */ -istream& operator >>(istream& fs, gtsam::EssentialMatrix& E) { - double rx, ry, rz, dx, dy, dz; - - // Read the rotation rxyz - fs >> rx >> ry >> rz; - - // Read the translation dxyz - fs >> dx >> dy >> dz; - - // Create EssentialMatrix: Construct from rotation and translation - gtsam::Rot3 rot = gtsam::Rot3::RzRyRx(rx, ry, rz); - gtsam::Sphere2 dt = gtsam::Sphere2(dx, dy, dz); - E = gtsam::EssentialMatrix(rot, dt); - return fs; -} - //************************************************************************* TEST (EssentialMatrix, streaming) { EssentialMatrix expected(c1Rc2, c1Tc2), actual;