gtsam 4.1.1
gtsam
InvDepthFactor3.h
Go to the documentation of this file.
1
15#pragma once
16
19#include <gtsam_unstable/geometry/InvDepthCamera3.h>
20
21namespace gtsam {
22
26template<class POSE, class LANDMARK, class INVDEPTH>
27class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
28protected:
29
30 // Keep a copy of measurement and calibration for I/O
32 boost::shared_ptr<Cal3_S2> K_;
33
34public:
35
38
41
43 typedef boost::shared_ptr<This> shared_ptr;
44
47 measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
48 }
49
60 InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
61 const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
62 Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
63
65 ~InvDepthFactor3() override {}
66
72 void print(const std::string& s = "InvDepthFactor3",
73 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
74 Base::print(s, keyFormatter);
76 }
77
79 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
80 const This *e = dynamic_cast<const This*>(&p);
81 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
82 }
83
85 Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
86 boost::optional<Matrix&> H1=boost::none,
87 boost::optional<Matrix&> H2=boost::none,
88 boost::optional<Matrix&> H3=boost::none) const override {
89 try {
90 InvDepthCamera3<Cal3_S2> camera(pose, K_);
91 return camera.project(point, invDepth, H1, H2, H3) - measured_;
92 } catch( CheiralityException& e) {
93 if (H1) *H1 = Matrix::Zero(2,6);
94 if (H2) *H2 = Matrix::Zero(2,5);
95 if (H3) *H2 = Matrix::Zero(2,1);
96 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
97 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
98 return Vector::Ones(2) * 2.0 * K_->fx();
99 }
100 return (Vector(1) << 0.0).finished();
101 }
102
104 const Point2& imagePoint() const {
105 return measured_;
106 }
107
109 inline const Cal3_S2::shared_ptr calibration() const {
110 return K_;
111 }
112
113private:
114
117 template<class ARCHIVE>
118 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
119 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
120 ar & BOOST_SERIALIZATION_NVP(measured_);
121 ar & BOOST_SERIALIZATION_NVP(K_);
122 }
123};
124} // \ namespace gtsam
The most common 5DOF 3D->2D calibration.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Definition: Cal3_S2.h:34
Definition: CalibratedCamera.h:32
This is the base class for all factor types.
Definition: Factor.h:56
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:444
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:478
Ternary factor representing a visual measurement that includes inverse depth.
Definition: InvDepthFactor3.h:27
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactor3.h:79
boost::shared_ptr< Cal3_S2 > K_
shared pointer to calibration object
Definition: InvDepthFactor3.h:32
void print(const std::string &s="InvDepthFactor3", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InvDepthFactor3.h:72
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactor3.h:104
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactor3.h:43
NoiseModelFactor3< POSE, LANDMARK, INVDEPTH > Base
shorthand for base class type
Definition: InvDepthFactor3.h:37
Vector evaluateError(const POSE &pose, const Vector5 &point, const INVDEPTH &invDepth, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactor3.h:85
InvDepthFactor3< POSE, LANDMARK, INVDEPTH > This
shorthand for this class
Definition: InvDepthFactor3.h:40
InvDepthFactor3(const Point2 &measured, const SharedNoiseModel &model, const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr &K)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: InvDepthFactor3.h:60
Point2 measured_
2D measurement
Definition: InvDepthFactor3.h:31
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactor3.h:109
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactor3.h:116
InvDepthFactor3()
Default constructor.
Definition: InvDepthFactor3.h:46
~InvDepthFactor3() override
Virtual destructor.
Definition: InvDepthFactor3.h:65