gtsam 4.1.1
gtsam
Cal3.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
22#pragma once
23
25
26namespace gtsam {
27
46template <typename Cal, size_t Dim>
47void calibrateJacobians(const Cal& calibration, const Point2& pn,
48 OptionalJacobian<2, Dim> Dcal = boost::none,
49 OptionalJacobian<2, 2> Dp = boost::none) {
50 if (Dcal || Dp) {
51 Eigen::Matrix<double, 2, Dim> H_uncal_K;
52 Matrix22 H_uncal_pn, H_uncal_pn_inv;
53
54 // Compute uncalibrate Jacobians
55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
56
57 H_uncal_pn_inv = H_uncal_pn.inverse();
58
59 if (Dp) *Dp = H_uncal_pn_inv;
60 if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
61 }
62}
63
69class GTSAM_EXPORT Cal3 {
70 protected:
71 double fx_ = 1.0f, fy_ = 1.0f;
72 double s_ = 0.0f;
73 double u0_ = 0.0f, v0_ = 0.0f;
74
75 public:
76 enum { dimension = 5 };
78 using shared_ptr = boost::shared_ptr<Cal3>;
79
82
84 Cal3() = default;
85
87 Cal3(double fx, double fy, double s, double u0, double v0)
88 : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
89
91 Cal3(const Vector5& d)
92 : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
93
100 Cal3(double fov, int w, int h);
101
103 virtual ~Cal3() {}
104
108
118 Cal3(const std::string& path);
119
123
125 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
126 const Cal3& cal);
127
129 virtual void print(const std::string& s = "") const;
130
132 bool equals(const Cal3& K, double tol = 10e-9) const;
133
137
139 inline double fx() const { return fx_; }
140
142 inline double fy() const { return fy_; }
143
145 inline double aspectRatio() const { return fx_ / fy_; }
146
148 inline double skew() const { return s_; }
149
151 inline double px() const { return u0_; }
152
154 inline double py() const { return v0_; }
155
157 Point2 principalPoint() const { return Point2(u0_, v0_); }
158
160 Vector5 vector() const {
161 Vector5 v;
162 v << fx_, fy_, s_, u0_, v0_;
163 return v;
164 }
165
167 virtual Matrix3 K() const {
168 Matrix3 K;
169 K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
170 return K;
171 }
172
173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
175 Matrix3 matrix() const { return K(); }
176#endif
177
179 Matrix3 inverse() const;
180
182 inline virtual size_t dim() const { return Dim(); }
183
185 inline static size_t Dim() { return dimension; }
186
190
191 private:
193 friend class boost::serialization::access;
194 template <class Archive>
195 void serialize(Archive& ar, const unsigned int /*version*/) {
196 ar& BOOST_SERIALIZATION_NVP(fx_);
197 ar& BOOST_SERIALIZATION_NVP(fy_);
198 ar& BOOST_SERIALIZATION_NVP(s_);
199 ar& BOOST_SERIALIZATION_NVP(u0_);
200 ar& BOOST_SERIALIZATION_NVP(v0_);
201 }
202
204};
205
206} // \ namespace gtsam
2D Point
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
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
void calibrateJacobians(const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using...
Definition: Cal3.h:47
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Template to create a binary predicate.
Definition: Testable.h:111
Definition: Cal3.h:69
Cal3(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition: Cal3.h:87
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Cal3(const Vector5 &d)
constructor from vector
Definition: Cal3.h:91
virtual ~Cal3()
Virtual destructor.
Definition: Cal3.h:103
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
Cal3()=default
Create a default calibration that leaves coordinates unchanged.
double px() const
image center in x
Definition: Cal3.h:151
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3.h:185
Point2 principalPoint() const
return the principal point
Definition: Cal3.h:157
double fx() const
focal length x
Definition: Cal3.h:139
double py() const
image center in y
Definition: Cal3.h:154
double skew() const
skew
Definition: Cal3.h:148
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition: Cal3.h:182
double aspectRatio() const
aspect ratio
Definition: Cal3.h:145
double fy() const
focal length y
Definition: Cal3.h:142