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 #if defined(__GNUC__) && !defined(__clang__)
13 #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
14 #endif
15 
16 namespace gtsam {
17 
18 //*************************************************************************
19 Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, //
20  const Matrix3& Fcb, const Point2& pb) {
21  // Create lines in camera a from projections of the other two cameras
22  Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1);
23  Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1);
24 
25  // Cross the lines to find the intersection point
26  Vector3 intersectionPoint = line_a.cross(line_b);
27 
28  // Normalize the intersection point
29  intersectionPoint /= intersectionPoint(2);
30 
31  return intersectionPoint.head<2>(); // Return the 2D point
32 }
33 
34 //*************************************************************************
35 FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s,
36  const Matrix3& V) {
37  initialize(U, s, V);
38 }
39 
41  // Perform SVD
43 
44  // Extract U and V
45  Matrix3 U = svd.matrixU();
46  Matrix3 V = svd.matrixV();
47  Vector3 singularValues = svd.singularValues();
48 
49  // Scale the singular values
50  double scale = singularValues(0);
51  if (scale != 0) {
52  singularValues /= scale; // Normalize the first singular value to 1.0
53  }
54 
55  // Check if the third singular value is close to zero (valid F condition)
56  if (std::abs(singularValues(2)) > 1e-9) {
57  throw std::invalid_argument(
58  "The input matrix does not represent a valid fundamental matrix.");
59  }
60 
61  initialize(U, singularValues(1), V);
62 }
63 
64 void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
65  // Check if U is a reflection and flip third column if so
66  if (U.determinant() < 0) U.col(2) *= -1;
67 
68  // Same check for V
69  if (V.determinant() < 0) V.col(2) *= -1;
70 
71  U_ = Rot3(U);
72  s_ = s;
73  V_ = Rot3(V);
74 }
75 
76 Matrix3 FundamentalMatrix::matrix() const {
77  return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
78  V_.transpose().matrix();
79 }
80 
83  Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates
84  Vector3 line = matrix() * point; // Compute the epipolar line
85 
86  if (H) {
87  // Compute the Jacobian if requested
88  throw std::runtime_error(
89  "FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
90  }
91 
92  return line; // Return the epipolar line
93 }
94 
95 void FundamentalMatrix::print(const std::string& s) const {
96  std::cout << s << matrix() << std::endl;
97 }
98 
100  double tol) const {
101  return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
102  V_.equals(other.V_, tol);
103 }
104 
106  Vector result(7);
107  result.head<3>() = U_.localCoordinates(F.U_);
108  result(3) = F.s_ - s_; // Difference in scalar
109  result.tail<3>() = V_.localCoordinates(F.V_);
110  return result;
111 }
112 
114  const Rot3 newU = U_.retract(delta.head<3>());
115  const double newS = s_ + delta(3);
116  const Rot3 newV = V_.retract(delta.tail<3>());
117  return FundamentalMatrix(newU, newS, newV);
118 }
119 
120 //*************************************************************************
122  Matrix3 K;
123  K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
124  return K;
125 }
126 
128  Matrix3 K;
129  K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
130  return K;
131 }
132 
134  return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
135 }
136 
139  Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates
140  Vector3 line = matrix() * point; // Compute the epipolar line
141 
142  if (H) {
143  // Compute the Jacobian if requested
144  throw std::runtime_error(
145  "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
146  }
147 
148  return line; // Return the epipolar line
149 }
150 
151 void SimpleFundamentalMatrix::print(const std::string& s) const {
152  std::cout << s << " E:\n"
153  << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
154  << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose()
155  << std::endl;
156 }
157 
159  double tol) const {
160  return E_.equals(other.E_, tol) && std::abs(fa_ - other.fa_) < tol &&
161  std::abs(fb_ - other.fb_) < tol && (ca_ - other.ca_).norm() < tol &&
162  (cb_ - other.cb_).norm() < tol;
163 }
164 
166  const SimpleFundamentalMatrix& F) const {
167  Vector result(7);
168  result.head<5>() = E_.localCoordinates(F.E_);
169  result(5) = F.fa_ - fa_; // Difference in fa
170  result(6) = F.fb_ - fb_; // Difference in fb
171  return result;
172 }
173 
175  const Vector& delta) const {
176  EssentialMatrix newE = E_.retract(delta.head<5>());
177  double newFa = fa_ + delta(5); // Update fa
178  double newFb = fb_ + delta(6); // Update fb
179  return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_);
180 }
181 
182 //*************************************************************************
183 
184 } // namespace gtsam
gtsam::SimpleFundamentalMatrix::fa_
double fa_
Focal length for left camera.
Definition: FundamentalMatrix.h:135
gtsam::FundamentalMatrix::s_
double s_
Scalar parameter for S.
Definition: FundamentalMatrix.h:32
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::FundamentalMatrix::equals
bool equals(const FundamentalMatrix &other, double tol=1e-9) const
Definition: FundamentalMatrix.cpp:99
Eigen::ComputeFullV
@ ComputeFullV
Definition: Constants.h:397
gtsam::FundamentalMatrix::U_
Rot3 U_
Left rotation.
Definition: FundamentalMatrix.h:31
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:133
gtsam::FundamentalMatrix::initialize
void initialize(Matrix3 U, double s, Matrix3 V)
Initialize SO(3) matrices from general O(3) matrices.
Definition: FundamentalMatrix.cpp:64
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:165
gtsam::SimpleFundamentalMatrix::epipolarLine
Vector3 epipolarLine(const Point2 &p, OptionalJacobian< 3, 7 > H={})
Computes the epipolar line in a (left) for a given point in b (right)
Definition: FundamentalMatrix.cpp:137
gtsam::SimpleFundamentalMatrix::ca_
Point2 ca_
Principal point for left camera.
Definition: FundamentalMatrix.h:137
Eigen::ComputeFullU
@ ComputeFullU
Definition: Constants.h:393
gtsam::SimpleFundamentalMatrix::cb_
Point2 cb_
Principal point for right camera.
Definition: FundamentalMatrix.h:138
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
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:19
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
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::FundamentalMatrix::epipolarLine
Vector3 epipolarLine(const Point2 &p, OptionalJacobian< 3, 7 > H={})
Computes the epipolar line in a (left) for a given point in b (right)
Definition: FundamentalMatrix.cpp:81
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
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:148
gtsam::FundamentalMatrix::matrix
Matrix3 matrix() const
Return the fundamental matrix representation.
Definition: FundamentalMatrix.cpp:76
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SimpleFundamentalMatrix::Ka
Matrix3 Ka() const
Return the left calibration matrix.
Definition: FundamentalMatrix.cpp:121
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:174
gtsam::SimpleFundamentalMatrix::equals
bool equals(const SimpleFundamentalMatrix &other, double tol=1e-9) const
Check equality within a tolerance.
Definition: FundamentalMatrix.cpp:158
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:548
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:95
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:132
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
FundamentalMatrix.h
gtsam::SimpleFundamentalMatrix::print
void print(const std::string &s="") const
Definition: FundamentalMatrix.cpp:151
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:105
gtsam::tol
const G double tol
Definition: Group.h:79
U
@ U
Definition: testDecisionTree.cpp:342
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:127
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::FundamentalMatrix::FundamentalMatrix
FundamentalMatrix()
Default constructor.
Definition: FundamentalMatrix.h:37
gtsam::FundamentalMatrix
Represents a fundamental matrix in computer vision, which encodes the epipolar geometry between two v...
Definition: FundamentalMatrix.h:29
gtsam::Rot3::equals
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:100
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::SimpleFundamentalMatrix::E_
EssentialMatrix E_
Essential matrix.
Definition: FundamentalMatrix.h:134
gtsam::SimpleFundamentalMatrix::fb_
double fb_
Focal length for right camera.
Definition: FundamentalMatrix.h:136
gtsam::FundamentalMatrix::V_
Rot3 V_
Right rotation.
Definition: FundamentalMatrix.h:33
gtsam::FundamentalMatrix::retract
FundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new FundamentalMatrix.
Definition: FundamentalMatrix.cpp:113


gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:01:23