FundamentalMatrix.h

/*
 * @file FundamentalMatrix.h
 * @brief FundamentalMatrix classes
 * @author Frank Dellaert
 * @date October 2024
 */
 
#pragma once
 
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
 
namespace gtsam {

class GTSAM_EXPORT FundamentalMatrix {
 private:
  Rot3 U_;    
  double s_;  
  Rot3 V_;    
 
 public:
  FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}

  FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V);

  FundamentalMatrix(const Matrix3& F);

  FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E,
                    const Matrix3& Kb)
      : FundamentalMatrix(Ka.transpose().inverse() * E.matrix() *
                          Kb.inverse()) {}

  FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb)
      : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {}

  Matrix3 matrix() const;

  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});

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

  bool equals(const FundamentalMatrix& other, double tol = 1e-9) const;

  inline constexpr static auto dimension = 7;  // 3 for U, 1 for s, 3 for V
  inline static size_t Dim() { return dimension; }
  inline size_t dim() const { return dimension; }

  Vector localCoordinates(const FundamentalMatrix& F) const;

  FundamentalMatrix retract(const Vector& delta) const;
 private:
  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
      : U_(U), s_(s), V_(V) {}

  void initialize(Matrix3 U, double s, Matrix3 V);
};

class GTSAM_EXPORT SimpleFundamentalMatrix {
 private:
  EssentialMatrix E_;  
  double fa_;          
  double fb_;          
  Point2 ca_;          
  Point2 cb_;          

  Matrix3 Ka() const;

  Matrix3 Kb() const;
 
 public:
  SimpleFundamentalMatrix()
      : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {}

  SimpleFundamentalMatrix(const EssentialMatrix& E,  //
                          double fa, double fb, const Point2& ca,
                          const Point2& cb)
      : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}

  Matrix3 matrix() const;

  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});

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

  bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const;

  inline constexpr static auto dimension = 7;  // 5 for E, 1 for fa, 1 for fb
  inline static size_t Dim() { return dimension; }
  inline size_t dim() const { return dimension; }

  Vector localCoordinates(const SimpleFundamentalMatrix& F) const;

  SimpleFundamentalMatrix retract(const Vector& delta) const;
};

GTSAM_EXPORT Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa,
                                     const Matrix3& Fcb, const Point2& pb);

template <typename F>
struct TripleF {
  F Fab, Fbc, Fca;

  Point2 transferToA(const Point2& pb, const Point2& pc) {
    return EpipolarTransfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc);
  }

  Point2 transferToB(const Point2& pa, const Point2& pc) {
    return EpipolarTransfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc);
  }

  Point2 transferToC(const Point2& pa, const Point2& pb) {
    return EpipolarTransfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb);
  }
};
 
template <>
struct traits<FundamentalMatrix>
    : public internal::Manifold<FundamentalMatrix> {};
 
template <>
struct traits<SimpleFundamentalMatrix>
    : public internal::Manifold<SimpleFundamentalMatrix> {};
 
}  // namespace gtsam