diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 062178fea..7b90edc39 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -13,64 +13,46 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { - const Rot3& _1R2_ = _1P2_.rotation(); - const Point3& _1T2_ = _1P2_.translation(); + const Rot3& aRb = aPb.rotation(); + const Point3& aTb = aPb.translation(); if (!H) { // just make a direction out of translation and create E - Unit3 direction(_1T2_); - return EssentialMatrix(_1R2_, direction); + Unit3 direction(aTb); + return EssentialMatrix(aRb, 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 Unit3::FromPoint3 Matrix23 D_direction_1T2; - Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); + Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2); *H << I_3x3, Z_3x3, // - Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); - return EssentialMatrix(_1R2_, direction); + Matrix23::Zero(), D_direction_1T2 * aRb.matrix(); + return EssentialMatrix(aRb, direction); } } /* ************************************************************************* */ void EssentialMatrix::print(const string& s) const { 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); - Unit3 t = aTb_.retract(z); - return EssentialMatrix(R, t); -} - -/* ************************************************************************* */ -Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - Vector5 v; - v << aRb_.localCoordinates(other.aRb_), - aTb_.localCoordinates(other.aTb_); - return v; + rotation().print("R:\n"); + direction().print("d: "); } /* ************************************************************************* */ Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, OptionalJacobian<3, 3> Dpoint) const { - Pose3 pose(aRb_, aTb_.point3()); + Pose3 pose(rotation(), direction().point3()); Matrix36 DE_; Point3 q = pose.transform_to(p, DE ? &DE_ : 0, 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() + // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // Duy made an educated guess that this needs to be rotated to the local frame Matrix35 H; - H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); + H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis(); *DE = H; } return q; @@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); + Rot3 c1Rc2 = rotation().conjugate(cRb); if (!HE && !HR) { // Rotate translation direction and return - Unit3 c1Tc2 = cRb * aTb_; + Unit3 c1Tc2 = cRb * direction(); return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives Matrix23 D_c1Tc2_cRb; // 2*3 Matrix2 D_c1Tc2_aTb; // 2*2 - Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) *HE << cRb.matrix(), Matrix32::Zero(), // Matrix23::Zero(), D_c1Tc2_aTb; @@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); *H << HR, HD; } return dot(vA, E_ * vB); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index c5243c68b..6a94c8b11 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,6 +7,55 @@ #pragma once +#include + +namespace gtsam { + +/// CRTP to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes manifold structure from M1 and M2, and binary constructor +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +private: + const M1& g() const {return this->first;} + const M2& h() const {return this->second;} + +public: + // Dimension of the manifold + enum { dimension = M1::dimension + M2::dimension }; + + typedef Eigen::Matrix TangentVector; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductManifold(const M1& g, const M2& h):std::pair(g,h) {} + + /// Retract delta to manifold + Derived retract(const TangentVector& xi) const { + return Derived(traits::Retract(g(),xi.head(M1::dimension)), + traits::Retract(h(),xi.tail(M2::dimension))); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const Derived& other) const { + TangentVector xi; + xi << traits::Local(g(),other.g()), traits::Local(h(),other.h()); + return xi; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold< + ProductManifold > { +}; + +} // namespace gtsam + #include #include #include @@ -20,18 +69,15 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix { +class GTSAM_EXPORT EssentialMatrix : public ProductManifold { private: - Rot3 aRb_; ///< Rotation between a and b - Unit3 aTb_; ///< translation direction from a to b + typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix public: - enum { dimension = 5 }; - /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { return Vector3(p.x(), p.y(), 1); @@ -42,12 +88,12 @@ public: /// Default constructor EssentialMatrix() : - aTb_(1, 0, 0), E_(aTb_.skew()) { + Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) { } /// Construct from rotation and translation EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : - aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { + Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) @@ -72,43 +118,23 @@ public: /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { - return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + return rotation().equals(other.rotation(), tol) + && direction().equals(other.direction(), tol); } /// @} - /// @name Manifold - /// @{ - - /// Dimensionality of tangent space = 5 DOF - inline static size_t Dim() { - return 5; - } - - /// Return the dimensionality of the tangent space - size_t dim() const { - return 5; - } - - /// Retract delta to manifold - EssentialMatrix retract(const Vector& xi) const; - - /// Compute the coordinates in the tangent space - Vector5 localCoordinates(const EssentialMatrix& other) const; - - /// @} - /// @name Essential matrix methods /// @{ /// Rotation inline const Rot3& rotation() const { - return aRb_; + return this->first; } /// Direction inline const Unit3& direction() const { - return aTb_; + return this->second; } /// Return 3*3 matrix representation @@ -118,12 +144,12 @@ public: /// Return epipole in image_a , as Unit3 to allow for infinity inline const Unit3& epipole_a() const { - return aTb_; // == direction() + return direction(); } /// Return epipole in image_b, as Unit3 to allow for infinity inline Unit3 epipole_b() const { - return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + return rotation().unrotate(direction()); } /** @@ -180,8 +206,8 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(aRb_); - ar & BOOST_SERIALIZATION_NVP(aTb_); + ar & BOOST_SERIALIZATION_NVP(rotation()); + ar & BOOST_SERIALIZATION_NVP(direction()); ar & boost::serialization::make_nvp("E11", E_(0,0)); ar & boost::serialization::make_nvp("E12", E_(0,1)); @@ -195,7 +221,6 @@ private: } /// @} - }; template<>