Go to the documentation of this file.
32 template<
class POSE,
class LANDMARK,
class CALIBRATION = Cal3_S2>
38 std::shared_ptr<CALIBRATION>
K_;
47 typedef NoiseModelFactor3<POSE, POSE, LANDMARK>
Base;
50 using Base::evaluateError;
75 const std::shared_ptr<CALIBRATION>&
K) :
92 const std::shared_ptr<CALIBRATION>&
K,
102 return std::static_pointer_cast<NonlinearFactor>(
111 std::cout <<
s <<
"ProjectionFactorPPP, z = ";
118 const This *
e =
dynamic_cast<const This*
>(&
p);
120 && Base::equals(
p,
tol)
122 && this->K_->equals(*
e->K_,
tol);
135 return reprojectionError;
141 if (H1) *H1 = Matrix::Zero(2,6);
142 if (H2) *H2 = Matrix::Zero(2,6);
143 if (H3) *H3 = Matrix::Zero(2,3);
145 std::cout <<
e.what() <<
": Landmark "
147 <<
" moved behind camera "
153 return Vector::Ones(2) * 2.0 *
K_->fx();
174 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
175 friend class boost::serialization::access;
177 template<
class ARCHIVE>
178 void serialize(ARCHIVE & ar,
const unsigned int ) {
179 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
181 ar & BOOST_SERIALIZATION_NVP(
K_);
189 template<
class POSE,
class LANDMARK,
class CALIBRATION>
191 public Testable<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > {
std::shared_ptr< This > shared_ptr
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
const gtsam::Key pointKey
~ProjectionFactorPPP() override
Array< double, 1, 3 > e(1./3., 0.5, 2.)
The most common 5DOF 3D->2D calibration.
ProjectionFactorPPP(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const std::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality)
const Point2 & measured() const
NonlinearFactor::shared_ptr clone() const override
bool throwCheirality() const
Vector evaluateError(const Pose3 &pose, const Pose3 &transform, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
ProjectionFactorPPP()
Default constructor.
ProjectionFactorPPP(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const std::shared_ptr< CALIBRATION > &K)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Point2 measured_
2D measurement
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
NoiseModelFactor3< POSE, POSE, LANDMARK > Base
shorthand for base class type
std::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Base class for all pinhole cameras.
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Non-linear factor base classes.
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
const std::shared_ptr< CALIBRATION > calibration() const
bool verboseCheirality() const
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Matrix * OptionalMatrixType
static const CalibratedCamera camera(kDefaultPose)
std::uint64_t Key
Integer nonlinear key type.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:49