gtsam 4.2.0
gtsam
Cal3DS2_Base.h
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#include <boost/shared_ptr.hpp>
25
26namespace gtsam {
27
42class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
43 protected:
44 double k1_ = 0.0f, k2_ = 0.0f;
45 double p1_ = 0.0f, p2_ = 0.0f;
46 double tol_ = 1e-5;
47
48 public:
49 enum { dimension = 9 };
50
52 using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
53
56
58 Cal3DS2_Base() = default;
59
60 Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
61 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
62 : Cal3(fx, fy, s, u0, v0),
63 k1_(k1),
64 k2_(k2),
65 p1_(p1),
66 p2_(p2),
67 tol_(tol) {}
68
69 ~Cal3DS2_Base() override {}
70
74
75 Cal3DS2_Base(const Vector9& v)
76 : Cal3(v(0), v(1), v(2), v(3), v(4)),
77 k1_(v(5)),
78 k2_(v(6)),
79 p1_(v(7)),
80 p2_(v(8)) {}
81
85
87 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
88 const Cal3DS2_Base& cal);
89
91 void print(const std::string& s = "") const override;
92
94 bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
95
99
101 inline double k1() const { return k1_; }
102
104 inline double k2() const { return k2_; }
105
107 inline double p1() const { return p1_; }
108
110 inline double p2() const { return p2_; }
111
113 Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
114
116 Vector9 vector() const;
117
125 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
126 OptionalJacobian<2, 2> Dp = boost::none) const;
127
129 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
130 OptionalJacobian<2, 2> Dp = boost::none) const;
131
133 Matrix2 D2d_intrinsic(const Point2& p) const;
134
136 Matrix29 D2d_calibration(const Point2& p) const;
137
139 size_t dim() const override { return Dim(); }
140
142 inline static size_t Dim() { return dimension; }
143
147
149 virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
150 return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
151 }
152
154
155 private:
158
160 friend class boost::serialization::access;
161 template <class Archive>
162 void serialize(Archive& ar, const unsigned int /*version*/) {
163 ar& boost::serialization::make_nvp(
164 "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
165 ar& BOOST_SERIALIZATION_NVP(k1_);
166 ar& BOOST_SERIALIZATION_NVP(k2_);
167 ar& BOOST_SERIALIZATION_NVP(p1_);
168 ar& BOOST_SERIALIZATION_NVP(p2_);
169 ar& BOOST_SERIALIZATION_NVP(tol_);
170 }
171
173};
174}
2D Point
Common code for all Calibration models.
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
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Common base class for all calibration models.
Definition: Cal3.h:69
Calibration of a camera with radial distortion.
Definition: Cal3DS2_Base.h:42
Cal3DS2_Base()=default
Default Constructor with only unit focal length.
double p2() const
Second tangential distortion coefficient.
Definition: Cal3DS2_Base.h:110
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3DS2_Base.h:142
virtual boost::shared_ptr< Cal3DS2_Base > clone() const
Definition: Cal3DS2_Base.h:149
double k2() const
Second distortion coefficient.
Definition: Cal3DS2_Base.h:104
double k1() const
First distortion coefficient.
Definition: Cal3DS2_Base.h:101
size_t dim() const override
return DOF, dimensionality of tangent space
Definition: Cal3DS2_Base.h:139
double p1() const
First tangential distortion coefficient.
Definition: Cal3DS2_Base.h:107
Vector4 k() const
return distortion parameter vector
Definition: Cal3DS2_Base.h:113