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 <iostream>

namespace gtsam {

class GTSAM_EXPORT EssentialMatrix: public DerivedValue<EssentialMatrix> {

private:

  Rot3 aRb_; 
  Unit3 aTb_; 
  Matrix3 E_; 

public:

  static Vector Homogeneous(const Point2& p) {
    return Vector(3) << p.x(), p.y(), 1;
  }


  EssentialMatrix() :
      aTb_(1, 0, 0), E_(aTb_.skew()) {
  }

  EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
      aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
  }

  static EssentialMatrix FromPose3(const Pose3& _1P2_,
      boost::optional<Matrix&> H = boost::none);

  template<typename Engine>
  static EssentialMatrix Random(Engine & rng) {
    return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
  }



  void print(const std::string& s = "") const;

  bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
    return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
  }



  inline static size_t Dim() {
    return 5;
  }

  virtual size_t dim() const {
    return 5;
  }

  virtual EssentialMatrix retract(const Vector& xi) const;

  virtual Vector localCoordinates(const EssentialMatrix& other) const;



  inline const Rot3& rotation() const {
    return aRb_;
  }

  inline const Unit3& direction() const {
    return aTb_;
  }

  inline const Matrix3& matrix() const {
    return E_;
  }

  inline const Unit3& epipole_a() const {
    return aTb_; // == direction()
  }

  inline Unit3 epipole_b() const {
    return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
  }

  Point3 transform_to(const Point3& p,
      boost::optional<Matrix&> DE = boost::none,
      boost::optional<Matrix&> Dpoint = boost::none) const;

  EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
      boost::none, boost::optional<Matrix&> HR = boost::none) const;

  friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
    return E.rotate(cRb);
  }

  double error(const Vector& vA, const Vector& vB, //
      boost::optional<Matrix&> 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::make_nvp("EssentialMatrix",
          boost::serialization::base_object<Value>(*this));
      ar & BOOST_SERIALIZATION_NVP(aRb_);
      ar & BOOST_SERIALIZATION_NVP(aTb_);

      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));
    }


};

} // gtsam