gtsam  4.1.0
gtsam
Cal3_S2Stereo.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 
18 #pragma once
19 
20 #include <gtsam/geometry/Cal3_S2.h>
21 #include <iosfwd>
22 
23 namespace gtsam {
24 
30  class GTSAM_EXPORT Cal3_S2Stereo {
31  private:
32 
33  Cal3_S2 K_;
34  double b_;
35 
36  public:
37 
38  enum { dimension = 6 };
39  typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;
40 
43 
46  K_(1, 1, 0, 0, 0), b_(1.0) {
47  }
48 
50  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
51  K_(fx, fy, s, u0, v0), b_(b) {
52  }
53 
55  Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
56 
58  Cal3_S2Stereo(double fov, int w, int h, double b) :
59  K_(fov, w, h), b_(b) {
60  }
61 
65 
66  void print(const std::string& s = "") const;
67 
69  bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
70 
74 
76  const Cal3_S2& calibration() const { return K_;}
77 
79  Matrix matrix() const { return K_.matrix();}
80 
82  inline double fx() const { return K_.fx();}
83 
85  inline double fy() const { return K_.fy();}
86 
88  inline double skew() const { return K_.skew();}
89 
91  inline double px() const { return K_.px();}
92 
94  inline double py() const { return K_.py();}
95 
97  Point2 principalPoint() const { return K_.principalPoint();}
98 
100  inline double baseline() const { return b_; }
101 
103  Vector6 vector() const {
104  Vector6 v;
105  v << K_.vector(), b_;
106  return v;
107  }
108 
112 
114  inline size_t dim() const { return dimension; }
115 
117  static size_t Dim() { return dimension; }
118 
120  inline Cal3_S2Stereo retract(const Vector& d) const {
121  return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
122  }
123 
125  Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
126  return T2.vector() - vector();
127  }
128 
129 
133 
134  private:
136  friend class boost::serialization::access;
137  template<class Archive>
138  void serialize(Archive & ar, const unsigned int /*version*/)
139  {
140  ar & BOOST_SERIALIZATION_NVP(K_);
141  ar & BOOST_SERIALIZATION_NVP(b_);
142  }
144 
145  };
146 
147  // Define GTSAM traits
148  template<>
149  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
150  };
151 
152  template<>
153  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
154  };
155 
156 } // \ namespace gtsam
gtsam::Cal3_S2Stereo::fy
double fy() const
focal length x
Definition: Cal3_S2Stereo.h:85
gtsam::Cal3_S2Stereo::fx
double fx() const
focal length x
Definition: Cal3_S2Stereo.h:82
gtsam::Cal3_S2Stereo::matrix
Matrix matrix() const
return calibration matrix K, same for left and right
Definition: Cal3_S2Stereo.h:79
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_S2Stereo::skew
double skew() const
skew
Definition: Cal3_S2Stereo.h:88
gtsam::Cal3_S2Stereo::px
double px() const
image center in x
Definition: Cal3_S2Stereo.h:91
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_S2Stereo::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3_S2Stereo.h:117
gtsam::Cal3_S2Stereo::principalPoint
Point2 principalPoint() const
return the principal point
Definition: Cal3_S2Stereo.h:97
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_S2Stereo::baseline
double baseline() const
return baseline
Definition: Cal3_S2Stereo.h:100
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::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_S2Stereo::localCoordinates
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
Definition: Cal3_S2Stereo.h:125
gtsam::Cal3_S2::fy
double fy() const
focal length y
Definition: Cal3_S2.h:97
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo()
default calibration leaves coordinates unchanged
Definition: Cal3_S2Stereo.h:45
gtsam::Cal3_S2Stereo::retract
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition: Cal3_S2Stereo.h:120
gtsam::Cal3_S2Stereo::calibration
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition: Cal3_S2Stereo.h:76
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_S2Stereo::py
double py() const
image center in y
Definition: Cal3_S2Stereo.h:94
gtsam::Cal3_S2::px
double px() const
image center in x
Definition: Cal3_S2.h:112
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(const Vector &d)
constructor from vector
Definition: Cal3_S2Stereo.h:55
gtsam::Cal3_S2::vector
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3_S2.h:127
gtsam::Cal3_S2Stereo::dim
size_t dim() const
return DOF, dimensionality of tangent space
Definition: Cal3_S2Stereo.h:114
gtsam::Cal3_S2Stereo::shared_ptr
boost::shared_ptr< Cal3_S2Stereo > shared_ptr
shared pointer to stereo calibration object
Definition: Cal3_S2Stereo.h:39
gtsam::Cal3_S2Stereo::vector
Vector6 vector() const
vectorized form (column-wise)
Definition: Cal3_S2Stereo.h:103
gtsam::Cal3_S2Stereo
Definition: Cal3_S2Stereo.h:30
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition: Cal3_S2Stereo.h:50
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fov, int w, int h, double b)
easy constructor; field-of-view in degrees, assumes zero skew
Definition: Cal3_S2Stereo.h:58