Streaming implemented with test

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -10,6 +10,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
 #include <CppUnitLite/TestHarness.h>
+#include <sstream>
 
 using namespace std;
 using namespace gtsam;
@@ -121,6 +122,41 @@ 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;
+  stringstream ss;
+  ss << expected;
+  ss >> actual;
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;