transform_to, but with bug in Jacobains...

gtsam/geometry/EssentialMatrix.h

@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Sphere2.h>
 #include <gtsam/geometry/Point2.h>
 #include <iostream>
@@ -56,7 +56,7 @@ public:
   }
 
   /// assert equality up to a tolerance
-  bool equals(const EssentialMatrix& other, double tol=1e-8) const {
+  bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
     return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
   }
 
@@ -110,6 +110,32 @@ public:
     return E_;
   }
 
+  /**
+   * @brief takes point in world coordinates and transforms it to pose with |t|==1
+   * @param p point in world coordinates
+   * @param DE optional 3*5 Jacobian wrpt to E
+   * @param Dpoint optional 3*3 Jacobian wrpt point
+   * @return point in pose coordinates
+   */
+  Point3 transform_to(const Point3& p,
+      boost::optional<Matrix&> DE = boost::none,
+      boost::optional<Matrix&> Dpoint = boost::none) const {
+    Pose3 pose(aRb_, aTb_.point3());
+    Point3 q = pose.transform_to(p, DE, Dpoint);
+    if (DE) {
+      // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
+      // The last 3 columns are derivative with respect to change in translation
+      // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
+      Matrix H(3, 5);
+      std::cout << *DE << std::endl << std::endl;
+      std::cout << aTb_.basis() << std::endl << std::endl;
+      H << DE->block < 3, 3 > (0, 0), DE->block < 3, 3 > (0, 3) * aTb_.basis();
+      std::cout << H << std::endl << std::endl;
+      *DE = H;
+    }
+    return q;
+  }
+
   /// epipolar error, algebraic
   double error(const Vector& vA, const Vector& vB, //
       boost::optional<Matrix&> H = boost::none) const {

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -7,8 +7,8 @@
 
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
-
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -44,6 +44,20 @@ TEST (EssentialMatrix, retract2) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); }
+TEST (EssentialMatrix, transform_to) {
+  EssentialMatrix E(aRb, aTb);
+  //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
+  static Point3 P(0.2,0.7,-2);
+  Matrix actH1, actH2;
+  E.transform_to(P,actH1,actH2);
+  Matrix expH1 = numericalDerivative21(transform_to_, E,P),
+       expH2 = numericalDerivative22(transform_to_, E,P);
+  EXPECT(assert_equal(expH1, actH1, 1e-8));
+  EXPECT(assert_equal(expH2, actH2, 1e-8));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;