gtsam  4.1.0
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 
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:
40  Key j_;
41 };
42 
47 class GTSAM_EXPORT StereoCamera {
48 
49 public:
50 
56  typedef StereoPoint2Vector MeasurementVector;
57 
58 private:
59  Pose3 leftCamPose_;
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 
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 
170  OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
171  boost::none) const;
172 
174 
175 private:
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 
186 template<>
187 struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
188 };
189 
190 template<>
191 struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
192 };
193 }
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
gtsam::StereoCamera::localCoordinates
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition: StereoCamera.h:120
gtsam::StereoCheiralityException
Definition: StereoCamera.h:26
gtsam::Pose3::equals
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:114
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::StereoPoint2
Definition: StereoPoint2.h:32
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::StereoCamera::Measurement
StereoPoint2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition: StereoCamera.h:55
gtsam::StereoCamera::pose
const Pose3 & pose() const
pose
Definition: StereoCamera.h:129
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::Pose3::print
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:109
Pose3.h
3D Pose
gtsam::StereoCamera::calibration
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition: StereoCamera.h:80
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::StereoCamera::baseline
double baseline() const
baseline
Definition: StereoCamera.h:134
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:135
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:84
gtsam::StereoCamera::equals
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition: StereoCamera.h:95
gtsam::StereoCamera::print
void print(const std::string &s="") const
print
Definition: StereoCamera.h:89
gtsam::StereoCamera::StereoCamera
StereoCamera()
Default constructor allocates a calibration!
Definition: StereoCamera.h:72
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Pose3
Definition: Pose3.h:37
gtsam::StereoCamera
Definition: StereoCamera.h:47
gtsam::Point3
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
StereoPoint2.h
A 2D stereo point (uL,uR,v)
gtsam::Cal3_S2Stereo::shared_ptr
boost::shared_ptr< Cal3_S2Stereo > shared_ptr
shared pointer to stereo calibration object
Definition: Cal3_S2Stereo.h:39
gtsam::StereoCamera::retract
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition: StereoCamera.h:115
gtsam::Cal3_S2Stereo
Definition: Cal3_S2Stereo.h:30
gtsam::StereoCamera::Dim
static size_t Dim()
Dimensionality of the tangent space.
Definition: StereoCamera.h:110
gtsam::StereoCamera::dim
size_t dim() const
Dimensionality of the tangent space.
Definition: StereoCamera.h:105