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
12/**
13 * @file Cal3Fisheye.h
14 * @brief Calibration of a fisheye camera
15 * @date Apr 8, 2020
16 * @author ghaggin
17 * @author Varun Agrawal
18 */
19
20#pragma once
21
22#include <gtsam/geometry/Cal3.h>
23#include <gtsam/geometry/Point2.h>
24
25#include <memory>
26
27#include <string>
28
29namespace gtsam {
30
31/**
32 * @brief Calibration of a fisheye camera
33 * @ingroup geometry
34 * \nosubgrouping
35 *
36 * Uses same distortionmodel as OpenCV, with
37 * https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html
38 * 3D point in camera frame
39 * p = (x, y, z)
40 * Intrinsic coordinates:
41 * [x_i;y_i] = [x/z; y/z]
42 * Distorted coordinates:
43 * r² = (x_i)² + (y_i)²
44 * th = atan(r)
45 * th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
46 * [x_d; y_d] = (th_d / r)*[x_i; y_i]
47 * Pixel coordinates:
48 * K = [fx s u0; 0 fy v0 ;0 0 1]
49 * [u; v; 1] = K*[x_d; y_d; 1]
50 */
51class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
52 private:
53 double k1_ = 0.0f, k2_ = 0.0f; ///< fisheye distortion coefficients
54 double k3_ = 0.0f, k4_ = 0.0f; ///< fisheye distortion coefficients
55 double tol_ = 1e-5; ///< tolerance value when calibrating
56
57 public:
58 constexpr static auto dimension = 9;
59 ///< shared pointer to fisheye calibration object
60 using shared_ptr = std::shared_ptr<Cal3Fisheye>;
61
62 /// @name Standard Constructors
63 /// @{
64
65 /// Default Constructor with only unit focal length
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
80 /// @}
81 /// @name Advanced Constructors
82 /// @{
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
91 /// @}
92 /// @name Standard Interface
93 /// @{
94
95 /// First distortion coefficient
96 double k1() const { return k1_; }
97
98 /// Second distortion coefficient
99 double k2() const { return k2_; }
100
101 /// First tangential distortion coefficient
102 double k3() const { return k3_; }
103
104 /// Second tangential distortion coefficient
105 double k4() const { return k4_; }
106
107 /// return distortion parameter vector
108 Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
109
110 /// Return all parameters as a vector
111 Vector9 vector() const;
112
113 /// Helper function that calculates atan(r)/r
114 static double Scaling(double r);
115
116 /**
117 * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
118 * coordinates [u; v]
119 * @param p point in intrinsic coordinates
120 * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
121 * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
122 * @return point in (distorted) image coordinates
123 */
124 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
125 OptionalJacobian<2, 2> Dp = {}) const;
126
127 /**
128 * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
129 * y_i]
130 * @param p point in image coordinates
131 * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
132 * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
133 * @return point in intrinsic coordinates
134 */
135 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
136 OptionalJacobian<2, 2> Dp = {}) const;
137
138 /// @}
139 /// @name Testable
140 /// @{
141
142 /// Output stream operator
143 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
144 const Cal3Fisheye& cal);
145
146 /// print with optional string
147 void print(const std::string& s = "") const override;
148
149 /// assert equality up to a tolerance
150 bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
151
152 /// @}
153 /// @name Manifold
154 /// @{
155
156 /// Return dimensions of calibration manifold object
157 size_t dim() const { return Dim(); }
158
159 /// Return dimensions of calibration manifold object
160 static size_t Dim() { return dimension; }
161
162 /// Given delta vector, update calibration
163 Cal3Fisheye retract(const Vector& d) const {
164 return Cal3Fisheye(vector() + d);
165 }
166
167 /// Given a different calibration, calculate update to obtain it
168 Vector localCoordinates(const Cal3Fisheye& T2) const {
169 return T2.vector() - vector();
170 }
171
172 /// @}
173 /// @name Clone
174 /// @{
175
176 /// @return a deep copy of this object
177 virtual std::shared_ptr<Cal3Fisheye> clone() const {
178 return std::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
179 }
180
181 /// @}
182
183 private:
184 /// @name Advanced Interface
185 /// @{
186
187#if GTSAM_ENABLE_BOOST_SERIALIZATION
188 /** Serialization function */
189 friend class boost::serialization::access;
190 template <class Archive>
191 void serialize(Archive& ar, const unsigned int /*version*/) {
192 ar& boost::serialization::make_nvp(
193 n: "Cal3Fisheye", v&: boost::serialization::base_object<Cal3>(d&: *this));
194 ar& BOOST_SERIALIZATION_NVP(k1_);
195 ar& BOOST_SERIALIZATION_NVP(k2_);
196 ar& BOOST_SERIALIZATION_NVP(k3_);
197 ar& BOOST_SERIALIZATION_NVP(k4_);
198 }
199#endif
200
201 /// @}
202};
203
204template <>
205struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
206
207template <>
208struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
209
210} // namespace gtsam
211