diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 6584c1728..c5436b554 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -129,7 +129,7 @@ public: 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(); + H << DE->block < 3, 3 > (0, 0), -aRb_.inverse().matrix() * aTb_.basis(); std::cout << H << std::endl << std::endl; *DE = H; } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index ada90ee1a..1a2965089 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -47,7 +47,10 @@ TEST (EssentialMatrix, retract2) { //************************************************************************* Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); } TEST (EssentialMatrix, transform_to) { - EssentialMatrix E(aRb, aTb); + // test with a more complicated EssentialMatrix + Rot3 aRb2 = Rot3::yaw(M_PI/3.0)*Rot3::pitch(M_PI_4)*Rot3::roll(M_PI/6.0); + Point3 aTb2(19.2, 3.7, 5.9); + EssentialMatrix E(aRb2, aTb2); //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0))); static Point3 P(0.2,0.7,-2); Matrix actH1, actH2;