diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 2d9c4e0da..6584c1728 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,7 +7,7 @@ #pragma once -#include +#include #include #include #include @@ -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 DE = boost::none, + boost::optional 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 H = boost::none) const { diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 83e41ce64..ada90ee1a 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -7,8 +7,8 @@ #include #include +#include #include - #include 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;