gtsam 4.2.0
gtsam
Cal3Fisheye.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
20#pragma once
21
22#include <gtsam/geometry/Cal3.h>
24
25#include <boost/shared_ptr.hpp>
26
27#include <string>
28
29namespace gtsam {
30
51class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
52 private:
53 double k1_ = 0.0f, k2_ = 0.0f;
54 double k3_ = 0.0f, k4_ = 0.0f;
55 double tol_ = 1e-5;
56
57 public:
58 enum { dimension = 9 };
60 using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
61
64
66 Cal3Fisheye() = default;
67
68 Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
69 const double v0, const double k1, const double k2,
70 const double k3, const double k4, double tol = 1e-5)
71 : Cal3(fx, fy, s, u0, v0),
72 k1_(k1),
73 k2_(k2),
74 k3_(k3),
75 k4_(k4),
76 tol_(tol) {}
77
78 ~Cal3Fisheye() override {}
79
83
84 explicit Cal3Fisheye(const Vector9& v)
85 : Cal3(v(0), v(1), v(2), v(3), v(4)),
86 k1_(v(5)),
87 k2_(v(6)),
88 k3_(v(7)),
89 k4_(v(8)) {}
90
94
96 inline double k1() const { return k1_; }
97
99 inline double k2() const { return k2_; }
100
102 inline double k3() const { return k3_; }
103
105 inline double k4() const { return k4_; }
106
108 Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
109
111 Vector9 vector() const;
112
114 static double Scaling(double r);
115
124 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
125 OptionalJacobian<2, 2> Dp = boost::none) const;
126
135 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
136 OptionalJacobian<2, 2> Dp = boost::none) const;
137
141
143 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
144 const Cal3Fisheye& cal);
145
147 void print(const std::string& s = "") const override;
148
150 bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
151
155
157 size_t dim() const override { return Dim(); }
158
160 inline static size_t Dim() { return dimension; }
161
163 inline Cal3Fisheye retract(const Vector& d) const {
164 return Cal3Fisheye(vector() + d);
165 }
166
168 Vector localCoordinates(const Cal3Fisheye& T2) const {
169 return T2.vector() - vector();
170 }
171
175
177 virtual boost::shared_ptr<Cal3Fisheye> clone() const {
178 return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
179 }
180
182
183 private:
186
188 friend class boost::serialization::access;
189 template <class Archive>
190 void serialize(Archive& ar, const unsigned int /*version*/) {
191 ar& boost::serialization::make_nvp(
192 "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
193 ar& BOOST_SERIALIZATION_NVP(k1_);
194 ar& BOOST_SERIALIZATION_NVP(k2_);
195 ar& BOOST_SERIALIZATION_NVP(k3_);
196 ar& BOOST_SERIALIZATION_NVP(k4_);
197 }
198
200};
201
202template <>
203struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
204
205template <>
206struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
207
208} // namespace gtsam
Common code for all Calibration models.
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:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Template to create a binary predicate.
Definition: Testable.h:111
Common base class for all calibration models.
Definition: Cal3.h:69
Calibration of a fisheye camera.
Definition: Cal3Fisheye.h:51
double k4() const
Second tangential distortion coefficient.
Definition: Cal3Fisheye.h:105
double k1() const
First distortion coefficient.
Definition: Cal3Fisheye.h:96
double k3() const
First tangential distortion coefficient.
Definition: Cal3Fisheye.h:102
Vector localCoordinates(const Cal3Fisheye &T2) const
Given a different calibration, calculate update to obtain it.
Definition: Cal3Fisheye.h:168
static size_t Dim()
Return dimensions of calibration manifold object.
Definition: Cal3Fisheye.h:160
double k2() const
Second distortion coefficient.
Definition: Cal3Fisheye.h:99
size_t dim() const override
Return dimensions of calibration manifold object.
Definition: Cal3Fisheye.h:157
Vector9 vector() const
Return all parameters as a vector.
Definition: Cal3Fisheye.cpp:28
virtual boost::shared_ptr< Cal3Fisheye > clone() const
Definition: Cal3Fisheye.h:177
Vector4 k() const
return distortion parameter vector
Definition: Cal3Fisheye.h:108
Cal3Fisheye()=default
Default Constructor with only unit focal length.
Cal3Fisheye retract(const Vector &d) const
Given delta vector, update calibration.
Definition: Cal3Fisheye.h:163