EssentialMatrixFactor.h

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
/*
 * @file EssentialMatrixFactor.h
 * @brief EssentialMatrixFactor class
 * @author Frank Dellaert
 * @author Ayush Baid
 * @author Akshay Krishnan
 * @date December 17, 2013
 */
 
#pragma once
 
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <iostream>
 
namespace gtsam {

class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
  Vector3 vA_, vB_;  
 
  typedef NoiseModelFactorN<EssentialMatrix> Base;
  typedef EssentialMatrixFactor This;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
                        const SharedNoiseModel& model)
      : Base(model, key) {
    vA_ = EssentialMatrix::Homogeneous(pA);
    vB_ = EssentialMatrix::Homogeneous(pB);
  }

  template <class CALIBRATION>
  EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
                        const SharedNoiseModel& model,
                        std::shared_ptr<CALIBRATION> K)
      : Base(model, key) {
#ifndef NDEBUG
    if (!K) throw;
#endif
    vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
    vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "  EssentialMatrixFactor with measurements\n  ("
              << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
              << std::endl;
  }

  Vector evaluateError(const EssentialMatrix& E,
                       OptionalMatrixType H) const override {
    Vector error(1);
    error(0) = E.error(vA_, vB_, H);
    return error;
  }
};

class EssentialMatrixFactor2
    : public NoiseModelFactorT<Vector2, EssentialMatrix, double> {
  Point3 dP1_;  
  Point2 pn_;   
  double f_;    
 
  typedef NoiseModelFactorT<Vector2, EssentialMatrix, double> Base;
  typedef EssentialMatrixFactor2 This;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
                         const SharedNoiseModel& model)
      : Base(model, key1, key2),
        dP1_(EssentialMatrix::Homogeneous(pA)),
        pn_(pB) {
    f_ = 1.0;
  }

  template <class CALIBRATION>
  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
                         const SharedNoiseModel& model,
                         std::shared_ptr<CALIBRATION> K)
      : Base(model, key1, key2),
        dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
        pn_(K->calibrate(pB)) {
    f_ = 0.5 * (K->fx() + K->fy());
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
              << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
              << std::endl;
  }
 
  /*
   * Vector of errors returns 2D vector
   * @param E essential matrix
   * @param d inverse depth d
   */
  Vector2 evaluateError(const EssentialMatrix& E, const double& d,
                        OptionalMatrixType DE,
                        OptionalMatrixType Dd) const override {
    // We have point x,y in image 1
    // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
    // We then convert to second camera by P2 = 1R2'*(P1-1T2)
    // The homogeneous coordinates of can be written as
    //   2R1*(P1-1T2) ==  2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
    // where we multiplied with d which yields equivalent homogeneous
    // coordinates. Note that this is just the homography 2R1 for d==0 The point
    // d*P1 = (x,y,1) is computed in constructor as dP1_
 
    // Project to normalized image coordinates, then uncalibrate
    Point2 pn(0, 0);
    if (!DE && !Dd) {
      Point3 _1T2 = E.direction().point3();
      Point3 d1T2 = d * _1T2;
      Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);  // 2R1*((x,y,1)-d*1T2)
      pn = PinholeBase::Project(dP2);
 
    } else {
      // Calculate derivatives. TODO if slow: optimize with Mathematica
      //     3*2        3*3       3*3
      Matrix D_1T2_dir, DdP2_rot, DP2_point;
 
      Point3 _1T2 = E.direction().point3(D_1T2_dir);
      Point3 d1T2 = d * _1T2;
      Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
 
      Matrix23 Dpn_dP2;
      pn = PinholeBase::Project(dP2, Dpn_dP2);
 
      if (DE) {
        Matrix DdP2_E(3, 5);
        DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir;  // (3*3), (3*3) * (3*2)
        *DE = f_ * Dpn_dP2 * DdP2_E;                     // (2*3) * (3*5)
      }
 
      if (Dd)  // efficient backwards computation:
        //      (2*3)    * (3*3)      * (3*1)
        *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
    }
    Point2 reprojectionError = pn - pn_;
    return f_ * reprojectionError;
  }
};
// EssentialMatrixFactor2

class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
  typedef EssentialMatrixFactor2 Base;
  typedef EssentialMatrixFactor3 This;
 
  Rot3 cRb_;  
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
                         const Rot3& cRb, const SharedNoiseModel& model)
      : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}

  template <class CALIBRATION>
  EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
                         const Rot3& cRb, const SharedNoiseModel& model,
                         std::shared_ptr<CALIBRATION> K)
      : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "  EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
  }
 
  /*
   * Vector of errors returns 2D vector
   * @param E essential matrix
   * @param d inverse depth d
   */
  Vector2 evaluateError(const EssentialMatrix& E, const double& d,
                        OptionalMatrixType DE,
                        OptionalMatrixType Dd) const override {
    if (!DE) {
      // Convert E from body to camera frame
      EssentialMatrix cameraE = cRb_ * E;
      // Evaluate error
      return Base::evaluateError(cameraE, d, OptionalNone, Dd);
    } else {
      // Version with derivatives
      Matrix D_e_cameraE, D_cameraE_E;  // 2*5, 5*5
      EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
      // Using the pointer version of evaluateError since the Base class
      // (EssentialMatrixFactor2) does not have the matrix reference version of
      // evaluateError
      Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
      *DE = D_e_cameraE * D_cameraE_E;  // (2*5) * (5*5)
      return e;
    }
  }
};
// EssentialMatrixFactor3

template <class CALIBRATION>
class EssentialMatrixFactor4
    : public NoiseModelFactorT<Vector1, EssentialMatrix, CALIBRATION> {
 private:
  Point2 pA_, pB_;  
 
  typedef NoiseModelFactorT<Vector1, EssentialMatrix, CALIBRATION> Base;
  typedef EssentialMatrixFactor4 This;
 
  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
  typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
                         const SharedNoiseModel& model = nullptr)
      : Base(noiseModel::validOrDefault(0.0, model), keyE, keyK),
        pA_(pA), pB_(pB) {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "  EssentialMatrixFactor4 with measurements\n  ("
              << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
              << std::endl;
  }

  Vector1 evaluateError(const EssentialMatrix& E, const CALIBRATION& K,
                        OptionalMatrixType HE,
                        OptionalMatrixType HK) const override {
    // converting from pixel coordinates to normalized coordinates cA and cB
    JacobianCalibration cA_H_K;  // dcA/dK
    JacobianCalibration cB_H_K;  // dcB/dK
    Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone);
    Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone);
 
    // convert to homogeneous coordinates
    Vector3 vA = EssentialMatrix::Homogeneous(cA);
    Vector3 vB = EssentialMatrix::Homogeneous(cB);
 
    if (HK) {
      // compute the jacobian of error w.r.t K
 
      // error function f = vA.T * E * vB
      // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
      // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
      // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
      Matrix DynamicH_K = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
          vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;  // (1*2) * (2*DimK)
      *HK = DynamicH_K;
    }
 
    return Vector1(E.error(vA, vB, HE));
  }
};
// EssentialMatrixFactor4

template <class CALIBRATION>
class EssentialMatrixFactor5
    : public NoiseModelFactorT<Vector1, EssentialMatrix, CALIBRATION,
                               CALIBRATION> {
 private:
  Point2 pA_, pB_;  
 
  typedef NoiseModelFactorT<Vector1, EssentialMatrix, CALIBRATION, CALIBRATION>
      Base;
  typedef EssentialMatrixFactor5 This;
 
  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
  typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
                         const Point2& pB,
                         const SharedNoiseModel& model = nullptr)
      : Base(noiseModel::validOrDefault(0.0, model), keyE, keyKa, keyKb),
        pA_(pA), pB_(pB) {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "  EssentialMatrixFactor5 with measurements\n  ("
              << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
              << std::endl;
  }

  Vector1 evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka,
                        const CALIBRATION& Kb, OptionalMatrixType HE,
                        OptionalMatrixType HKa,
                        OptionalMatrixType HKb) const override {
    // converting from pixel coordinates to normalized coordinates cA and cB
    JacobianCalibration cA_H_Ka;  // dcA/dKa
    JacobianCalibration cB_H_Kb;  // dcB/dKb
    Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone);
    Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone);
 
    // Convert to homogeneous coordinates.
    Vector3 vA = EssentialMatrix::Homogeneous(cA);
    Vector3 vB = EssentialMatrix::Homogeneous(cB);
 
    if (HKa) {
      // Compute the jacobian of error w.r.t Ka.
      Matrix DynamicHka = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
      *HKa = DynamicHka;
    }
 
    if (HKb) {
      // Compute the jacobian of error w.r.t Kb.
      Matrix DynamicHkb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
      *HKb = DynamicHkb;
    }
 
    return Vector1(E.error(vA, vB, HE));
  }
};
// EssentialMatrixFactor5
 
}  // namespace gtsam