gtsam  4.1.0
gtsam
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 
19 #pragma once
20 
21 #include <gtsam/geometry/Point2.h>
22 
23 namespace gtsam {
24 
30 class GTSAM_EXPORT Cal3Bundler {
31 
32 private:
33  double f_;
34  double k1_, k2_;
35  double u0_, v0_;
36  double tol_;
37 
38 public:
39 
40  enum { dimension = 3 };
41 
44 
46  Cal3Bundler();
47 
57  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
58  double tol = 1e-5);
59 
60  virtual ~Cal3Bundler() {}
61 
65 
67  void print(const std::string& s = "") const;
68 
70  bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
71 
75 
76  Matrix3 K() const;
77  Vector4 k() const;
78 
79  Vector3 vector() const;
80 
82  inline double fx() const {
83  return f_;
84  }
85 
87  inline double fy() const {
88  return f_;
89  }
90 
92  inline double k1() const {
93  return k1_;
94  }
95 
97  inline double k2() const {
98  return k2_;
99  }
100 
102  inline double px() const {
103  return u0_;
104  }
105 
107  inline double py() const {
108  return v0_;
109  }
110 
111 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
112  inline double u0() const {
114  return u0_;
115  }
116 
118  inline double v0() const {
119  return v0_;
120  }
121 #endif
122 
123 
132  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
133  OptionalJacobian<2, 2> Dp = boost::none) const;
134 
143  Point2 calibrate(const Point2& pi,
144  OptionalJacobian<2, 3> Dcal = boost::none,
145  OptionalJacobian<2, 2> Dp = boost::none) const;
146 
148  Matrix2 D2d_intrinsic(const Point2& p) const;
149 
151  Matrix23 D2d_calibration(const Point2& p) const;
152 
154  Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
155 
159 
161  Cal3Bundler retract(const Vector& d) const;
162 
164  Vector3 localCoordinates(const Cal3Bundler& T2) const;
165 
167  virtual size_t dim() const {
168  return 3;
169  }
170 
172  static size_t Dim() {
173  return 3;
174  }
175 
176 private:
177 
181 
183  friend class boost::serialization::access;
184  template<class Archive>
185  void serialize(Archive & ar, const unsigned int /*version*/) {
186  ar & BOOST_SERIALIZATION_NVP(f_);
187  ar & BOOST_SERIALIZATION_NVP(k1_);
188  ar & BOOST_SERIALIZATION_NVP(k2_);
189  ar & BOOST_SERIALIZATION_NVP(u0_);
190  ar & BOOST_SERIALIZATION_NVP(v0_);
191  ar & BOOST_SERIALIZATION_NVP(tol_);
192  }
193 
195 
196 };
197 
198 template<>
199 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
200 
201 template<>
202 struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
203 
204 } // namespace gtsam
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Cal3Bundler::k2
double k2() const
distorsion parameter k2
Definition: Cal3Bundler.h:97
gtsam::Cal3Bundler::fy
double fy() const
focal length y
Definition: Cal3Bundler.h:87
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::Cal3Bundler::dim
virtual size_t dim() const
dimensionality
Definition: Cal3Bundler.h:167
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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::Cal3Bundler::px
double px() const
image center in x
Definition: Cal3Bundler.h:102
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::Cal3Bundler::py
double py() const
image center in y
Definition: Cal3Bundler.h:107
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Cal3Bundler
Definition: Cal3Bundler.h:30
gtsam::Cal3Bundler::k1
double k1() const
distorsion parameter k1
Definition: Cal3Bundler.h:92
gtsam::Cal3Bundler::Dim
static size_t Dim()
dimensionality
Definition: Cal3Bundler.h:172
Point2.h
2D Point
gtsam::Cal3Bundler::fx
double fx() const
focal length x
Definition: Cal3Bundler.h:82