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
12/**
13 * @file StereoCamera.h
14 * @brief A Rectified Stereo Camera
15 * @author Chris Beall
16 */
17
18#pragma once
19
20#include <gtsam/geometry/Cal3_S2Stereo.h>
21#include <gtsam/geometry/Pose3.h>
22#include <gtsam/geometry/StereoPoint2.h>
23
24namespace gtsam {
25
26class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
27public:
28 StereoCheiralityException()
29 : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
30
31 StereoCheiralityException(Key j)
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
43/**
44 * A stereo camera class, parameterize by left camera pose and stereo calibration
45 * @ingroup geometry
46 */
47class GTSAM_EXPORT StereoCamera {
48
49public:
50
51 /**
52 * Some classes template on either PinholeCamera or StereoCamera,
53 * and this typedef informs those classes what "project" returns.
54 */
55 using Measurement = StereoPoint2;
56 using MeasurementVector = StereoPoint2Vector;
57
58private:
59 Pose3 leftCamPose_;
60 Cal3_S2Stereo::shared_ptr K_;
61
62public:
63
64 inline constexpr static auto dimension = 6;
65
66 /// @name Standard Constructors
67 /// @{
68
69 /// Default constructor allocates a calibration!
70 StereoCamera() :
71 K_(new Cal3_S2Stereo()) {
72 }
73
74 /// Construct from pose and shared calibration
75 StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
76
77 /// Return shared pointer to calibration
78 const Cal3_S2Stereo& calibration() const {
79 return *K_;
80 }
81
82 /// @}
83 /// @name Testable
84 /// @{
85
86 /// print
87 void print(const std::string& s = "") const {
88 leftCamPose_.print(s: s + ".camera.");
89 K_->print(s: s + ".calibration.");
90 }
91
92 /// equals
93 bool equals(const StereoCamera &camera, double tol = 1e-9) const {
94 return leftCamPose_.equals(pose: camera.leftCamPose_, tol)
95 && K_->equals(other: *camera.K_, tol);
96 }
97
98 /// @}
99 /// @name Manifold
100 /// @{
101
102 /// Dimensionality of the tangent space
103 inline size_t dim() const {
104 return 6;
105 }
106
107 /// Dimensionality of the tangent space
108 static inline size_t Dim() {
109 return 6;
110 }
111
112 /// Updates a with tangent space delta
113 inline StereoCamera retract(const Vector& v) const {
114 return StereoCamera(pose().retract(v), K_);
115 }
116
117 /// Local coordinates of manifold neighborhood around current value
118 inline Vector6 localCoordinates(const StereoCamera& t2) const {
119 return leftCamPose_.localCoordinates(g: t2.leftCamPose_);
120 }
121
122 /// @}
123 /// @name Standard Interface
124 /// @{
125
126 /// pose
127 const Pose3& pose() const {
128 return leftCamPose_;
129 }
130
131 /// baseline
132 double baseline() const {
133 return K_->baseline();
134 }
135
136 /// Project 3D point to StereoPoint2 (uL,uR,v)
137 StereoPoint2 project(const Point3& point) const;
138
139 /** Project 3D point and compute optional derivatives
140 * @param H1 derivative with respect to pose
141 * @param H2 derivative with respect to point
142 */
143 StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
144 {}, OptionalJacobian<3, 3> H2 = {}) const;
145
146 /// back-project a measurement
147 Point3 backproject(const StereoPoint2& z) const;
148
149 /** Back-project the 2D point and compute optional derivatives
150 * @param H1 derivative with respect to pose
151 * @param H2 derivative with respect to point
152 */
153 Point3 backproject2(const StereoPoint2& z,
154 OptionalJacobian<3, 6> H1 = {},
155 OptionalJacobian<3, 3> H2 = {}) const;
156
157 /// @}
158 /// @name Deprecated
159 /// @{
160
161 /** Project 3D point and compute optional derivatives
162 * @deprecated, use project2 - this class has fixed calibration
163 * @param H1 derivative with respect to pose
164 * @param H2 derivative with respect to point
165 * @param H3 IGNORED (for calibration)
166 */
167 StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
168 OptionalJacobian<3, 3> H2 = {}, OptionalJacobian<3, 0> H3 =
169 {}) const;
170
171 /// for Nonlinear Triangulation
172 Vector defaultErrorWhenTriangulatingBehindCamera() const {
173 return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(value: 2.0 * K_->fx());
174 }
175
176 /// @}
177
178private:
179
180#if GTSAM_ENABLE_BOOST_SERIALIZATION
181 friend class boost::serialization::access;
182 template<class Archive>
183 void serialize(Archive & ar, const unsigned int /*version*/) {
184 ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
185 ar & BOOST_SERIALIZATION_NVP(K_);
186 }
187#endif
188
189};
190
191template<>
192struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
193};
194
195template<>
196struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
197};
198}
199