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/Cal3.h>
25 #include <gtsam/geometry/Point2.h>
26 
27 namespace gtsam {
28 
34 class GTSAM_EXPORT Cal3_S2 : public Cal3 {
35  public:
36  enum { dimension = 5 };
37 
39  using shared_ptr = std::shared_ptr<Cal3_S2>;
40 
43 
45  Cal3_S2() = default;
46 
48  Cal3_S2(double fx, double fy, double s, double u0, double v0)
49  : Cal3(fx, fy, s, u0, v0) {}
50 
52  Cal3_S2(const Vector5& d) : Cal3(d) {}
53 
60  Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
61 
70  OptionalJacobian<2, 2> Dp = {}) const;
71 
79  Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {},
80  OptionalJacobian<2, 2> Dp = {}) const;
81 
87  Vector3 calibrate(const Vector3& p) const;
88 
92 
94  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
95  const Cal3_S2& cal);
96 
98  void print(const std::string& s = "Cal3_S2") const override;
99 
101  bool equals(const Cal3_S2& K, double tol = 10e-9) const;
102 
104  inline Cal3_S2 between(const Cal3_S2& q,
105  OptionalJacobian<5, 5> H1 = {},
106  OptionalJacobian<5, 5> H2 = {}) const {
107  if (H1) *H1 = -I_5x5;
108  if (H2) *H2 = I_5x5;
109  return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
110  q.v0_ - v0_);
111  }
112 
116 
118  inline static size_t Dim() { return dimension; }
119 
121  inline Cal3_S2 retract(const Vector& d) const {
122  return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
123  }
124 
126  Vector5 localCoordinates(const Cal3_S2& T2) const {
127  return T2.vector() - vector();
128  }
129 
133 
134  private:
135 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
136  friend class boost::serialization::access;
138  template <class Archive>
139  void serialize(Archive& ar, const unsigned int /*version*/) {
140  ar& boost::serialization::make_nvp(
141  "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
142  }
143 #endif
144 
146 };
147 
148 template <>
149 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
150 
151 template <>
152 struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
153 
154 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Cal3_S2(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition: Cal3_S2.h:48
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Eigen::Vector3d Vector3
Definition: Vector.h:43
std::string serialize(const T &input)
serializes to a string
Vector2 Point2
Definition: Point2.h:32
Vector5 localCoordinates(const Cal3_S2 &T2) const
Unretraction for the calibration.
Definition: Cal3_S2.h:126
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
double u0_
Definition: Cal3.h:73
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
Cal3_S2(const Vector5 &d)
constructor from vector
Definition: Cal3_S2.h:52
Common base class for all calibration models.
Definition: Cal3.h:69
const double fy
Eigen::VectorXd Vector
Definition: Vector.h:38
double fy_
focal length
Definition: Cal3.h:71
double s_
skew
Definition: Cal3.h:72
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
Common code for all Calibration models.
static const double u0
RowVector3d w
double v0_
principal point
Definition: Cal3.h:73
traits
Definition: chartTesting.h:28
Cal3_S2 retract(const Vector &d) const
Given 5-dim tangent vector, create new calibration.
Definition: Cal3_S2.h:121
const double h
ofstream os("timeSchurFactors.csv")
static const double v0
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
float * p
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
const G double tol
Definition: Group.h:86
const double fx
double fx_
Definition: Cal3.h:71
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3_S2.h:118
2D Point
Cal3_S2(double fov, int w, int h)
Definition: Cal3_S2.h:60
Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1={}, OptionalJacobian< 5, 5 > H2={}) const
"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
Definition: Cal3_S2.h:104
static double fov


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