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 = boost::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 
65  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
66  OptionalJacobian<2, 2> Dp = boost::none) const;
67 
75  Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
76  OptionalJacobian<2, 2> Dp = boost::none) const;
77 
83  Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
84 
88 
90  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
91  const Cal3_S2Stereo& cal);
92 
94  void print(const std::string& s = "") const override;
95 
97  bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
98 
102 
104  const Cal3_S2& calibration() const { return *this; }
105 
107  Matrix3 K() const override { return Cal3_S2::K(); }
108 
110  inline double baseline() const { return b_; }
111 
113  Vector6 vector() const {
114  Vector6 v;
115  v << Cal3_S2::vector(), b_;
116  return v;
117  }
118 
122 
124  inline size_t dim() const override { return Dim(); }
125 
127  inline static size_t Dim() { return dimension; }
128 
130  inline Cal3_S2Stereo retract(const Vector& d) const {
131  return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
132  py() + d(4), b_ + d(5));
133  }
134 
136  Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
137  return T2.vector() - vector();
138  }
139 
143 
144  private:
146  friend class boost::serialization::access;
147  template <class Archive>
148  void serialize(Archive& ar, const unsigned int /*version*/) {
149  ar& boost::serialization::make_nvp(
150  "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
151  ar& BOOST_SERIALIZATION_NVP(b_);
152  }
154 };
155 
156 // Define GTSAM traits
157 template <>
158 struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
159 
160 template <>
161 struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
162 };
163 
164 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
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
Vector6 vector() const
vectorized form (column-wise)
Vector2 Point2
Definition: Point2.h:27
ArrayXcf v
Definition: Cwise_arg.cpp:1
double baseline() const
return baseline
Pose2 T2(M_PI/2.0, Point2(0.0, 2.0))
Matrix3 K() const override
return calibration matrix K, same for left and right
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition: Cal3_S2Stereo.h:47
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
Definition: Cal3_S2.cpp:53
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition: Cal3_S2Stereo.h:51
Vector3 calibrate(const Vector3 &p) const
Definition: Cal3_S2Stereo.h:83
const double fy
Eigen::VectorXd Vector
Definition: Vector.h:38
int RealScalar int RealScalar * py
boost::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
RealScalar RealScalar * px
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
void serialize(Archive &ar, const unsigned int)
static const double u0
const G & b
Definition: Group.h:83
RowVector3d w
traits
Definition: chartTesting.h:28
const double h
ofstream os("timeSchurFactors.csv")
static const double v0
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
float * p
static double fov
const G double tol
Definition: Group.h:83
const double fx
const Cal3_S2 & calibration() const
return calibration, same for left and right
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.
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:41:44