FundamentalMatrix.h
Go to the documentation of this file.
1 /*
2  * @file FundamentalMatrix.h
3  * @brief FundamentalMatrix classes
4  * @author Frank Dellaert
5  * @date October 2024
6  */
7 
8 #pragma once
9 
11 #include <gtsam/geometry/Rot3.h>
12 #include <gtsam/geometry/Unit3.h>
13 
14 namespace gtsam {
15 
28 class GTSAM_EXPORT FundamentalMatrix {
29  private:
30  Rot3 U_;
31  double s_;
32  Rot3 V_;
33 
34  public:
36  FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
37 
44  FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V);
45 
54  FundamentalMatrix(const Matrix3& F);
55 
68  FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E,
69  const Matrix3& Kb)
70  : FundamentalMatrix(Ka.transpose().inverse() * E.matrix() *
71  Kb.inverse()) {}
72 
83  FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb)
84  : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {}
85 
87  Matrix3 matrix() const;
88 
92  void print(const std::string& s = "") const;
93 
96  bool equals(const FundamentalMatrix& other, double tol = 1e-9) const;
98 
101  enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V
102  inline static size_t Dim() { return dimension; }
103  inline size_t dim() const { return dimension; }
104 
106  Vector localCoordinates(const FundamentalMatrix& F) const;
107 
109  FundamentalMatrix retract(const Vector& delta) const;
111  private:
113  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
114  : U_(U), s_(s), V_(V) {}
115 
117  void initialize(Matrix3 U, double s, Matrix3 V);
118 };
119 
128 class GTSAM_EXPORT SimpleFundamentalMatrix {
129  private:
131  double fa_;
132  double fb_;
135 
137  Matrix3 Ka() const;
138 
140  Matrix3 Kb() const;
141 
142  public:
145  : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {}
146 
156  double fa, double fb, const Point2& ca,
157  const Point2& cb)
158  : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
159 
162  Matrix3 matrix() const;
163 
167  void print(const std::string& s = "") const;
168 
170  bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const;
172 
175  enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb
176  inline static size_t Dim() { return dimension; }
177  inline size_t dim() const { return dimension; }
178 
180  Vector localCoordinates(const SimpleFundamentalMatrix& F) const;
181 
185 };
186 
193 GTSAM_EXPORT Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa,
194  const Matrix3& Fcb, const Point2& pb);
195 
198 template <typename F>
199 struct TripleF {
201 
203  Point2 transferToA(const Point2& pb, const Point2& pc) {
204  return EpipolarTransfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc);
205  }
206 
208  Point2 transferToB(const Point2& pa, const Point2& pc) {
209  return EpipolarTransfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc);
210  }
211 
213  Point2 transferToC(const Point2& pa, const Point2& pb) {
214  return EpipolarTransfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb);
215  }
216 };
217 
218 template <>
220  : public internal::Manifold<FundamentalMatrix> {};
221 
222 template <>
224  : public internal::Manifold<SimpleFundamentalMatrix> {};
225 
226 } // namespace gtsam
gtsam::SimpleFundamentalMatrix::fa_
double fa_
Focal length for left camera.
Definition: FundamentalMatrix.h:131
gtsam::FundamentalMatrix::s_
double s_
Scalar parameter for S.
Definition: FundamentalMatrix.h:31
EssentialMatrix.h
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::SimpleFundamentalMatrix::Dim
static size_t Dim()
Definition: FundamentalMatrix.h:176
gtsam::TripleF::transferToC
Point2 transferToC(const Point2 &pa, const Point2 &pb)
Transfers a point from cameras a,b to camera c.
Definition: FundamentalMatrix.h:213
gtsam::FundamentalMatrix::U_
Rot3 U_
Left rotation.
Definition: FundamentalMatrix.h:30
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::TripleF::Fbc
F Fbc
Definition: FundamentalMatrix.h:200
gtsam::SimpleFundamentalMatrix::ca_
Point2 ca_
Principal point for left camera.
Definition: FundamentalMatrix.h:133
gtsam::SimpleFundamentalMatrix::SimpleFundamentalMatrix
SimpleFundamentalMatrix(const EssentialMatrix &E, double fa, double fb, const Point2 &ca, const Point2 &cb)
Construct from essential matrix and focal lengths.
Definition: FundamentalMatrix.h:155
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::FundamentalMatrix::FundamentalMatrix
FundamentalMatrix(const Rot3 &U, double s, const Rot3 &V)
Private constructor for internal use.
Definition: FundamentalMatrix.h:113
gtsam::SimpleFundamentalMatrix::cb_
Point2 cb_
Principal point for right camera.
Definition: FundamentalMatrix.h:134
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
gtsam::EpipolarTransfer
Point2 EpipolarTransfer(const Matrix3 &Fca, const Point2 &pa, const Matrix3 &Fcb, const Point2 &pb)
Transfer projections from cameras a and b to camera c.
Definition: FundamentalMatrix.cpp:15
gtsam::TripleF::transferToB
Point2 transferToB(const Point2 &pa, const Point2 &pc)
Transfers a point from camera a,c to camera b.
Definition: FundamentalMatrix.h:208
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
Unit3.h
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::TripleF::Fab
F Fab
Definition: FundamentalMatrix.h:200
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SimpleFundamentalMatrix::dim
size_t dim() const
Definition: FundamentalMatrix.h:177
gtsam::SimpleFundamentalMatrix::SimpleFundamentalMatrix
SimpleFundamentalMatrix()
Default constructor.
Definition: FundamentalMatrix.h:144
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::FundamentalMatrix::dim
size_t dim() const
Definition: FundamentalMatrix.h:103
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
pc
int RealScalar int RealScalar int RealScalar * pc
Definition: level1_cplx_impl.h:119
gtsam::equals
Definition: Testable.h:112
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
E
DiscreteKey E(5, 2)
gtsam::FundamentalMatrix::FundamentalMatrix
FundamentalMatrix(const Matrix3 &Ka, const EssentialMatrix &E, const Matrix3 &Kb)
Construct from essential matrix and calibration matrices.
Definition: FundamentalMatrix.h:68
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::SimpleFundamentalMatrix
Class for representing a simple fundamental matrix.
Definition: FundamentalMatrix.h:128
gtsam::FundamentalMatrix::FundamentalMatrix
FundamentalMatrix(const Matrix3 &Ka, const Pose3 &aPb, const Matrix3 &Kb)
Construct from calibration matrices Ka, Kb, and pose aPb.
Definition: FundamentalMatrix.h:83
gtsam::TripleF::transferToA
Point2 transferToA(const Point2 &pb, const Point2 &pc)
Transfers a point from cameras b,c to camera a.
Definition: FundamentalMatrix.h:203
gtsam::TripleF
Definition: FundamentalMatrix.h:199
gtsam::tol
const G double tol
Definition: Group.h:79
U
@ U
Definition: testDecisionTree.cpp:349
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
gtsam::FundamentalMatrix::FundamentalMatrix
FundamentalMatrix()
Default constructor.
Definition: FundamentalMatrix.h:36
gtsam::FundamentalMatrix
Represents a fundamental matrix in computer vision, which encodes the epipolar geometry between two v...
Definition: FundamentalMatrix.h:28
gtsam::lago::initialize
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::SimpleFundamentalMatrix::E_
EssentialMatrix E_
Essential matrix.
Definition: FundamentalMatrix.h:130
gtsam::FundamentalMatrix::Dim
static size_t Dim()
Definition: FundamentalMatrix.h:102
gtsam::SimpleFundamentalMatrix::fb_
double fb_
Focal length for right camera.
Definition: FundamentalMatrix.h:132
gtsam::FundamentalMatrix::V_
Rot3 V_
Right rotation.
Definition: FundamentalMatrix.h:32
gtsam::TripleF::Fca
F Fca
Definition: FundamentalMatrix.h:200


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:33