gtsam  4.1.0
gtsam
PinholeCamera.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 
23 
24 namespace gtsam {
25 
32 template<typename Calibration>
33 class PinholeCamera: public PinholeBaseK<Calibration> {
34 
35 public:
36 
42  typedef Point2Vector MeasurementVector;
43 
44 private:
45 
47  Calibration K_;
48 
49  // Get dimensions of calibration type at compile time
50  static const int DimK = FixedDimension<Calibration>::value;
51 
52 public:
53 
54  enum {
55  dimension = 6 + DimK
56  };
57 
60 
63  }
64 
66  explicit PinholeCamera(const Pose3& pose) :
67  Base(pose) {
68  }
69 
71  PinholeCamera(const Pose3& pose, const Calibration& K) :
72  Base(pose), K_(K) {
73  }
74 
78 
86  static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
87  double height) {
88  return PinholeCamera(Base::LevelPose(pose2, height), K);
89  }
90 
92  static PinholeCamera Level(const Pose2& pose2, double height) {
93  return PinholeCamera::Level(Calibration(), pose2, height);
94  }
95 
105  static PinholeCamera Lookat(const Point3& eye, const Point3& target,
106  const Point3& upVector, const Calibration& K = Calibration()) {
107  return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
108  }
109 
110  // Create PinholeCamera, with derivatives
111  static PinholeCamera Create(const Pose3& pose, const Calibration &K,
112  OptionalJacobian<dimension, 6> H1 = boost::none, //
113  OptionalJacobian<dimension, DimK> H2 = boost::none) {
114  typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
115  if (H1)
116  *H1 << I_6x6, MatrixK6::Zero();
117  typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
118  typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
119  if (H2)
120  *H2 << Matrix6K::Zero(), MatrixK::Identity();
121  return PinholeCamera(pose,K);
122  }
123 
127 
129  explicit PinholeCamera(const Vector &v) :
130  Base(v.head<6>()) {
131  if (v.size() > 6)
132  K_ = Calibration(v.tail<DimK>());
133  }
134 
136  PinholeCamera(const Vector &v, const Vector &K) :
137  Base(v), K_(K) {
138  }
139 
143 
145  bool equals(const Base &camera, double tol = 1e-9) const {
146  const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
147  return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
148  }
149 
151  void print(const std::string& s = "PinholeCamera") const {
152  Base::print(s);
153  K_.print(s + ".calibration");
154  }
155 
159 
160  virtual ~PinholeCamera() {
161  }
162 
164  const Pose3& pose() const {
165  return Base::pose();
166  }
167 
170  if (H) {
171  H->setZero();
172  H->template block<6, 6>(0, 0) = I_6x6;
173  }
174  return Base::pose();
175  }
176 
178  const Calibration& calibration() const override {
179  return K_;
180  }
181 
185 
187  size_t dim() const {
188  return dimension;
189  }
190 
192  static size_t Dim() {
193  return dimension;
194  }
195 
196  typedef Eigen::Matrix<double, dimension, 1> VectorK6;
197 
199  PinholeCamera retract(const Vector& d) const {
200  if ((size_t) d.size() == 6)
201  return PinholeCamera(this->pose().retract(d), calibration());
202  else
203  return PinholeCamera(this->pose().retract(d.head<6>()),
204  calibration().retract(d.tail(calibration().dim())));
205  }
206 
208  VectorK6 localCoordinates(const PinholeCamera& T2) const {
209  VectorK6 d;
210  d.template head<6>() = this->pose().localCoordinates(T2.pose());
211  d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
212  return d;
213  }
214 
217  return PinholeCamera(); // assumes that the default constructor is valid
218  }
219 
223 
224  typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
225 
229  template<class POINT>
232  // We just call 3-derivative version in Base
233  Matrix26 Dpose;
234  Eigen::Matrix<double, 2, DimK> Dcal;
235  Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
236  Dcamera ? &Dcal : 0);
237  if (Dcamera)
238  *Dcamera << Dpose, Dcal;
239  return pi;
240  }
241 
244  boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
245  return _project2(pw, Dcamera, Dpoint);
246  }
247 
250  boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
251  return _project2(pw, Dcamera, Dpoint);
252  }
253 
259  double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
260  boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
261  Matrix16 Dpose_;
262  double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
263  if (Dcamera)
264  *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
265  return result;
266  }
267 
274  boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
275  Matrix16 Dpose_;
276  double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
277  if (Dcamera)
278  *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
279  return result;
280  }
281 
287  template<class CalibrationB>
288  double range(const PinholeCamera<CalibrationB>& camera,
289  OptionalJacobian<1, dimension> Dcamera = boost::none,
290  OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
291  Matrix16 Dcamera_, Dother_;
292  double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
293  Dother ? &Dother_ : 0);
294  if (Dcamera) {
295  *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
296  }
297  if (Dother) {
298  Dother->setZero();
299  Dother->template block<1, 6>(0, 0) = Dother_;
300  }
301  return result;
302  }
303 
309  double range(const CalibratedCamera& camera,
310  OptionalJacobian<1, dimension> Dcamera = boost::none,
311  OptionalJacobian<1, 6> Dother = boost::none) const {
312  return range(camera.pose(), Dcamera, Dother);
313  }
314 
315 private:
316 
319  template<class Archive>
320  void serialize(Archive & ar, const unsigned int /*version*/) {
321  ar
322  & boost::serialization::make_nvp("PinholeBaseK",
323  boost::serialization::base_object<Base>(*this));
324  ar & BOOST_SERIALIZATION_NVP(K_);
325  }
326 
327 public:
329 };
330 
331 // manifold traits
332 
333 template <typename Calibration>
334 struct traits<PinholeCamera<Calibration> >
335  : public internal::Manifold<PinholeCamera<Calibration> > {};
336 
337 template <typename Calibration>
338 struct traits<const PinholeCamera<Calibration> >
339  : public internal::Manifold<PinholeCamera<Calibration> > {};
340 
341 // range traits, used in RangeFactor
342 template <typename Calibration, typename T>
343 struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
344 
345 } // \ gtsam
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition: CalibratedCamera.cpp:37
gtsam::HasRange
Definition: BearingRange.h:193
gtsam::PinholeBaseK
Definition: PinholePose.h:34
BearingRange.h
Bearing-Range product.
gtsam::CalibratedCamera
Definition: CalibratedCamera.h:249
gtsam::PinholeBaseK< Calibration >::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
gtsam::PinholeCamera::pose
const Pose3 & pose() const
return pose
Definition: PinholeCamera.h:164
gtsam::PinholeCamera::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholeCamera.h:243
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::PinholeCamera::range
double range(const Point3 &point, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition: PinholeCamera.h:259
gtsam::PinholeCamera::print
void print(const std::string &s="PinholeCamera") const
print
Definition: PinholeCamera.h:151
gtsam::PinholeCamera::dim
size_t dim() const
Definition: PinholeCamera.h:187
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Pose2 &pose2, double height)
PinholeCamera::level with default calibration.
Definition: PinholeCamera.h:92
gtsam::PinholeCamera::range
double range(const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6+CalibrationB::dimension > Dother=boost::none) const
Calculate range to another camera.
Definition: PinholeCamera.h:288
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: CalibratedCamera.cpp:69
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
gtsam::Pose2
Definition: Pose2.h:36
gtsam::PinholeCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a calibrated camera.
Definition: PinholeCamera.h:309
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
PinholePose.h
Pinhole camera with known calibration.
gtsam::PinholeCamera::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project a point at infinity from world coordinates into the image
Definition: PinholeCamera.h:249
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition: PinholeCamera.h:136
gtsam::PinholeCamera::Dim
static size_t Dim()
Definition: PinholeCamera.h:192
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::PinholeCamera::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: PinholeCamera.h:145
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose, const Calibration &K)
constructor with pose and calibration
Definition: PinholeCamera.h:71
gtsam::PinholeCamera::retract
PinholeCamera retract(const Vector &d) const
move a cameras according to d
Definition: PinholeCamera.h:199
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Calibration &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition: PinholeCamera.h:86
gtsam::PinholeCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition: PinholeCamera.h:273
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::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::PinholeCamera::_project2
Point2 _project2(const POINT &pw, OptionalJacobian< 2, dimension > Dcamera, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint) const
Templated projection of a 3D point or a point at infinity into the image.
Definition: PinholeCamera.h:230
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::PinholeCamera::identity
static PinholeCamera identity()
for Canonical
Definition: PinholeCamera.h:216
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::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v)
Init from vector, can be 6D (default calibration) or dim.
Definition: PinholeCamera.h:129
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition: CalibratedCamera.cpp:49
gtsam::PinholeCamera::calibration
const Calibration & calibration() const override
return calibration
Definition: PinholeCamera.h:178
gtsam::PinholeCamera::PinholeCamera
PinholeCamera()
default constructor
Definition: PinholeCamera.h:62
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::PinholeCamera::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition: PinholeCamera.h:41
gtsam::PinholeCamera::access
friend class boost::serialization::access
Serialization function.
Definition: PinholeCamera.h:318
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition: CalibratedCamera.cpp:27
gtsam::PinholeCamera::getPose
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition: PinholeCamera.h:169
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Create a camera pose at the given eye position looking at a target point in the scene with the specif...
Definition: CalibratedCamera.cpp:58
gtsam::PinholeCamera::localCoordinates
VectorK6 localCoordinates(const PinholeCamera &T2) const
return canonical coordinate
Definition: PinholeCamera.h:208
gtsam::PinholeCamera::Lookat
static PinholeCamera Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const Calibration &K=Calibration())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition: PinholeCamera.h:105
gtsam::PinholeBase::print
void print(const std::string &s="PinholeBase") const
print
Definition: CalibratedCamera.cpp:74
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose)
constructor with pose
Definition: PinholeCamera.h:66