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 Cal3Bundler.h
14 * @brief Calibration used by Bundler
15 * @date Sep 25, 2010
16 * @author Yong Dian Jian
17 * @author Varun Agrawal
18 */
19
20#pragma once
21
22#include <gtsam/geometry/Cal3f.h>
23#include <gtsam/geometry/Point2.h>
24
25namespace gtsam {
26
27/**
28 * @brief Calibration used by Bundler
29 * @ingroup geometry
30 * \nosubgrouping
31 */
32class GTSAM_EXPORT Cal3Bundler : public Cal3f {
33 private:
34 double k1_ = 0.0, k2_ = 0.0; ///< radial distortion coefficients
35 double tol_ = 1e-5; ///< tolerance value when calibrating
36
37 // Note: u0 and v0 are constants and not optimized.
38
39 public:
40 constexpr static auto dimension = 3;
41 using shared_ptr = std::shared_ptr<Cal3Bundler>;
42
43 /// @name Constructors
44 /// @{
45
46 /// Default constructor
47 Cal3Bundler() = default;
48
49 /**
50 * Constructor
51 * @param f focal length
52 * @param k1 first radial distortion coefficient (quadratic)
53 * @param k2 second radial distortion coefficient (quartic)
54 * @param u0 optional image center (default 0), considered a constant
55 * @param v0 optional image center (default 0), considered a constant
56 * @param tol optional calibration tolerance value
57 */
58 Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
59 double tol = 1e-5)
60 : Cal3f(f, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
61
62 ~Cal3Bundler() override = default;
63
64 /// @}
65 /// @name Testable
66 /// @{
67
68 /// Output stream operator
69 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
70 const Cal3Bundler& cal);
71
72 /// print with optional string
73 void print(const std::string& s = "") const override;
74
75 /// assert equality up to a tolerance
76 bool equals(const Cal3Bundler& K, double tol = 1e-9) const;
77
78 /// @}
79 /// @name Standard Interface
80 /// @{
81
82 /// distortion parameter k1
83 double k1() const { return k1_; }
84
85 /// distortion parameter k2
86 double k2() const { return k2_; }
87
88 Matrix3 K() const override; ///< Standard 3*3 calibration matrix
89 Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
90
91 Vector3 vector() const;
92
93 /**
94 * @brief: convert intrinsic coordinates xy to image coordinates uv
95 * Version of uncalibrate with derivatives
96 * @param p point in intrinsic coordinates
97 * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
98 * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
99 * @return point in image coordinates
100 */
101 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = {},
102 OptionalJacobian<2, 2> Dp = {}) const;
103
104 /**
105 * Convert a pixel coordinate to ideal coordinate xy
106 * @param pi point in image coordinates
107 * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
108 * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
109 * @return point in intrinsic coordinates
110 */
111 Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {},
112 OptionalJacobian<2, 2> Dp = {}) const;
113
114 /// @deprecated might be removed in next release, use uncalibrate
115 Matrix2 D2d_intrinsic(const Point2& p) const;
116
117 /// @deprecated might be removed in next release, use uncalibrate
118 Matrix23 D2d_calibration(const Point2& p) const;
119
120 /// @deprecated might be removed in next release, use uncalibrate
121 Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
122
123 /// @}
124 /// @name Manifold
125 /// @{
126
127 /// Return DOF, dimensionality of tangent space
128 size_t dim() const { return Dim(); }
129
130 /// Return DOF, dimensionality of tangent space
131 static size_t Dim() { return dimension; }
132
133 /// Update calibration with tangent space delta
134 Cal3Bundler retract(const Vector& d) const {
135 return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
136 }
137
138 /// Calculate local coordinates to another calibration
139 Vector3 localCoordinates(const Cal3Bundler& T2) const {
140 return T2.vector() - vector();
141 }
142
143 private:
144 /// @}
145 /// @name Advanced Interface
146 /// @{
147
148#if GTSAM_ENABLE_BOOST_SERIALIZATION
149 /** Serialization function */
150 friend class boost::serialization::access;
151 template <class Archive>
152 void serialize(Archive& ar, const unsigned int /*version*/) {
153 ar& boost::serialization::make_nvp(
154 n: "Cal3Bundler", v&: boost::serialization::base_object<Cal3>(d&: *this));
155 ar& BOOST_SERIALIZATION_NVP(k1_);
156 ar& BOOST_SERIALIZATION_NVP(k2_);
157 ar& BOOST_SERIALIZATION_NVP(tol_);
158 }
159#endif
160
161 /// @}
162};
163
164template <>
165struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
166
167template <>
168struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
169
170} // namespace gtsam
171