gtsam  4.1.0
gtsam
Cal3Fisheye.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 
19 #pragma once
20 
21 #include <gtsam/geometry/Point2.h>
22 
23 #include <string>
24 
25 namespace gtsam {
26 
47 class GTSAM_EXPORT Cal3Fisheye {
48  private:
49  double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
50  double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
51 
52  public:
53  enum { dimension = 9 };
54  typedef boost::shared_ptr<Cal3Fisheye>
56 
59 
62  : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
63 
64  Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
65  const double v0, const double k1, const double k2,
66  const double k3, const double k4)
67  : fx_(fx),
68  fy_(fy),
69  s_(s),
70  u0_(u0),
71  v0_(v0),
72  k1_(k1),
73  k2_(k2),
74  k3_(k3),
75  k4_(k4) {}
76 
77  virtual ~Cal3Fisheye() {}
78 
82 
83  explicit Cal3Fisheye(const Vector9& v);
84 
88 
90  inline double fx() const { return fx_; }
91 
93  inline double fy() const { return fy_; }
94 
96  inline double skew() const { return s_; }
97 
99  inline double px() const { return u0_; }
100 
102  inline double py() const { return v0_; }
103 
105  inline double k1() const { return k1_; }
106 
108  inline double k2() const { return k2_; }
109 
111  inline double k3() const { return k3_; }
112 
114  inline double k4() const { return k4_; }
115 
117  Matrix3 K() const;
118 
120  Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
121 
123  Vector9 vector() const;
124 
126  static double Scaling(double r);
127 
137  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
138  OptionalJacobian<2, 2> Dp = boost::none) const;
139 
142  Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
143 
147 
149  virtual void print(const std::string& s = "") const;
150 
152  bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
153 
157 
159  Cal3Fisheye retract(const Vector& d) const;
160 
162  Vector localCoordinates(const Cal3Fisheye& T2) const;
163 
165  virtual size_t dim() const { return 9; }
166 
168  static size_t Dim() { return 9; }
169 
173 
175  virtual boost::shared_ptr<Cal3Fisheye> clone() const {
176  return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
177  }
178 
180 
181  private:
184 
186  friend class boost::serialization::access;
187  template <class Archive>
188  void serialize(Archive& ar, const unsigned int /*version*/) {
189  ar& BOOST_SERIALIZATION_NVP(fx_);
190  ar& BOOST_SERIALIZATION_NVP(fy_);
191  ar& BOOST_SERIALIZATION_NVP(s_);
192  ar& BOOST_SERIALIZATION_NVP(u0_);
193  ar& BOOST_SERIALIZATION_NVP(v0_);
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 
201 };
202 
203 template <>
204 struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
205 
206 template <>
207 struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
208 
209 } // namespace gtsam
gtsam::Cal3Fisheye::shared_ptr
boost::shared_ptr< Cal3Fisheye > shared_ptr
shared pointer to fisheye calibration object
Definition: Cal3Fisheye.h:55
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Cal3Fisheye::clone
virtual boost::shared_ptr< Cal3Fisheye > clone() const
Definition: Cal3Fisheye.h:175
gtsam::Cal3Fisheye::dim
virtual size_t dim() const
Return dimensions of calibration manifold object.
Definition: Cal3Fisheye.h:165
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::Cal3Fisheye::k3
double k3() const
First tangential distortion coefficient.
Definition: Cal3Fisheye.h:111
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Cal3Fisheye::px
double px() const
image center in x
Definition: Cal3Fisheye.h:99
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::Cal3Fisheye::k1
double k1() const
First distortion coefficient.
Definition: Cal3Fisheye.h:105
gtsam::Cal3Fisheye::fy
double fy() const
focal length x
Definition: Cal3Fisheye.h:93
gtsam::Cal3Fisheye::k2
double k2() const
Second distortion coefficient.
Definition: Cal3Fisheye.h:108
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Cal3Fisheye::skew
double skew() const
skew
Definition: Cal3Fisheye.h:96
gtsam::Cal3Fisheye
Definition: Cal3Fisheye.h:47
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Point2
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
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Cal3Fisheye::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition: Cal3Fisheye.h:168
Point2.h
2D Point
gtsam::Cal3Fisheye::k4
double k4() const
Second tangential distortion coefficient.
Definition: Cal3Fisheye.h:114
gtsam::Cal3Fisheye::k
Vector4 k() const
return distortion parameter vector
Definition: Cal3Fisheye.h:120
gtsam::Cal3Fisheye::Cal3Fisheye
Cal3Fisheye()
Default Constructor with only unit focal length.
Definition: Cal3Fisheye.h:61
gtsam::Cal3Fisheye::fx
double fx() const
focal length x
Definition: Cal3Fisheye.h:90
gtsam::Cal3Fisheye::py
double py() const
image center in y
Definition: Cal3Fisheye.h:102