gtsam  4.1.0
gtsam
CalibratedCamera.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 
19 #pragma once
20 
22 #include <gtsam/geometry/Point2.h>
23 #include <gtsam/geometry/Pose3.h>
24 #include <gtsam/base/concepts.h>
25 #include <gtsam/base/Manifold.h>
26 #include <gtsam/base/ThreadsafeException.h>
27 #include <gtsam/dllexport.h>
28 #include <boost/serialization/nvp.hpp>
29 
30 namespace gtsam {
31 
32 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
33 public:
35  : CheiralityException(std::numeric_limits<Key>::max()) {}
36 
38  : ThreadsafeException<CheiralityException>("CheiralityException"),
39  j_(j) {}
40 
41  Key nearbyVariable() const {return j_;}
42 
43 private:
44  Key j_;
45 };
46 
52 class GTSAM_EXPORT PinholeBase {
53 
54 public:
55 
57  typedef Rot3 Rotation;
58  typedef Point3 Translation;
59 
65  typedef Point2Vector MeasurementVector;
66 
67 private:
68 
69  Pose3 pose_;
70 
71 protected:
72 
75 
81  static Matrix26 Dpose(const Point2& pn, double d);
82 
89  static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
90 
92 
93 public:
94 
97 
105  static Pose3 LevelPose(const Pose2& pose2, double height);
106 
115  static Pose3 LookatPose(const Point3& eye, const Point3& target,
116  const Point3& upVector);
117 
121 
124  }
125 
127  explicit PinholeBase(const Pose3& pose) :
128  pose_(pose) {
129  }
130 
134 
135  explicit PinholeBase(const Vector &v) :
136  pose_(Pose3::Expmap(v)) {
137  }
138 
142 
144  bool equals(const PinholeBase &camera, double tol = 1e-9) const;
145 
147  void print(const std::string& s = "PinholeBase") const;
148 
152 
154  const Pose3& pose() const {
155  return pose_;
156  }
157 
159  const Rot3& rotation() const {
160  return pose_.rotation();
161  }
162 
164  const Point3& translation() const {
165  return pose_.translation();
166  }
167 
169  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
170 
174 
180  static Point2 Project(const Point3& pc, //
181  OptionalJacobian<2, 3> Dpoint = boost::none);
182 
188  static Point2 Project(const Unit3& pc, //
189  OptionalJacobian<2, 2> Dpoint = boost::none);
190 
192  std::pair<Point2, bool> projectSafe(const Point3& pw) const;
193 
199  Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
200  boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
201 
207  Point2 project2(const Unit3& point,
208  OptionalJacobian<2, 6> Dpose = boost::none,
209  OptionalJacobian<2, 2> Dpoint = boost::none) const;
210 
212  static Point3 BackprojectFromCamera(const Point2& p, const double depth,
213  OptionalJacobian<3, 2> Dpoint = boost::none,
214  OptionalJacobian<3, 1> Ddepth = boost::none);
215 
219 
225  inline static std::pair<size_t, size_t> translationInterval() {
226  return std::make_pair(3, 5);
227  }
228 
230 
231 private:
232 
234  friend class boost::serialization::access;
235  template<class Archive>
236  void serialize(Archive & ar, const unsigned int /*version*/) {
237  ar & BOOST_SERIALIZATION_NVP(pose_);
238  }
239 };
240 // end of class PinholeBase
241 
249 class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
250 
251 public:
252 
253  enum {
254  dimension = 6
255  };
256 
259 
262  }
263 
265  explicit CalibratedCamera(const Pose3& pose) :
266  PinholeBase(pose) {
267  }
268 
272 
273  // Create CalibratedCamera, with derivatives
274  static CalibratedCamera Create(const Pose3& pose,
275  OptionalJacobian<dimension, 6> H1 = boost::none) {
276  if (H1)
277  *H1 << I_6x6;
278  return CalibratedCamera(pose);
279  }
280 
287  static CalibratedCamera Level(const Pose2& pose2, double height);
288 
297  static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
298  const Point3& upVector);
299 
303 
305  explicit CalibratedCamera(const Vector &v) :
306  PinholeBase(v) {
307  }
308 
312 
314  virtual ~CalibratedCamera() {
315  }
316 
320 
322  CalibratedCamera retract(const Vector& d) const;
323 
325  Vector localCoordinates(const CalibratedCamera& T2) const;
326 
328  inline size_t dim() const {
329  return dimension;
330  }
331 
333  inline static size_t Dim() {
334  return dimension;
335  }
336 
340 
345  Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
346  boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
347 
349  Point3 backproject(const Point2& pn, double depth,
350  OptionalJacobian<3, 6> Dresult_dpose = boost::none,
351  OptionalJacobian<3, 2> Dresult_dp = boost::none,
352  OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
353 
354  Matrix32 Dpoint_dpn;
355  Matrix31 Dpoint_ddepth;
356  const Point3 point = BackprojectFromCamera(pn, depth,
357  Dresult_dp ? &Dpoint_dpn : 0,
358  Dresult_ddepth ? &Dpoint_ddepth : 0);
359 
360  Matrix33 Dresult_dpoint;
361  const Point3 result = pose().transformFrom(point, Dresult_dpose,
362  (Dresult_ddepth ||
363  Dresult_dp) ? &Dresult_dpoint : 0);
364 
365  if (Dresult_dp)
366  *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
367  if (Dresult_ddepth)
368  *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
369 
370  return result;
371  }
372 
378  double range(const Point3& point,
379  OptionalJacobian<1, 6> Dcamera = boost::none,
380  OptionalJacobian<1, 3> Dpoint = boost::none) const {
381  return pose().range(point, Dcamera, Dpoint);
382  }
383 
389  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
390  OptionalJacobian<1, 6> Dpose = boost::none) const {
391  return this->pose().range(pose, Dcamera, Dpose);
392  }
393 
399  double range(const CalibratedCamera& camera, //
400  OptionalJacobian<1, 6> H1 = boost::none, //
401  OptionalJacobian<1, 6> H2 = boost::none) const {
402  return pose().range(camera.pose(), H1, H2);
403  }
404 
406 
407 private:
408 
411 
413  friend class boost::serialization::access;
414  template<class Archive>
415  void serialize(Archive & ar, const unsigned int /*version*/) {
416  ar
417  & boost::serialization::make_nvp("PinholeBase",
418  boost::serialization::base_object<PinholeBase>(*this));
419  }
420 
422 };
423 
424 // manifold traits
425 template <>
426 struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
427 
428 template <>
429 struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
430 
431 // range traits, used in RangeFactor
432 template <typename T>
433 struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
434 
435 } // namespace gtsam
gtsam::CalibratedCamera::~CalibratedCamera
virtual ~CalibratedCamera()
destructor
Definition: CalibratedCamera.h:314
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
gtsam::PinholeBase::PinholeBase
PinholeBase(const Pose3 &pose)
constructor with pose
Definition: CalibratedCamera.h:127
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Pose3 &pose)
construct with pose
Definition: CalibratedCamera.h:265
gtsam::HasRange
Definition: BearingRange.h:193
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:298
BearingRange.h
Bearing-Range product.
gtsam::CalibratedCamera
Definition: CalibratedCamera.h:249
gtsam::PinholeBase::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition: CalibratedCamera.h:57
gtsam::CheiralityException
Definition: CalibratedCamera.h:32
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::PinholeBase::rotation
const Rot3 & rotation() const
get rotation
Definition: CalibratedCamera.h:159
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::CalibratedCamera::dim
size_t dim() const
Definition: CalibratedCamera.h:328
gtsam::PinholeBase::translation
const Point3 & translation() const
get translation
Definition: CalibratedCamera.h:164
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
Pose3.h
3D Pose
gtsam::Pose2
Definition: Pose2.h:36
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera()
default constructor
Definition: CalibratedCamera.h:261
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Vector &v)
construct from vector
Definition: CalibratedCamera.h:305
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:84
gtsam::CalibratedCamera::backproject
Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: CalibratedCamera.h:349
gtsam::Point2
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
gtsam::Rot3
Definition: Rot3.h:59
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition: Pose3.cpp:334
gtsam::PinholeBase::PinholeBase
PinholeBase()
default constructor
Definition: CalibratedCamera.h:123
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Range
Definition: BearingRange.h:39
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::PinholeBase
Definition: CalibratedCamera.h:52
gtsam::Pose3
Definition: Pose3.h:37
Point2.h
2D Point
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
gtsam::PinholeBase::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition: CalibratedCamera.h:64
gtsam::CalibratedCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition: CalibratedCamera.h:389
gtsam::CalibratedCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > H1=boost::none, OptionalJacobian< 1, 6 > H2=boost::none) const
Calculate range to another camera.
Definition: CalibratedCamera.h:399
Manifold.h
Base class and basic functions for Manifold types.
gtsam::ThreadsafeException
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition: ThreadsafeException.h:42
gtsam::PinholeBase::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition: CalibratedCamera.h:225
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
gtsam::CalibratedCamera::Dim
static size_t Dim()
Definition: CalibratedCamera.h:333
gtsam::CalibratedCamera::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition: CalibratedCamera.h:378