FundamentalMatrix.cpp
Go to the documentation of this file.
1 /*
2  * @file FundamentalMatrix.cpp
3  * @brief FundamentalMatrix classes
4  * @author Frank Dellaert
5  * @date October 2024
6  */
7 
8 #include <gtsam/base/Matrix.h>
10 #include <gtsam/geometry/Point2.h>
11 
12 namespace gtsam {
13 
14 //*************************************************************************
15 Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, //
16  const Matrix3& Fcb, const Point2& pb) {
17  // Create lines in camera a from projections of the other two cameras
18  Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1);
19  Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1);
20 
21  // Cross the lines to find the intersection point
22  Vector3 intersectionPoint = line_a.cross(line_b);
23 
24  // Normalize the intersection point
25  intersectionPoint /= intersectionPoint(2);
26 
27  return intersectionPoint.head<2>(); // Return the 2D point
28 }
29 
30 //*************************************************************************
31 FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s,
32  const Matrix3& V) {
33  initialize(U, s, V);
34 }
35 
37  // Perform SVD
39 
40  // Extract U and V
41  Matrix3 U = svd.matrixU();
42  Matrix3 V = svd.matrixV();
43  Vector3 singularValues = svd.singularValues();
44 
45  // Scale the singular values
46  double scale = singularValues(0);
47  if (scale != 0) {
48  singularValues /= scale; // Normalize the first singular value to 1.0
49  }
50 
51  // Check if the third singular value is close to zero (valid F condition)
52  if (std::abs(singularValues(2)) > 1e-9) {
53  throw std::invalid_argument(
54  "The input matrix does not represent a valid fundamental matrix.");
55  }
56 
57  initialize(U, singularValues(1), V);
58 }
59 
60 void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
61  // Check if U is a reflection and flip third column if so
62  if (U.determinant() < 0) U.col(2) *= -1;
63 
64  // Same check for V
65  if (V.determinant() < 0) V.col(2) *= -1;
66 
67  U_ = Rot3(U);
68  s_ = s;
69  V_ = Rot3(V);
70 }
71 
72 Matrix3 FundamentalMatrix::matrix() const {
73  return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
74  V_.transpose().matrix();
75 }
76 
77 void FundamentalMatrix::print(const std::string& s) const {
78  std::cout << s << matrix() << std::endl;
79 }
80 
82  double tol) const {
83  return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
84  V_.equals(other.V_, tol);
85 }
86 
88  Vector result(7);
89  result.head<3>() = U_.localCoordinates(F.U_);
90  result(3) = F.s_ - s_; // Difference in scalar
91  result.tail<3>() = V_.localCoordinates(F.V_);
92  return result;
93 }
94 
96  const Rot3 newU = U_.retract(delta.head<3>());
97  const double newS = s_ + delta(3);
98  const Rot3 newV = V_.retract(delta.tail<3>());
99  return FundamentalMatrix(newU, newS, newV);
100 }
101 
102 //*************************************************************************
104  Matrix3 K;
105  K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
106  return K;
107 }
108 
110  Matrix3 K;
111  K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
112  return K;
113 }
114 
116  return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
117 }
118 
119 void SimpleFundamentalMatrix::print(const std::string& s) const {
120  std::cout << s << " E:\n"
121  << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
122  << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose()
123  << std::endl;
124 }
125 
127  double tol) const {
128  return E_.equals(other.E_, tol) && std::abs(fa_ - other.fa_) < tol &&
129  std::abs(fb_ - other.fb_) < tol && (ca_ - other.ca_).norm() < tol &&
130  (cb_ - other.cb_).norm() < tol;
131 }
132 
134  const SimpleFundamentalMatrix& F) const {
135  Vector result(7);
136  result.head<5>() = E_.localCoordinates(F.E_);
137  result(5) = F.fa_ - fa_; // Difference in fa
138  result(6) = F.fb_ - fb_; // Difference in fb
139  return result;
140 }
141 
143  const Vector& delta) const {
144  EssentialMatrix newE = E_.retract(delta.head<5>());
145  double newFa = fa_ + delta(5); // Update fa
146  double newFb = fb_ + delta(6); // Update fb
147  return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_);
148 }
149 
150 //*************************************************************************
151 
152 } // 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
gtsam::FundamentalMatrix::equals
bool equals(const FundamentalMatrix &other, double tol=1e-9) const
Definition: FundamentalMatrix.cpp:81
Eigen::ComputeFullV
@ ComputeFullV
Definition: Constants.h:397
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::SimpleFundamentalMatrix::matrix
Matrix3 matrix() const
Definition: FundamentalMatrix.cpp:115
gtsam::FundamentalMatrix::initialize
void initialize(Matrix3 U, double s, Matrix3 V)
Initialize SO(3) matrices from general O(3) matrices.
Definition: FundamentalMatrix.cpp:60
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::SimpleFundamentalMatrix::localCoordinates
Vector localCoordinates(const SimpleFundamentalMatrix &F) const
Return local coordinates with respect to another SimpleFundamentalMatrix.
Definition: FundamentalMatrix.cpp:133
gtsam::SimpleFundamentalMatrix::ca_
Point2 ca_
Principal point for left camera.
Definition: FundamentalMatrix.h:133
Eigen::ComputeFullU
@ ComputeFullU
Definition: Constants.h:393
gtsam::SimpleFundamentalMatrix::cb_
Point2 cb_
Principal point for right camera.
Definition: FundamentalMatrix.h:134
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
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::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::EssentialMatrix::retract
EssentialMatrix retract(const Vector5 &xi) const
Retract delta to manifold.
Definition: EssentialMatrix.h:92
Point2.h
2D Point
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::EssentialMatrix::localCoordinates
Vector5 localCoordinates(const EssentialMatrix &other) const
Compute the coordinates in the tangent space.
Definition: EssentialMatrix.h:97
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::SimpleFundamentalMatrix::SimpleFundamentalMatrix
SimpleFundamentalMatrix()
Default constructor.
Definition: FundamentalMatrix.h:144
gtsam::FundamentalMatrix::matrix
Matrix3 matrix() const
Return the fundamental matrix representation.
Definition: FundamentalMatrix.cpp:72
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SimpleFundamentalMatrix::Ka
Matrix3 Ka() const
Return the left calibration matrix.
Definition: FundamentalMatrix.cpp:103
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::SimpleFundamentalMatrix::retract
SimpleFundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new SimpleFundamentalMatrix.
Definition: FundamentalMatrix.cpp:142
gtsam::SimpleFundamentalMatrix::equals
bool equals(const SimpleFundamentalMatrix &other, double tol=1e-9) const
Check equality within a tolerance.
Definition: FundamentalMatrix.cpp:126
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:558
Eigen::JacobiSVD
Two-sided Jacobi SVD decomposition of a rectangular matrix.
Definition: ForwardDeclarations.h:278
gtsam::FundamentalMatrix::print
void print(const std::string &s="") const
Definition: FundamentalMatrix.cpp:77
gtsam::EssentialMatrix::matrix
const Matrix3 & matrix() const
Return 3*3 matrix representation.
Definition: EssentialMatrix.h:120
gtsam::EssentialMatrix::equals
bool equals(const EssentialMatrix &other, double tol=1e-8) const
assert equality up to a tolerance
Definition: EssentialMatrix.h:76
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::SimpleFundamentalMatrix
Class for representing a simple fundamental matrix.
Definition: FundamentalMatrix.h:128
FundamentalMatrix.h
gtsam::SimpleFundamentalMatrix::print
void print(const std::string &s="") const
Definition: FundamentalMatrix.cpp:119
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::scale
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Definition: Chebyshev.cpp:35
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::FundamentalMatrix::localCoordinates
Vector localCoordinates(const FundamentalMatrix &F) const
Return local coordinates with respect to another FundamentalMatrix.
Definition: FundamentalMatrix.cpp:87
gtsam::tol
const G double tol
Definition: Group.h:79
U
@ U
Definition: testDecisionTree.cpp:349
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
gtsam::SimpleFundamentalMatrix::Kb
Matrix3 Kb() const
Return the right calibration matrix.
Definition: FundamentalMatrix.cpp:109
abs
#define abs(x)
Definition: datatypes.h:17
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::Rot3::equals
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:99
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::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::FundamentalMatrix::retract
FundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new FundamentalMatrix.
Definition: FundamentalMatrix.cpp:95


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