gtsam  4.1.0
gtsam
Cal3_S2.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 
33 class GTSAM_EXPORT Cal3_S2 {
34 private:
35  double fx_, fy_, s_, u0_, v0_;
36 
37 public:
38  enum { dimension = 5 };
39  typedef boost::shared_ptr<Cal3_S2> shared_ptr;
40 
43 
45  Cal3_S2() :
46  fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
47  }
48 
50  Cal3_S2(double fx, double fy, double s, double u0, double v0) :
51  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
52  }
53 
55  Cal3_S2(const Vector &d) :
56  fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
57  }
58 
65  Cal3_S2(double fov, int w, int h);
66 
70 
72  Cal3_S2(const std::string &path);
73 
77 
79  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
80 
82  void print(const std::string& s = "Cal3_S2") const;
83 
85  bool equals(const Cal3_S2& K, double tol = 10e-9) const;
86 
90 
92  inline double fx() const {
93  return fx_;
94  }
95 
97  inline double fy() const {
98  return fy_;
99  }
100 
102  inline double aspectRatio() const {
103  return fx_/fy_;
104  }
105 
107  inline double skew() const {
108  return s_;
109  }
110 
112  inline double px() const {
113  return u0_;
114  }
115 
117  inline double py() const {
118  return v0_;
119  }
120 
123  return Point2(u0_, v0_);
124  }
125 
127  Vector5 vector() const {
128  Vector5 v;
129  v << fx_, fy_, s_, u0_, v0_;
130  return v;
131  }
132 
134  Matrix3 K() const {
135  Matrix3 K;
136  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
137  return K;
138  }
139 
141  Matrix3 matrix() const {
142  return K();
143  }
144 
146  Matrix3 matrix_inverse() const {
147  const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
148  Matrix3 K_inverse;
149  K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
150  1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
151  return K_inverse;
152  }
153 
161  Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
162  OptionalJacobian<2,2> Dp = boost::none) const;
163 
171  Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
172  OptionalJacobian<2,2> Dp = boost::none) const;
173 
179  Vector3 calibrate(const Vector3& p) const;
180 
182  inline Cal3_S2 between(const Cal3_S2& q,
183  OptionalJacobian<5,5> H1=boost::none,
184  OptionalJacobian<5,5> H2=boost::none) const {
185  if(H1) *H1 = -I_5x5;
186  if(H2) *H2 = I_5x5;
187  return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
188  }
189 
190 
194 
196  inline size_t dim() const { return dimension; }
197 
199  static size_t Dim() { return dimension; }
200 
202  inline Cal3_S2 retract(const Vector& d) const {
203  return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
204  }
205 
207  Vector5 localCoordinates(const Cal3_S2& T2) const {
208  return T2.vector() - vector();
209  }
210 
214 
215 private:
216 
218  friend class boost::serialization::access;
219  template<class Archive>
220  void serialize(Archive & ar, const unsigned int /*version*/) {
221  ar & BOOST_SERIALIZATION_NVP(fx_);
222  ar & BOOST_SERIALIZATION_NVP(fy_);
223  ar & BOOST_SERIALIZATION_NVP(s_);
224  ar & BOOST_SERIALIZATION_NVP(u0_);
225  ar & BOOST_SERIALIZATION_NVP(v0_);
226  }
227 
229 
230 };
231 
232 template<>
233 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
234 
235 template<>
236 struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
237 
238 } // \ namespace gtsam
gtsam::Cal3_S2::shared_ptr
boost::shared_ptr< Cal3_S2 > shared_ptr
shared pointer to calibration object
Definition: Cal3_S2.h:39
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Cal3_S2::py
double py() const
image center in y
Definition: Cal3_S2.h:117
gtsam::Cal3_S2::retract
Cal3_S2 retract(const Vector &d) const
Given 5-dim tangent vector, create new calibration.
Definition: Cal3_S2.h:202
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::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::Cal3_S2::Cal3_S2
Cal3_S2(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition: Cal3_S2.h:50
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::Cal3_S2::matrix
Matrix3 matrix() const
Definition: Cal3_S2.h:141
gtsam::Cal3_S2
Definition: Cal3_S2.h:33
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Cal3_S2::dim
size_t dim() const
return DOF, dimensionality of tangent space
Definition: Cal3_S2.h:196
gtsam::Cal3_S2::skew
double skew() const
skew
Definition: Cal3_S2.h:107
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::Cal3_S2::principalPoint
Point2 principalPoint() const
return the principal point
Definition: Cal3_S2.h:122
gtsam::Cal3_S2::between
Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1=boost::none, OptionalJacobian< 5, 5 > H2=boost::none) const
"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
Definition: Cal3_S2.h:182
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::Cal3_S2::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3_S2.h:199
gtsam::Cal3_S2::fy
double fy() const
focal length y
Definition: Cal3_S2.h:97
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(const Vector &d)
constructor from vector
Definition: Cal3_S2.h:55
gtsam::Cal3_S2::matrix_inverse
Matrix3 matrix_inverse() const
return inverted calibration matrix inv(K)
Definition: Cal3_S2.h:146
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Cal3_S2::fx
double fx() const
focal length x
Definition: Cal3_S2.h:92
gtsam::Cal3_S2::px
double px() const
image center in x
Definition: Cal3_S2.h:112
gtsam::Cal3_S2::K
Matrix3 K() const
return calibration matrix K
Definition: Cal3_S2.h:134
Point2.h
2D Point
gtsam::Cal3_S2::vector
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3_S2.h:127
gtsam::Cal3_S2::Cal3_S2
Cal3_S2()
Create a default calibration that leaves coordinates unchanged.
Definition: Cal3_S2.h:45
gtsam::Cal3_S2::aspectRatio
double aspectRatio() const
aspect ratio
Definition: Cal3_S2.h:102
gtsam::Cal3_S2::localCoordinates
Vector5 localCoordinates(const Cal3_S2 &T2) const
Unretraction for the calibration.
Definition: Cal3_S2.h:207