gtsam 4.2.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
24namespace gtsam {
25
32template<typename Calibration>
33class PinholeCamera: public PinholeBaseK<Calibration> {
34
35public:
36
42 typedef Point2Vector MeasurementVector;
43
44private:
45
47 Calibration K_;
48
49 // Get dimensions of calibration type at compile time
50 static const int DimK = FixedDimension<Calibration>::value;
51
52public:
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 override {
152 Base::print(s);
153 K_.print(s + ".calibration");
154 }
155
159
160 ~PinholeCamera() override {
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 if (Dcamera){
234 Matrix26 Dpose;
235 Eigen::Matrix<double, 2, DimK> Dcal;
236 const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
237 *Dcamera << Dpose, Dcal;
238 return pi;
239 } else {
240 return Base::project(pw, boost::none, Dpoint, boost::none);
241 }
242 }
243
246 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
247 return _project2(pw, Dcamera, Dpoint);
248 }
249
252 boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
253 return _project2(pw, Dcamera, Dpoint);
254 }
255
261 double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
262 boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
263 Matrix16 Dpose_;
264 double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
265 if (Dcamera)
266 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
267 return result;
268 }
269
276 boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
277 Matrix16 Dpose_;
278 double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
279 if (Dcamera)
280 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
281 return result;
282 }
283
289 template<class CalibrationB>
290 double range(const PinholeCamera<CalibrationB>& camera,
291 OptionalJacobian<1, dimension> Dcamera = boost::none,
292 OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
293 Matrix16 Dcamera_, Dother_;
294 double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
295 Dother ? &Dother_ : 0);
296 if (Dcamera) {
297 *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
298 }
299 if (Dother) {
300 Dother->setZero();
301 Dother->template block<1, 6>(0, 0) = Dother_;
302 }
303 return result;
304 }
305
311 double range(const CalibratedCamera& camera,
312 OptionalJacobian<1, dimension> Dcamera = boost::none,
313 OptionalJacobian<1, 6> Dother = boost::none) const {
314 return range(camera.pose(), Dcamera, Dother);
315 }
316
318 Matrix34 cameraProjectionMatrix() const {
319 return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
320 }
321
324 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
325 }
326
327private:
328
331 template<class Archive>
332 void serialize(Archive & ar, const unsigned int /*version*/) {
333 ar
334 & boost::serialization::make_nvp("PinholeBaseK",
335 boost::serialization::base_object<Base>(*this));
336 ar & BOOST_SERIALIZATION_NVP(K_);
337 }
338
339public:
341};
342
343// manifold traits
344
345template <typename Calibration>
346struct traits<PinholeCamera<Calibration> >
347 : public internal::Manifold<PinholeCamera<Calibration> > {};
348
349template <typename Calibration>
350struct traits<const PinholeCamera<Calibration> >
351 : public internal::Manifold<PinholeCamera<Calibration> > {};
352
353// range traits, used in RangeFactor
354template <typename Calibration, typename T>
355struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
356
357} // \ gtsam
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:308
Pinhole camera with known calibration.
Bearing-Range product.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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
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:36
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Definition: BearingRange.h:40
Definition: BearingRange.h:194
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition: CalibratedCamera.cpp:27
virtual void print(const std::string &s="PinholeBase") const
print
Definition: CalibratedCamera.cpp:74
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:152
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition: CalibratedCamera.cpp:49
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: CalibratedCamera.cpp:69
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition: CalibratedCamera.cpp:37
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
A Calibrated camera class [R|-R't], calibration K=I.
Definition: CalibratedCamera.h:247
A pinhole camera class that has a Pose3 and a Calibration.
Definition: PinholeCamera.h:33
void print(const std::string &s="PinholeCamera") const override
print
Definition: PinholeCamera.h:151
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
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: PinholeCamera.h:323
const Calibration & calibration() const override
return calibration
Definition: PinholeCamera.h:178
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition: PinholeCamera.h:169
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:245
PinholeCamera(const Pose3 &pose)
constructor with pose
Definition: PinholeCamera.h:66
static PinholeCamera Level(const Pose2 &pose2, double height)
PinholeCamera::level with default calibration.
Definition: PinholeCamera.h:92
PinholeCamera(const Pose3 &pose, const Calibration &K)
constructor with pose and calibration
Definition: PinholeCamera.h:71
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
size_t dim() const
Definition: PinholeCamera.h:187
static PinholeCamera Identity()
for Canonical
Definition: PinholeCamera.h:216
static size_t Dim()
Definition: PinholeCamera.h:192
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: PinholeCamera.h:145
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:275
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:290
PinholeCamera(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition: PinholeCamera.h:136
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
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition: PinholeCamera.h:318
VectorK6 localCoordinates(const PinholeCamera &T2) const
return canonical coordinate
Definition: PinholeCamera.h:208
PinholeCamera retract(const Vector &d) const
move a cameras according to d
Definition: PinholeCamera.h:199
friend class boost::serialization::access
Serialization function.
Definition: PinholeCamera.h:330
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:251
const Pose3 & pose() const
return pose
Definition: PinholeCamera.h:164
PinholeCamera(const Vector &v)
Init from vector, can be 6D (default calibration) or dim.
Definition: PinholeCamera.h:129
PinholeCamera()
default constructor
Definition: PinholeCamera.h:62
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition: PinholeCamera.h:41
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:311
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:261
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition: PinholePose.h:34
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
A 2D pose (Point2,Rot2)
Definition: Pose2.h:36
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
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:399
Matrix4 matrix() const
convert to 4*4 matrix
Definition: Pose3.cpp:323
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
Represents a 3D point on a unit sphere.
Definition: Unit3.h:43