gtsam 4.1.1
gtsam
InvDepthFactorVariant2.h
Go to the documentation of this file.
1
12#pragma once
13
20
21#include <boost/bind/bind.hpp>
22
23namespace gtsam {
24
28class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
29protected:
30
31 // Keep a copy of measurement and calibration for I/O
33 Cal3_S2::shared_ptr K_;
35
36public:
37
40
43
45 typedef boost::shared_ptr<This> shared_ptr;
46
49 measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
50 }
51
60 InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey,
61 const Point2& measured, const Cal3_S2::shared_ptr& K, const Point3 referencePoint,
62 const SharedNoiseModel& model) :
63 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), referencePoint_(referencePoint) {}
64
67
73 void print(const std::string& s = "InvDepthFactorVariant2",
74 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
75 Base::print(s, keyFormatter);
77 }
78
80 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
81 const This *e = dynamic_cast<const This*>(&p);
82 return e
83 && Base::equals(p, tol)
84 && traits<Point2>::Equals(this->measured_, e->measured_, tol)
85 && this->K_->equals(*e->K_, tol)
86 && traits<Point3>::Equals(this->referencePoint_, e->referencePoint_, tol);
87 }
88
89 Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
90 try {
91 // Calculate the 3D coordinates of the landmark in the world frame
92 double theta = landmark(0), phi = landmark(1), rho = landmark(2);
93 Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
94 // Project landmark into Pose2
95 PinholeCamera<Cal3_S2> camera(pose, *K_);
96 return camera.project(world_P_landmark) - measured_;
97 } catch( CheiralityException& e) {
98 std::cout << e.what()
99 << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
100 << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
101 << std::endl;
102 return Vector::Ones(2) * 2.0 * K_->fx();
103 }
104 return (Vector(1) << 0.0).finished();
105 }
106
108 Vector evaluateError(const Pose3& pose, const Vector3& landmark,
109 boost::optional<Matrix&> H1=boost::none,
110 boost::optional<Matrix&> H2=boost::none) const override {
111
112 if (H1) {
113 (*H1) = numericalDerivative11<Vector, Pose3>(
114 std::bind(&InvDepthFactorVariant2::inverseDepthError, this,
115 std::placeholders::_1, landmark), pose);
116 }
117 if (H2) {
118 (*H2) = numericalDerivative11<Vector, Vector3>(
119 std::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
120 std::placeholders::_1), landmark);
121 }
122
123 return inverseDepthError(pose, landmark);
124 }
125
127 const Point2& imagePoint() const {
128 return measured_;
129 }
130
132 const Cal3_S2::shared_ptr calibration() const {
133 return K_;
134 }
135
137 const Point3& referencePoint() const {
138 return referencePoint_;
139 }
140
141private:
142
145 template<class ARCHIVE>
146 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
147 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
148 ar & BOOST_SERIALIZATION_NVP(measured_);
149 ar & BOOST_SERIALIZATION_NVP(K_);
150 ar & BOOST_SERIALIZATION_NVP(referencePoint_);
151 }
152};
153
154} // \ namespace gtsam
Some functions to compute numerical derivatives.
The most common 5DOF 3D->2D calibration.
Base class for all pinhole cameras.
3D Pose
2D Point
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
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
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: PinholeCamera.h:33
Definition: Pose3.h:37
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 2 variables.
Definition: NonlinearFactor.h:369
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:401
Binary factor representing a visual measurement using an inverse-depth parameterization.
Definition: InvDepthFactorVariant2.h:28
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant2.h:33
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactorVariant2.h:127
Point3 referencePoint_
the reference point/origin for this landmark
Definition: InvDepthFactorVariant2.h:34
InvDepthFactorVariant2()
Default constructor.
Definition: InvDepthFactorVariant2.h:48
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant2.h:80
~InvDepthFactorVariant2() override
Virtual destructor.
Definition: InvDepthFactorVariant2.h:66
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactorVariant2.h:132
const Point3 & referencePoint() const
return the calibration object
Definition: InvDepthFactorVariant2.h:137
void print(const std::string &s="InvDepthFactorVariant2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InvDepthFactorVariant2.h:73
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant2.h:45
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactorVariant2.h:144
NoiseModelFactor2< Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant2.h:39
Vector evaluateError(const Pose3 &pose, const Vector3 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant2.h:108
InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const Point3 referencePoint, const SharedNoiseModel &model)
Constructor.
Definition: InvDepthFactorVariant2.h:60
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant2.h:32
InvDepthFactorVariant2 This
shorthand for this class
Definition: InvDepthFactorVariant2.h:42