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
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
fov
static double fov
Definition: testProjectionFactor.cpp:33
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
gtsam::Cal3_S2::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:53
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
gtsam::Cal3_S2Stereo::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3_S2Stereo.h:128
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition: Cal3_S2Stereo.h:51
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
h
const double h
Definition: testSimpleHelicopter.cpp:19
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Cal3_S2Stereo::K
Matrix3 K() const override
return calibration matrix K, same for left and right
Definition: Cal3_S2Stereo.h:108
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:47
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
gtsam::Cal3_S2Stereo::calibrate
Vector3 calibrate(const Vector3 &p) const
Definition: Cal3_S2Stereo.h:85
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
gtsam::uncalibrate
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
Definition: slam/expressions.h:172
gtsam::equals
Definition: Testable.h:112
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Cal3_S2Stereo::vector
Vector6 vector() const
vectorized form (column-wise)
Definition: Cal3_S2Stereo.h:114
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Cal3_S2Stereo::baseline
double baseline() const
return baseline
Definition: Cal3_S2Stereo.h:111
gtsam::Cal3_S2Stereo::localCoordinates
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
Definition: Cal3_S2Stereo.h:137
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::tol
const G double tol
Definition: Group.h:79
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:55
py
int RealScalar int RealScalar * py
Definition: level1_cplx_impl.h:119
gtsam::Cal3_S2Stereo::retract
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition: Cal3_S2Stereo.h:131
gtsam::Cal3::shared_ptr
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
gtsam::Cal3_S2Stereo::calibration
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition: Cal3_S2Stereo.h:105
gtsam::Cal3_S2Stereo::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition: Cal3_S2Stereo.h:125
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
fy
const double fy
Definition: testSmartStereoFactor_iSAM2.cpp:48
px
RealScalar RealScalar * px
Definition: level1_cplx_impl.h:28


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:03