diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 77d8e98cf..54fd94bbc 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -88,7 +88,7 @@ public: /// Retract delta to manifold virtual EssentialMatrix retract(const Vector& xi) const { - assert(xi.size()==5); + assert(xi.size() == 5); Vector3 omega(sub(xi, 0, 3)); Vector2 z(sub(xi, 3, 5)); Rot3 R = aRb_.retract(omega); @@ -97,8 +97,9 @@ public: } /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& value) const { - return Vector(5) << 0, 0, 0, 0, 0; + virtual Vector localCoordinates(const EssentialMatrix& other) const { + return Vector(5) << // + aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); } /// @} @@ -139,21 +140,56 @@ public: // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame Matrix H(3, 5); - H << DE->block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); + H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; } + /** + * Given essential matrix E in camera frame B, convert to body frame C + * @param cRb rotation from body frame to camera frame + * @param E essential matrix E in camera frame C + */ + EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = + boost::none, boost::optional HR = boost::none) const { + + // The rotation must be conjugated to act in the camera frame + Rot3 c1Rc2 = aRb_.conjugate(cRb); + + if (!HE && !HR) { + // Rotate translation direction and return + Sphere2 c1Tc2 = cRb * aTb_; + return EssentialMatrix(c1Rc2, c1Tc2); + } else { + // Calculate derivatives + Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + if (HE) { + *HE = zeros(5, 5); + HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 + HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) + } + if (HR) { + throw std::runtime_error( + "EssentialMatrix::rotate: derivative HR not implemented yet"); + /* + HR->resize(5, 3); + HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? + HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? + */ + } + return EssentialMatrix(c1Rc2, c1Tc2); + } + } + /** * Given essential matrix E in camera frame B, convert to body frame C * @param cRb rotation from body frame to camera frame * @param E essential matrix E in camera frame C */ friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) { - Rot3 c1Rc2 = E.aRb_.conjugate(cRb); - Sphere2 c1Tc2 = cRb * E.aTb_; - return EssentialMatrix(c1Rc2, c1Tc2); + return E.rotate(cRb); } /// epipolar error, algebraic diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 69f5ec4e7..da5240b15 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -58,14 +58,16 @@ TEST (EssentialMatrix, transform_to) { 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); + 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)); } //************************************************************************* - +EssentialMatrix rotate_(const EssentialMatrix& E, const Rot3& cRb) { + return E.rotate(cRb); +} TEST (EssentialMatrix, rotate) { // Suppose the essential matrix is specified in a body coordinate frame B // which is rotated with respect to the camera frame C, via rotation bRc. @@ -78,9 +80,19 @@ TEST (EssentialMatrix, rotate) { Point3 b1Tb2 = bRc * c1Tc2; EssentialMatrix bodyE(b1Rb2, b1Tb2); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); + EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); // Let's go back to camera frame: EXPECT(assert_equal(trueE, cRb * bodyE, 1e-8)); + EXPECT(assert_equal(trueE, bodyE.rotate(cRb), 1e-8)); + + // Derivatives + Matrix actH1, actH2; + try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception + Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // + expH2 = numericalDerivative22(rotate_, bodyE, cRb); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */