From c8c25b16abf10f58d6f1da1f2fa30a9692f9885f Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 1 Jul 2014 11:21:58 -0400
Subject: [PATCH] Added epipoles

---
 gtsam/geometry/EssentialMatrix.h             | 10 ++++++
 gtsam/geometry/tests/testEssentialMatrix.cpp | 33 ++++++++++++++++++--
 2 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index 3c9568c3d..32b966261 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -112,6 +112,16 @@ public:
     return E_;
   }
 
+  /// Return epipole in image_a , as Unit3 to allow for infinity
+  inline const Unit3& epipole_a() const {
+    return aTb_; // == direction()
+  }
+
+  /// Return epipole in image_b, as Unit3 to allow for infinity
+  inline Unit3 epipole_b() const {
+    return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
+  }
+
   /**
    * @brief takes point in world coordinates and transforms it to pose with |t|==1
    * @param p point in world coordinates
diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
index 9ad30bc41..4d34bbe3d 100644
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -144,9 +144,9 @@ TEST (EssentialMatrix, FromPose3_b) {
   Matrix actualH;
   Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
   Point3 c1Tc2(0.4, 0.5, 0.6);
-  EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
+  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
   Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
-  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
       boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
@@ -161,6 +161,35 @@ TEST (EssentialMatrix, streaming) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, epipoles) {
+  // Create an E
+  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Point3 c1Tc2(0.4, 0.5, 0.6);
+  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
+
+  // Calculate expected values through SVD
+  Matrix U, V;
+  Vector S;
+  gtsam::svd(E.matrix(), U, S, V);
+
+  // check rank 2 constraint
+  CHECK(fabs(S(2))<1e-10);
+
+  // check epipoles not at infinity
+  CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10);
+
+  // Check epipoles
+
+  // Epipole in image 1 is just E.direction()
+  Unit3 e1(U(0, 2), U(1, 2), U(2, 2));
+  EXPECT(assert_equal(e1, E.epipole_a()));
+
+  // Epipole in image 2 is E.rotation().unrotate(E.direction())
+  Unit3 e2(V(0, 2), V(1, 2), V(2, 2));
+  EXPECT(assert_equal(e2, E.epipole_b()));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;