Go to the documentation of this file.
26 template<
class POSE,
class LANDMARK,
class INVDEPTH>
32 std::shared_ptr<Cal3_S2>
K_;
37 typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH>
Base;
40 using Base::evaluateError;
75 void print(
const std::string&
s =
"InvDepthFactor3",
94 if (H1) *H1 = Matrix::Zero(2,6);
95 if (H2) *H2 = Matrix::Zero(2,5);
96 if (H3) *H3 = Matrix::Zero(2,1);
99 return Vector::Ones(2) * 2.0 *
K_->fx();
101 return (
Vector(1) << 0.0).finished();
116 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
117 friend class boost::serialization::access;
119 template<
class ARCHIVE>
120 void serialize(ARCHIVE & ar,
const unsigned int ) {
121 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
123 ar & BOOST_SERIALIZATION_NVP(
K_);
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
const Cal3_S2::shared_ptr calibration() const
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
const gtsam::Key pointKey
Vector evaluateError(const POSE &pose, const Vector5 &point, const INVDEPTH &invDepth, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
The most common 5DOF 3D->2D calibration.
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
InvDepthFactor3< POSE, LANDMARK, INVDEPTH > This
shorthand for this class
NoiseModelFactor3< POSE, LANDMARK, INVDEPTH > Base
shorthand for base class type
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
~InvDepthFactor3() override
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Inverse Depth Camera based on Civera09tro, Montiel06rss. Landmarks are initialized from the first cam...
noiseModel::Base::shared_ptr SharedNoiseModel
std::shared_ptr< Cal3_S2 > shared_ptr
noiseModel::Diagonal::shared_ptr model
Non-linear factor base classes.
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
InvDepthFactor3()
Default constructor.
const Point2 & imagePoint() const
The most common 5DOF 3D->2D calibration.
Matrix * OptionalMatrixType
static const CalibratedCamera camera(kDefaultPose)
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
std::uint64_t Key
Integer nonlinear key type.
std::shared_ptr< Cal3_S2 > K_
shared pointer to calibration object
Point2 measured_
2D measurement
void print(const std::string &s="InvDepthFactor3", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
InvDepthFactor3(const Point2 &measured, const SharedNoiseModel &model, const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr &K)
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:28