gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
59
67 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
68 OptionalJacobian<2, 2> Dp = boost::none) const;
69
77 Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
78 OptionalJacobian<2, 2> Dp = boost::none) 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:
147 friend class boost::serialization::access;
148 template <class Archive>
149 void serialize(Archive& ar, const unsigned int /*version*/) {
150 ar& boost::serialization::make_nvp(
151 "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
152 ar& BOOST_SERIALIZATION_NVP(b_);
153 }
155};
156
157// Define GTSAM traits
158template <>
159struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
160
161template <>
162struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
163};
164
165} // \ namespace gtsam
The most common 5DOF 3D->2D calibration.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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:41
Template to create a binary predicate.
Definition Testable.h:111
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
Vector6 vector() const
vectorized form (column-wise)
Definition Cal3_S2Stereo.h:114
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:137
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition Cal3_S2Stereo.h:105
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:125
Vector3 calibrate(const Vector3 &p) const
Convert homogeneous image coordinates to intrinsic coordinates.
Definition Cal3_S2Stereo.h:85
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition Cal3_S2Stereo.h:131
Matrix3 K() const override
return calibration matrix K, same for left and right
Definition Cal3_S2Stereo.h:108
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:128
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:111
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition Cal3_S2Stereo.h:51
Cal3_S2Stereo()=default
default calibration leaves coordinates unchanged