EssentialMatrix.h

/*
 * @file EssentialMatrix.h
 * @brief EssentialMatrix class
 * @author Frank Dellaert
 * @date December 17, 2013
 */
 
#pragma once
 
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
 
#include <iosfwd>
#include <string>
 
namespace gtsam {
 
class EssentialMatrix {
 private:
  Rot3 R_;     
  Unit3 t_;    
  Matrix3 E_;  
 
 public:
  static Vector3 Homogeneous(const Point2& p) {
    return Vector3(p.x(), p.y(), 1);
  }
 
 
  EssentialMatrix() :E_(t_.skew()) {
  }
 
  EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
      R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) {
  }
 
  GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
                                                  OptionalJacobian<5, 3> H1 = boost::none,
                                                  OptionalJacobian<5, 2> H2 = boost::none);
 
  GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_,
      OptionalJacobian<5, 6> H = boost::none);
 
  template<typename Engine>
  static EssentialMatrix Random(Engine & rng) {
    return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
  }
 
  virtual ~EssentialMatrix() {}
 
 
 
  GTSAM_EXPORT void print(const std::string& s = "") const;
 
  bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
    return R_.equals(other.R_, tol)
        && t_.equals(other.t_, tol);
  }
 
 
  enum { dimension = 5 };
  inline static size_t Dim() { return dimension;}
  inline size_t dim() const { return dimension;}
 
  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
  EssentialMatrix retract(const Vector5& xi) const {
    return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>()));
  }
 
  Vector5 localCoordinates(const EssentialMatrix& other) const {
    auto v1 = R_.localCoordinates(other.R_);
    auto v2 = t_.localCoordinates(other.t_);
    Vector5 v;
    v << v1, v2;
    return v;
  }
 
 
  inline const Rot3& rotation() const {
    return R_;
  }
 
  inline const Unit3& direction() const {
    return t_;
  }
 
  inline const Matrix3& matrix() const {
    return E_;
  }
 
  inline const Unit3& epipole_a() const {
    return t_;
  }
 
  inline Unit3 epipole_b() const {
    return R_.unrotate(t_);
  }
 
  GTSAM_EXPORT Point3 transformTo(const Point3& p,
      OptionalJacobian<3, 5> DE = boost::none,
      OptionalJacobian<3, 3> Dpoint = boost::none) const;
 
  GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
      boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
 
  friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
    return E.rotate(cRb);
  }
 
  GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
      OptionalJacobian<1, 5> H = boost::none) const;
 
 
 
  GTSAM_EXPORT friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
 
  GTSAM_EXPORT friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
 
 
 private:
 
  friend class boost::serialization::access;
  template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(R_);
      ar & BOOST_SERIALIZATION_NVP(t_);
 
      ar & boost::serialization::make_nvp("E11", E_(0, 0));
      ar & boost::serialization::make_nvp("E12", E_(0, 1));
      ar & boost::serialization::make_nvp("E13", E_(0, 2));
      ar & boost::serialization::make_nvp("E21", E_(1, 0));
      ar & boost::serialization::make_nvp("E22", E_(1, 1));
      ar & boost::serialization::make_nvp("E23", E_(1, 2));
      ar & boost::serialization::make_nvp("E31", E_(2, 0));
      ar & boost::serialization::make_nvp("E32", E_(2, 1));
      ar & boost::serialization::make_nvp("E33", E_(2, 2));
    }
 
 
 public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
 
template<>
struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 
template<>
struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 
}  // namespace gtsam