gtsam 4.1.1
gtsam
StereoCamera.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
23
24namespace gtsam {
25
26class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
27public:
29 : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
30
32 : std::runtime_error("Stereo Cheirality Exception"),
33 j_(j) {}
34
35 Key nearbyVariable() const {
36 return j_;
37 }
38
39private:
40 Key j_;
41};
42
47class GTSAM_EXPORT StereoCamera {
48
49public:
50
56 typedef StereoPoint2Vector MeasurementVector;
57
58private:
59 Pose3 leftCamPose_;
60 Cal3_S2Stereo::shared_ptr K_;
61
62public:
63
64 enum {
65 dimension = 6
66 };
67
70
73 K_(new Cal3_S2Stereo()) {
74 }
75
77 StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
78
80 const Cal3_S2Stereo& calibration() const {
81 return *K_;
82 }
83
87
89 void print(const std::string& s = "") const {
90 leftCamPose_.print(s + ".camera.");
91 K_->print(s + ".calibration.");
92 }
93
95 bool equals(const StereoCamera &camera, double tol = 1e-9) const {
96 return leftCamPose_.equals(camera.leftCamPose_, tol)
97 && K_->equals(*camera.K_, tol);
98 }
99
103
105 inline size_t dim() const {
106 return 6;
107 }
108
110 static inline size_t Dim() {
111 return 6;
112 }
113
115 inline StereoCamera retract(const Vector& v) const {
116 return StereoCamera(pose().retract(v), K_);
117 }
118
120 inline Vector6 localCoordinates(const StereoCamera& t2) const {
121 return leftCamPose_.localCoordinates(t2.leftCamPose_);
122 }
123
127
129 const Pose3& pose() const {
130 return leftCamPose_;
131 }
132
134 double baseline() const {
135 return K_->baseline();
136 }
137
139 StereoPoint2 project(const Point3& point) const;
140
145 StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
146 boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
147
149 Point3 backproject(const StereoPoint2& z) const;
150
155 Point3 backproject2(const StereoPoint2& z,
156 OptionalJacobian<3, 6> H1 = boost::none,
157 OptionalJacobian<3, 3> H2 = boost::none) const;
158
162
171 boost::none) const;
172
174
175private:
176
177 friend class boost::serialization::access;
178 template<class Archive>
179 void serialize(Archive & ar, const unsigned int /*version*/) {
180 ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
181 ar & BOOST_SERIALIZATION_NVP(K_);
182 }
183
184};
185
186template<>
187struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
188};
189
190template<>
191struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
192};
193}
The most common 5DOF 3D->2D calibration + Stereo baseline.
A 2D stereo point (uL,uR,v)
3D Pose
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
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:96
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
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
Definition: Cal3_S2Stereo.h:30
Definition: Pose3.h:37
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:158
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:153
Definition: StereoCamera.h:26
Definition: StereoCamera.h:47
StereoCamera()
Default constructor allocates a calibration!
Definition: StereoCamera.h:72
static size_t Dim()
Dimensionality of the tangent space.
Definition: StereoCamera.h:110
void print(const std::string &s="") const
print
Definition: StereoCamera.h:89
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition: StereoCamera.h:120
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition: StereoCamera.h:95
size_t dim() const
Dimensionality of the tangent space.
Definition: StereoCamera.h:105
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition: StereoCamera.h:115
StereoPoint2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition: StereoCamera.h:55
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition: StereoCamera.h:80
double baseline() const
baseline
Definition: StereoCamera.h:134
const Pose3 & pose() const
pose
Definition: StereoCamera.h:129
Definition: StereoPoint2.h:32