gtsam 4.1.1
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
21#include <iosfwd>
22
23namespace gtsam {
24
30class 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
157template <>
158struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
159
160template <>
161struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
162};
163
164} // \ namespace gtsam
The most common 5DOF 3D->2D calibration.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Template to create a binary predicate.
Definition: Testable.h:111
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
Definition: Cal3_S2.h:34
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
Convert image coordinates uv to intrinsic coordinates xy.
Definition: Cal3_S2.cpp:53
Definition: Cal3_S2Stereo.h:30
Vector6 vector() const
vectorized form (column-wise)
Definition: Cal3_S2Stereo.h:113
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition: Cal3_S2Stereo.h:47
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
Definition: Cal3_S2Stereo.h:136
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition: Cal3_S2Stereo.h:104
size_t dim() const override
return DOF, dimensionality of tangent space
Definition: Cal3_S2Stereo.h:124
Vector3 calibrate(const Vector3 &p) const
Convert homogeneous image coordinates to intrinsic coordinates.
Definition: Cal3_S2Stereo.h:83
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition: Cal3_S2Stereo.h:130
Matrix3 K() const override
return calibration matrix K, same for left and right
Definition: Cal3_S2Stereo.h:107
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3_S2Stereo.h:127
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
double baseline() const
return baseline
Definition: Cal3_S2Stereo.h:110
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition: Cal3_S2Stereo.h:51
Cal3_S2Stereo()=default
default calibration leaves coordinates unchanged