Cal3Bundler.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>
23 #include <gtsam/geometry/Point2.h>
24 
25 namespace gtsam {
26 
32 class GTSAM_EXPORT Cal3Bundler : public Cal3 {
33  private:
34  double k1_ = 0.0f, k2_ = 0.0f;
35  double tol_ = 1e-5;
36 
37  // NOTE: We use the base class fx to represent the common focal length.
38  // Also, image center parameters (u0, v0) are not optimized
39  // but are treated as constants.
40 
41  public:
42  enum { dimension = 3 };
43 
45  using shared_ptr = std::shared_ptr<Cal3Bundler>;
46 
49 
51  Cal3Bundler() = default;
52 
62  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
63  double tol = 1e-5)
64  : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
65 
66  ~Cal3Bundler() override {}
67 
71 
73  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
74  const Cal3Bundler& cal);
75 
77  void print(const std::string& s = "") const override;
78 
80  bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
81 
85 
87  inline double k1() const { return k1_; }
88 
90  inline double k2() const { return k2_; }
91 
93  inline double px() const { return u0_; }
94 
96  inline double py() const { return v0_; }
97 
98  Matrix3 K() const override;
99  Vector4 k() const;
100 
101  Vector3 vector() const;
102 
112  OptionalJacobian<2, 2> Dp = {}) const;
113 
122  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {},
123  OptionalJacobian<2, 2> Dp = {}) const;
124 
126  Matrix2 D2d_intrinsic(const Point2& p) const;
127 
129  Matrix23 D2d_calibration(const Point2& p) const;
130 
132  Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
133 
137 
139  size_t dim() const override { return Dim(); }
140 
142  inline static size_t Dim() { return dimension; }
143 
145  inline Cal3Bundler retract(const Vector& d) const {
146  return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
147  }
148 
151  return T2.vector() - vector();
152  }
153 
154  private:
158 
159 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
160 
161  friend class boost::serialization::access;
162  template <class Archive>
163  void serialize(Archive& ar, const unsigned int /*version*/) {
164  ar& boost::serialization::make_nvp(
165  "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
166  ar& BOOST_SERIALIZATION_NVP(k1_);
167  ar& BOOST_SERIALIZATION_NVP(k2_);
168  ar& BOOST_SERIALIZATION_NVP(tol_);
169  }
170 #endif
171 
173 };
174 
175 template <>
176 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
177 
178 template <>
179 struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
180 
181 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Vector3 localCoordinates(const Cal3Bundler &T2) const
Calculate local coordinates to another calibration.
Definition: Cal3Bundler.h:150
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 vector() const
Definition: Cal3Bundler.cpp:42
std::string serialize(const T &input)
serializes to a string
Vector2 Point2
Definition: Point2.h:32
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
Common base class for all calibration models.
Definition: Cal3.h:69
~Cal3Bundler() override
Definition: Cal3Bundler.h:66
Eigen::VectorXd Vector
Definition: Vector.h:38
Cal3Bundler retract(const Vector &d) const
Update calibration with tangent space delta.
Definition: Cal3Bundler.h:145
double k1() const
distorsion parameter k1
Definition: Cal3Bundler.h:87
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Common code for all Calibration models.
static const double u0
size_t dim() const override
return DOF, dimensionality of tangent space
Definition: Cal3Bundler.h:139
traits
Definition: chartTesting.h:28
double k2() const
distorsion parameter k2
Definition: Cal3Bundler.h:90
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double tol=1e-5)
Definition: Cal3Bundler.h:62
ofstream os("timeSchurFactors.csv")
static const double v0
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
float * p
double px() const
image center in x
Definition: Cal3Bundler.h:93
const G double tol
Definition: Group.h:86
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3Bundler.h:142
2D Point
double py() const
image center in y
Definition: Cal3Bundler.h:96


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:00