Cal3.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 
22 #pragma once
23 
24 #include <gtsam/geometry/Point2.h>
25 
26 namespace gtsam {
27 
46 template <typename Cal, size_t Dim>
47 void calibrateJacobians(const Cal& calibration, const Point2& pn,
48  OptionalJacobian<2, Dim> Dcal = {},
49  OptionalJacobian<2, 2> Dp = {}) {
50  if (Dcal || Dp) {
52  Matrix22 H_uncal_pn, H_uncal_pn_inv;
53 
54  // Compute uncalibrate Jacobians
55  calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
56 
57  H_uncal_pn_inv = H_uncal_pn.inverse();
58 
59  if (Dp) *Dp = H_uncal_pn_inv;
60  if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
61  }
62 }
63 
69 class GTSAM_EXPORT Cal3 {
70  protected:
71  double fx_ = 1.0f, fy_ = 1.0f;
72  double s_ = 0.0f;
73  double u0_ = 0.0f, v0_ = 0.0f;
74 
75  public:
76  enum { dimension = 5 };
78  using shared_ptr = std::shared_ptr<Cal3>;
79 
82 
84  Cal3() = default;
85 
87  Cal3(double fx, double fy, double s, double u0, double v0)
88  : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
89 
91  Cal3(const Vector5& d)
92  : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
93 
100  Cal3(double fov, int w, int h);
101 
103  virtual ~Cal3() {}
104 
108 
118  Cal3(const std::string& path);
119 
123 
125  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
126  const Cal3& cal);
127 
129  virtual void print(const std::string& s = "") const;
130 
132  bool equals(const Cal3& K, double tol = 10e-9) const;
133 
137 
139  inline double fx() const { return fx_; }
140 
142  inline double fy() const { return fy_; }
143 
145  inline double aspectRatio() const { return fx_ / fy_; }
146 
148  inline double skew() const { return s_; }
149 
151  inline double px() const { return u0_; }
152 
154  inline double py() const { return v0_; }
155 
157  Point2 principalPoint() const { return Point2(u0_, v0_); }
158 
160  Vector5 vector() const {
161  Vector5 v;
162  v << fx_, fy_, s_, u0_, v0_;
163  return v;
164  }
165 
167  virtual Matrix3 K() const {
168  Matrix3 K;
169  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
170  return K;
171  }
172 
174  Matrix3 inverse() const;
175 
177  inline virtual size_t dim() const { return Dim(); }
178 
180  inline static size_t Dim() { return dimension; }
181 
185 
186  private:
187 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
188  friend class boost::serialization::access;
190  template <class Archive>
191  void serialize(Archive& ar, const unsigned int /*version*/) {
192  ar& BOOST_SERIALIZATION_NVP(fx_);
193  ar& BOOST_SERIALIZATION_NVP(fy_);
194  ar& BOOST_SERIALIZATION_NVP(s_);
195  ar& BOOST_SERIALIZATION_NVP(u0_);
196  ar& BOOST_SERIALIZATION_NVP(v0_);
197  }
198 #endif
199 
201 };
202 
203 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3.h:180
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
double skew() const
skew
Definition: Cal3.h:148
std::string serialize(const T &input)
serializes to a string
Point2 principalPoint() const
return the principal point
Definition: Cal3.h:157
Vector2 Point2
Definition: Point2.h:32
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
double py() const
image center in y
Definition: Cal3.h:154
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition: Cal3.h:177
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Common base class for all calibration models.
Definition: Cal3.h:69
void calibrateJacobians(const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal={}, OptionalJacobian< 2, 2 > Dp={})
Definition: Cal3.h:47
virtual ~Cal3()
Virtual destructor.
Definition: Cal3.h:103
const double fy
double fx() const
focal length x
Definition: Cal3.h:139
Array< int, Dynamic, 1 > v
double aspectRatio() const
aspect ratio
Definition: Cal3.h:145
Cal3(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition: Cal3.h:87
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static const double u0
RowVector3d w
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
traits
Definition: chartTesting.h:28
const double h
ofstream os("timeSchurFactors.csv")
static const double v0
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
const G double tol
Definition: Group.h:86
const double fx
The matrix class, also used for vectors and row-vectors.
2D Point
static double fov
double fy() const
focal length y
Definition: Cal3.h:142
Cal3(const Vector5 &d)
constructor from vector
Definition: Cal3.h:91
double px() const
image center in x
Definition: Cal3.h:151


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:59