Go to the documentation of this file.
35 typedef NoiseModelFactor2<Pose3, Vector6>
Base;
38 using Base::evaluateError;
71 void print(
const std::string&
s =
"InvDepthFactorVariant1",
81 && Base::equals(
p,
tol)
83 && this->K_->equals(*
e->K_,
tol);
100 return Vector::Ones(2) * 2.0 *
K_->fx();
102 return (
Vector(1) << 0.0).finished();
110 (*H1) = numericalDerivative11<Vector, Pose3>(
116 (*H2) = numericalDerivative11<Vector, Vector6>(
136 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
137 friend class boost::serialization::access;
139 template<
class ARCHIVE>
140 void serialize(ARCHIVE & ar,
const unsigned int ) {
141 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
143 ar & BOOST_SERIALIZATION_NVP(
K_);
const Cal3_S2::shared_ptr calibration() const
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Jet< T, N > sin(const Jet< T, N > &f)
The most common 5DOF 3D->2D calibration.
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 x
Jet< T, N > cos(const Jet< T, N > &f)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Some functions to compute numerical derivatives.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
InvDepthFactorVariant1(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const SharedNoiseModel &model)
Vector evaluateError(const Pose3 &pose, const Vector6 &landmark, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error h(x)-z and optionally derivatives.
~InvDepthFactorVariant1() override
static Point3 landmark(0, 0, 5)
Base class for all pinhole cameras.
noiseModel::Base::shared_ptr SharedNoiseModel
std::shared_ptr< Cal3_S2 > shared_ptr
InvDepthFactorVariant1 This
shorthand for this class
void print(const std::string &s="InvDepthFactorVariant1", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
noiseModel::Diagonal::shared_ptr model
Non-linear factor base classes.
Vector inverseDepthError(const Pose3 &pose, const Vector6 &landmark) const
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
const Point2 & imagePoint() const
NoiseModelFactor2< Pose3, Vector6 > Base
shorthand for base class type
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
The most common 5DOF 3D->2D calibration.
Matrix * OptionalMatrixType
static const CalibratedCamera camera(kDefaultPose)
InvDepthFactorVariant1()
Default constructor.
std::uint64_t Key
Integer nonlinear key type.
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Point2 measured_
2D measurement
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:42