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 
21 #include <gtsam/geometry/Pose3.h>
23 
24 namespace gtsam {
25 
26 class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
27 public:
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 
39 private:
41 };
42 
47 class GTSAM_EXPORT StereoCamera {
48 
49 public:
50 
57 
58 private:
61 
62 public:
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 
146  {}, OptionalJacobian<3, 3> H2 = {}) const;
147 
149  Point3 backproject(const StereoPoint2& z) const;
150 
155  Point3 backproject2(const StereoPoint2& z,
156  OptionalJacobian<3, 6> H1 = {},
157  OptionalJacobian<3, 3> H2 = {}) const;
158 
162 
171  {}) const;
172 
175  return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());
176  }
177 
179 
180 private:
181 
182 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
183  friend class boost::serialization::access;
184  template<class Archive>
185  void serialize(Archive & ar, const unsigned int /*version*/) {
186  ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
187  ar & BOOST_SERIALIZATION_NVP(K_);
188  }
189 #endif
190 
191 };
192 
193 template<>
194 struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
195 };
196 
197 template<>
198 struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
199 };
200 }
#define max(a, b)
Definition: datatypes.h:20
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition: StereoCamera.h:95
std::string serialize(const T &input)
serializes to a string
StereoPoint2 Measurement
Definition: StereoCamera.h:55
Point2_ project2(const Expression< CAMERA > &camera_, const Expression< POINT > &p_)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
StereoCamera()
Default constructor allocates a calibration!
Definition: StereoCamera.h:72
Cal3_S2Stereo::shared_ptr K_
Definition: StereoCamera.h:60
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: StereoCamera.h:174
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:157
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
size_t dim() const
Dimensionality of the tangent space.
Definition: StereoCamera.h:105
std::vector< StereoPoint2 > StereoPoint2Vector
Definition: StereoPoint2.h:168
StereoPoint2Vector MeasurementVector
Definition: StereoCamera.h:56
const Pose3 & pose() const
pose
Definition: StereoCamera.h:129
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
static size_t Dim()
Dimensionality of the tangent space.
Definition: StereoCamera.h:110
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition: StereoCamera.h:120
Eigen::VectorXd Vector
Definition: Vector.h:38
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition: StereoCamera.h:115
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
void print(const std::string &s="") const
print
Definition: StereoCamera.h:89
traits
Definition: chartTesting.h:28
The most common 5DOF 3D->2D calibration + Stereo baseline.
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:152
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition: StereoCamera.h:80
A 2D stereo point (uL,uR,v)
double baseline() const
baseline
Definition: StereoCamera.h:134
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
The matrix class, also used for vectors and row-vectors.
static const CalibratedCamera camera(kDefaultPose)
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:36:20