From 3cd45be423bbcb08b1005c28ba11ea53bace155a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:26:19 -0500 Subject: [PATCH] Named constructor FromPose3, with Jacobians --- gtsam/geometry/EssentialMatrix.cpp | 127 +++++++++++++++++++ gtsam/geometry/EssentialMatrix.h | 91 ++----------- gtsam/geometry/tests/testEssentialMatrix.cpp | 28 +++- 3 files changed, 168 insertions(+), 78 deletions(-) create mode 100644 gtsam/geometry/EssentialMatrix.cpp diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp new file mode 100644 index 000000000..9e50ad92a --- /dev/null +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -0,0 +1,127 @@ +/* + * @file EssentialMatrix.cpp + * @brief EssentialMatrix class + * @author Frank Dellaert + * @date December 5, 2014 + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, + boost::optional H) { + const Rot3& _1R2_ = _1P2_.rotation(); + const Point3& _1T2_ = _1P2_.translation(); + if (!H) { + // just make a direction out of translation and create E + Sphere2 direction(_1T2_); + return EssentialMatrix(_1R2_, direction); + } else { + // Calculate the 5*6 Jacobian H = D_E_1P2 + // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation + // First get 2*3 derivative from Sphere2::FromPoint3 + Matrix D_direction_1T2; + Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2); + H->resize(5, 6); + H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left + H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left + H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right + H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + return EssentialMatrix(_1R2_, direction); + } +} + +/* ************************************************************************* */ +void EssentialMatrix::print(const std::string& s) const { + std::cout << s; + aRb_.print("R:\n"); + aTb_.print("d: "); +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { + assert(xi.size() == 5); + Vector3 omega(sub(xi, 0, 3)); + Vector2 z(sub(xi, 3, 5)); + Rot3 R = aRb_.retract(omega); + Sphere2 t = aTb_.retract(z); + return EssentialMatrix(R, t); +} + +/* ************************************************************************* */ +Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + return Vector(5) << // + aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); +} + +/* ************************************************************************* */ +Point3 EssentialMatrix::transform_to(const Point3& p, + boost::optional DE, boost::optional Dpoint) 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() + // 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(); + *DE = H; + } + return q; +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, + boost::optional HE, boost::optional HR) 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); + } +} + +/* ************************************************************************* */ +double EssentialMatrix::error(const Vector& vA, const Vector& vB, // + boost::optional H) const { + if (H) { + H->resize(1, 5); + // See math.lyx + Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) + * aTb_.basis(); + *H << HR, HD; + } + return dot(vA, E_ * vB); +} + +/* ************************************************************************* */ + +} // gtsam + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 54fd94bbc..3c5836b5e 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,7 +10,6 @@ #include #include #include -#include namespace gtsam { @@ -26,7 +25,7 @@ private: Rot3 aRb_; ///< Rotation between a and b Sphere2 aTb_; ///< translation direction from a to b - Matrix E_; ///< Essential matrix + Matrix3 E_; ///< Essential matrix public: @@ -48,6 +47,10 @@ public: aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { } + /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) + static EssentialMatrix FromPose3(const Pose3& _1P2_, + boost::optional H = boost::none); + /// Random, using Rot3::Random and Sphere2::Random template static EssentialMatrix Random(Engine & rng) { @@ -60,11 +63,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const { - std::cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); - } + void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { @@ -87,20 +86,10 @@ public: } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Sphere2 t = aTb_.retract(z); - return EssentialMatrix(R, t); - } + virtual EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const { - return Vector(5) << // - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); - } + virtual Vector localCoordinates(const EssentialMatrix& other) const; /// @} @@ -108,17 +97,17 @@ public: /// @{ /// Rotation - const Rot3& rotation() const { + inline const Rot3& rotation() const { return aRb_; } /// Direction - const Sphere2& direction() const { + inline const Sphere2& direction() const { return aTb_; } /// Return 3*3 matrix representation - const Matrix& matrix() const { + inline const Matrix3& matrix() const { return E_; } @@ -131,20 +120,7 @@ public: */ 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() - // 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(); - *DE = H; - } - return q; - } + boost::optional Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -152,36 +128,7 @@ public: * @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); - } - } + boost::none, boost::optional HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -194,17 +141,7 @@ public: /// epipolar error, algebraic double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const { - if (H) { - H->resize(1, 5); - // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); - *H << HR, HD; - } - return dot(vA, E_ * vB); - } + boost::optional H = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index da5240b15..56fc8d8ad 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -88,13 +88,39 @@ TEST (EssentialMatrix, rotate) { // Derivatives Matrix actH1, actH2; - try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception + 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)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3_a) { + Matrix actualH; + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//************************************************************************* +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, c1Tc2); + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ int main() { TestResult tr;