Go to the documentation of this file.
69 template <
class CALIBRATION>
72 std::shared_ptr<CALIBRATION>
K)
81 return std::static_pointer_cast<gtsam::NonlinearFactor>(
87 const std::string&
s =
"",
90 std::cout <<
" EssentialMatrixFactor with measurements\n ("
91 <<
vA_.transpose() <<
")' and (" <<
vB_.transpose() <<
")'"
150 template <
class CALIBRATION>
153 std::shared_ptr<CALIBRATION>
K)
157 f_ = 0.5 * (
K->fx() +
K->fy());
162 return std::static_pointer_cast<gtsam::NonlinearFactor>(
168 const std::string&
s =
"",
171 std::cout <<
" EssentialMatrixFactor2 with measurements\n ("
172 <<
dP1_.transpose() <<
")' and (" <<
pn_.transpose() <<
")'"
196 Point3 _1T2 =
E.direction().point3();
204 Matrix D_1T2_dir, DdP2_rot, DP2_point;
206 Point3 _1T2 =
E.direction().point3(D_1T2_dir);
208 Point3 dP2 =
E.rotation().unrotate(
dP1_ - d1T2, DdP2_rot, DP2_point);
215 DdP2_E << DdP2_rot, -DP2_point *
d * D_1T2_dir;
216 *DE =
f_ * Dpn_dP2 * DdP2_E;
221 *Dd = -
f_ * (Dpn_dP2 * (DP2_point * _1T2));
224 return f_ * reprojectionError;
270 template <
class CALIBRATION>
273 std::shared_ptr<CALIBRATION>
K)
278 return std::static_pointer_cast<gtsam::NonlinearFactor>(
284 const std::string&
s =
"",
287 std::cout <<
" EssentialMatrixFactor3 with rotation " <<
cRb_ << std::endl;
305 Matrix D_e_cameraE, D_cameraE_E;
310 *DE = D_e_cameraE * D_cameraE_E;
333 template <
class CALIBRATION>
365 return std::static_pointer_cast<gtsam::NonlinearFactor>(
371 const std::string&
s =
"",
374 std::cout <<
" EssentialMatrixFactor4 with measurements\n ("
375 <<
pA_.transpose() <<
")' and (" <<
pB_.transpose() <<
")'"
408 *H2 =
vB.transpose() *
E.matrix().transpose().leftCols<2>() * cA_H_K +
409 vA.transpose() *
E.matrix().leftCols<2>() * cB_H_K;
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
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
std::shared_ptr< This > shared_ptr
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
static constexpr int DimK
Vector evaluateError(const EssentialMatrix &E, OptionalMatrixType H) const override
vector of errors returns 1D vector
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
double error(const Values &c) const override
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::NonlinearFactor::shared_ptr clone() const override
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Rot3 cRb_
Rotation from body to camera frame.
EssentialMatrixFactor3 This
Point2 pn_
Measurement in image 2, in ideal coordinates.
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
gtsam::NonlinearFactor::shared_ptr clone() const override
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Base class for all pinhole cameras.
NoiseModelFactorN< EssentialMatrix, double > Base
noiseModel::Base::shared_ptr SharedNoiseModel
EssentialMatrixFactor2 Base
NoiseModelFactorN< EssentialMatrix > Base
noiseModel::Diagonal::shared_ptr model
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint={})
Non-linear factor base classes.
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
NoiseModelFactorN< EssentialMatrix, CALIBRATION > Base
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Eigen::Matrix< double, 2, DimK > JacobianCalibration
gtsam::NonlinearFactor::shared_ptr clone() const override
double f_
approximate conversion factor for error scaling
gtsam::NonlinearFactor::shared_ptr clone() const override
EssentialMatrixFactor This
Point2 pB_
points in pixel coordinates
Matrix * OptionalMatrixType
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
The matrix class, also used for vectors and row-vectors.
Give fixed size dimension of a type, fails at compile time if dynamic.
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, OptionalMatrixType H1, OptionalMatrixType H2) const override
Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
EssentialMatrixFactor2 This
std::uint64_t Key
Integer nonlinear key type.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Vector3 vB_
Homogeneous versions, in ideal coordinates.
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
EssentialMatrixFactor4 This
Point3 dP1_
3D point corresponding to measurement in image 1
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:30