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 : public Cal3_S2 {
31  private:
32  double b_ = 1.0f;
33 
34  public:
35  enum { dimension = 6 };
36 
38  using shared_ptr = std::shared_ptr<Cal3_S2Stereo>;
39 
42 
44  Cal3_S2Stereo() = default;
45 
47  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
48  : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
49 
51  Cal3_S2Stereo(const Vector6& d)
52  : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
53 
55  Cal3_S2Stereo(double fov, int w, int h, double b)
56  : Cal3_S2(fov, w, h), b_(b) {}
57 
59 
68  OptionalJacobian<2, 2> Dp = {}) const;
69 
77  Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {},
78  OptionalJacobian<2, 2> Dp = {}) const;
79 
85  Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
86 
89 
91  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
92  const Cal3_S2Stereo& cal);
93 
95  void print(const std::string& s = "") const override;
96 
98  bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
99 
103 
105  const Cal3_S2& calibration() const { return *this; }
106 
108  Matrix3 K() const override { return Cal3_S2::K(); }
109 
111  inline double baseline() const { return b_; }
112 
114  Vector6 vector() const {
115  Vector6 v;
116  v << Cal3_S2::vector(), b_;
117  return v;
118  }
119 
123 
125  inline size_t dim() const override { return Dim(); }
126 
128  inline static size_t Dim() { return dimension; }
129 
131  inline Cal3_S2Stereo retract(const Vector& d) const {
132  return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
133  py() + d(4), b_ + d(5));
134  }
135 
137  Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
138  return T2.vector() - vector();
139  }
140 
144 
145  private:
146 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
147 
148  friend class boost::serialization::access;
149  template <class Archive>
150  void serialize(Archive& ar, const unsigned int /*version*/) {
151  ar& boost::serialization::make_nvp(
152  "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
153  ar& BOOST_SERIALIZATION_NVP(b_);
154  }
155 #endif
156 };
158 
159 // Define GTSAM traits
160 template <>
161 struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
162 
163 template <>
164 struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
165 };
166 
167 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
size_t dim() const override
return DOF, dimensionality of tangent space
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
static size_t Dim()
return DOF, dimensionality of tangent space
Eigen::Vector3d Vector3
Definition: Vector.h:43
std::string serialize(const T &input)
serializes to a string
Vector2 Point2
Definition: Point2.h:32
Matrix3 K() const override
return calibration matrix K, same for left and right
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:53
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition: Cal3_S2Stereo.h:47
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition: Cal3_S2Stereo.h:51
const double fy
Eigen::VectorXd Vector
Definition: Vector.h:38
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
int RealScalar int RealScalar * py
Array< int, Dynamic, 1 > v
RealScalar RealScalar * px
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
static const double u0
const G & b
Definition: Group.h:86
RowVector3d w
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
traits
Definition: chartTesting.h:28
const double h
Vector3 calibrate(const Vector3 &p) const
Definition: Cal3_S2Stereo.h:85
ofstream os("timeSchurFactors.csv")
static const double v0
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
const Cal3_S2 & calibration() const
return calibration, same for left and right
float * p
Vector6 vector() const
vectorized form (column-wise)
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
const G double tol
Definition: Group.h:86
const double fx
double baseline() const
return baseline
static double fov
Cal3_S2Stereo(double fov, int w, int h, double b)
easy constructor; field-of-view in degrees, assumes zero skew
Definition: Cal3_S2Stereo.h:55
The most common 5DOF 3D->2D calibration.


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