Go to the documentation of this file.
16 const Matrix3& Fcb,
const Point2& pb) {
22 Vector3 intersectionPoint = line_a.cross(line_b);
25 intersectionPoint /= intersectionPoint(2);
27 return intersectionPoint.head<2>();
41 Matrix3
U =
svd.matrixU();
42 Matrix3
V =
svd.matrixV();
46 double scale = singularValues(0);
48 singularValues /=
scale;
53 throw std::invalid_argument(
54 "The input matrix does not represent a valid fundamental matrix.");
62 if (
U.determinant() < 0)
U.col(2) *= -1;
65 if (
V.determinant() < 0)
V.col(2) *= -1;
84 throw std::runtime_error(
85 "FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
92 std::cout <<
s <<
matrix() << std::endl;
111 const double newS =
s_ +
delta(3);
130 return Ka().transpose().inverse() *
E_.
matrix() *
Kb().inverse();
140 throw std::runtime_error(
141 "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
148 std::cout <<
s <<
" E:\n"
150 <<
"\nca: " <<
ca_.transpose() <<
"\ncb: " <<
cb_.transpose()
double fa_
Focal length for left camera.
double s_
Scalar parameter for S.
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
bool equals(const FundamentalMatrix &other, double tol=1e-9) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
void initialize(Matrix3 U, double s, Matrix3 V)
Initialize SO(3) matrices from general O(3) matrices.
typedef and functions to augment Eigen's MatrixXd
Vector localCoordinates(const SimpleFundamentalMatrix &F) const
Return local coordinates with respect to another SimpleFundamentalMatrix.
Vector3 epipolarLine(const Point2 &p, OptionalJacobian< 3, 7 > H={})
Computes the epipolar line in a (left) for a given point in b (right)
Point2 ca_
Principal point for left camera.
Point2 cb_
Principal point for right camera.
Point2 EpipolarTransfer(const Matrix3 &Fca, const Point2 &pa, const Matrix3 &Fcb, const Point2 &pb)
Transfer projections from cameras a and b to camera c.
EssentialMatrix retract(const Vector5 &xi) const
Retract delta to manifold.
Vector3 epipolarLine(const Point2 &p, OptionalJacobian< 3, 7 > H={})
Computes the epipolar line in a (left) for a given point in b (right)
Vector5 localCoordinates(const EssentialMatrix &other) const
Compute the coordinates in the tangent space.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
SimpleFundamentalMatrix()
Default constructor.
Matrix3 matrix() const
Return the fundamental matrix representation.
Matrix3 Ka() const
Return the left calibration matrix.
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
SimpleFundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new SimpleFundamentalMatrix.
bool equals(const SimpleFundamentalMatrix &other, double tol=1e-9) const
Check equality within a tolerance.
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Two-sided Jacobi SVD decomposition of a rectangular matrix.
void print(const std::string &s="") const
const Matrix3 & matrix() const
Return 3*3 matrix representation.
bool equals(const EssentialMatrix &other, double tol=1e-8) const
assert equality up to a tolerance
Class for representing a simple fundamental matrix.
void print(const std::string &s="") const
Matrix3 transpose() const
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Vector localCoordinates(const FundamentalMatrix &F) const
Return local coordinates with respect to another FundamentalMatrix.
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Matrix3 Kb() const
Return the right calibration matrix.
FundamentalMatrix()
Default constructor.
Represents a fundamental matrix in computer vision, which encodes the epipolar geometry between two v...
bool equals(const Rot3 &p, double tol=1e-9) const
EssentialMatrix E_
Essential matrix.
double fb_
Focal length for right camera.
FundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new FundamentalMatrix.
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:33