45 measured_(0.0, 0.0), K_(new
Cal3_S2(444, 555, 666, 777, 888)) {
58 Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {}
68 void print(
const std::string&
s =
"InvDepthFactorVariant1",
76 const This *
e =
dynamic_cast<const This*
>(&
p);
80 && this->K_->equals(*e->K_,
tol);
97 return Vector::Ones(2) * 2.0 * K_->fx();
99 return (
Vector(1) << 0.0).finished();
104 boost::optional<Matrix&> H1=boost::none,
105 boost::optional<Matrix&> H2=boost::none)
const override {
108 (*H1) = numericalDerivative11<Vector, Pose3>(
113 (*H2) = numericalDerivative11<Vector, Vector6>(
135 template<
class ARCHIVE>
137 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
138 ar & BOOST_SERIALIZATION_NVP(measured_);
139 ar & BOOST_SERIALIZATION_NVP(K_);
Vector evaluateError(const Pose3 &pose, const Vector6 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
~InvDepthFactorVariant1() override
InvDepthFactorVariant1()
Default constructor.
noiseModel::Diagonal::shared_ptr model
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Point2 measured_
2D measurement
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
void print(const std::string &s="InvDepthFactorVariant1", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
void serialize(ARCHIVE &ar, const unsigned int)
Some functions to compute numerical derivatives.
friend class boost::serialization::access
Serialization function.
static const KeyFormatter DefaultKeyFormatter
const Point2 & imagePoint() const
static Point3 landmark(0, 0, 5)
Base class for all pinhole cameras.
EIGEN_DEVICE_FUNC const CosReturnType cos() const
InvDepthFactorVariant1 This
shorthand for this class
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
NoiseModelFactor2< Pose3, Vector6 > Base
shorthand for base class type
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Non-linear factor base classes.
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
const Cal3_S2::shared_ptr calibration() const
EIGEN_DEVICE_FUNC const SinReturnType sin() const
static const CalibratedCamera camera(kDefaultPose)
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
InvDepthFactorVariant1(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const SharedNoiseModel &model)
boost::shared_ptr< Cal3_S2 > shared_ptr
std::uint64_t Key
Integer nonlinear key type.
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
noiseModel::Base::shared_ptr SharedNoiseModel
The most common 5DOF 3D->2D calibration.
const char * what() const noexceptoverride
Vector inverseDepthError(const Pose3 &pose, const Vector6 &landmark) const