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
12/**
13 * @file CalibratedCamera.h
14 * @brief Calibrated camera for which only pose is unknown
15 * @date Aug 17, 2009
16 * @author Frank Dellaert
17 */
18
19#pragma once
20
21#include <gtsam/geometry/BearingRange.h>
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#if GTSAM_ENABLE_BOOST_SERIALIZATION
29#include <boost/serialization/nvp.hpp>
30#endif
31
32namespace gtsam {
33
34class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
35public:
36 CheiralityException()
37 : CheiralityException(std::numeric_limits<Key>::max()) {}
38
39 CheiralityException(Key j)
40 : ThreadsafeException<CheiralityException>("CheiralityException"),
41 j_(j) {}
42
43 Key nearbyVariable() const {return j_;}
44
45private:
46 Key j_;
47};
48
49/**
50 * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
51 * @ingroup geometry
52 * \nosubgrouping
53 */
54class GTSAM_EXPORT PinholeBase {
55
56public:
57
58 /** Pose Concept requirements */
59 using Rotation = Rot3;
60 using Translation = Point3;
61
62 /**
63 * Some classes template on either PinholeCamera or StereoCamera,
64 * and this typedef informs those classes what "project" returns.
65 */
66 using Measurement = Point2;
67 using MeasurementVector = Point2Vector;
68
69private:
70
71 Pose3 pose_; ///< 3D pose of camera
72
73protected:
74
75 /// @name Derivatives
76 /// @{
77
78 /**
79 * Calculate Jacobian with respect to pose
80 * @param pn projection in normalized coordinates
81 * @param d disparity (inverse depth)
82 */
83 static Matrix26 Dpose(const Point2& pn, double d);
84
85 /**
86 * Calculate Jacobian with respect to point
87 * @param pn projection in normalized coordinates
88 * @param d disparity (inverse depth)
89 * @param Rt transposed rotation matrix
90 */
91 static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
92
93 /// @}
94
95public:
96
97 /// @name Static functions
98 /// @{
99
100 /**
101 * Create a level pose at the given 2D pose and height
102 * @param K the calibration
103 * @param pose2 specifies the location and viewing direction
104 * (theta 0 = looking in direction of positive X axis)
105 * @param height camera height
106 */
107 static Pose3 LevelPose(const Pose2& pose2, double height);
108
109 /**
110 * Create a camera pose at the given eye position looking at a target point in the scene
111 * with the specified up direction vector.
112 * @param eye specifies the camera position
113 * @param target the point to look at
114 * @param upVector specifies the camera up direction vector,
115 * doesn't need to be on the image plane nor orthogonal to the viewing axis
116 */
117 static Pose3 LookatPose(const Point3& eye, const Point3& target,
118 const Point3& upVector);
119
120 /// @}
121 /// @name Standard Constructors
122 /// @{
123
124 /// Default constructor
125 PinholeBase() {}
126
127 /// Constructor with pose
128 explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
129
130 /// @}
131 /// @name Advanced Constructors
132 /// @{
133
134 explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(xi: v)) {}
135
136 /// Default destructor
137 virtual ~PinholeBase() = default;
138
139 /// @}
140 /// @name Testable
141 /// @{
142
143 /// assert equality up to a tolerance
144 bool equals(const PinholeBase &camera, double tol = 1e-9) const;
145
146 /// print
147 virtual void print(const std::string& s = "PinholeBase") const;
148
149 /// @}
150 /// @name Standard Interface
151 /// @{
152
153 /// return pose, constant version
154 const Pose3& pose() const {
155 return pose_;
156 }
157
158 /// get rotation
159 const Rot3& rotation() const {
160 return pose_.rotation();
161 }
162
163 /// get translation
164 const Point3& translation() const {
165 return pose_.translation();
166 }
167
168 /// return pose, with derivative
169 const Pose3& getPose(OptionalJacobian<6, 6> H) const;
170
171 /// @}
172 /// @name Transformations and measurement functions
173 /// @{
174
175 /**
176 * Project from 3D point in camera coordinates into image
177 * Does *not* throw a CheiralityException, even if pc behind image plane
178 * @param pc point in camera coordinates
179 */
180 static Point2 Project(const Point3& pc, //
181 OptionalJacobian<2, 3> Dpoint = {});
182
183 /**
184 * Project from 3D point at infinity in camera coordinates into image
185 * Does *not* throw a CheiralityException, even if pc behind image plane
186 * @param pc point in camera coordinates
187 */
188 static Point2 Project(const Unit3& pc, //
189 OptionalJacobian<2, 2> Dpoint = {});
190
191 /// Project a point into the image and check depth
192 std::pair<Point2, bool> projectSafe(const Point3& pw) const;
193
194 /** Project point into the image
195 * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
196 * @param point 3D point in world coordinates
197 * @return the intrinsic coordinates of the projected point
198 */
199 Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
200 {}, OptionalJacobian<2, 3> Dpoint = {}) const;
201
202 /** Project point at infinity into the image
203 * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
204 * @param point 3D point in world coordinates
205 * @return the intrinsic coordinates of the projected point
206 */
207 Point2 project2(const Unit3& point,
208 OptionalJacobian<2, 6> Dpose = {},
209 OptionalJacobian<2, 2> Dpoint = {}) const;
210
211 /// backproject a 2-dimensional point to a 3-dimensional point at given depth
212 static Point3 BackprojectFromCamera(const Point2& p, const double depth,
213 OptionalJacobian<3, 2> Dpoint = {},
214 OptionalJacobian<3, 1> Ddepth = {});
215
216 /// @}
217 /// @name Advanced interface
218 /// @{
219
220 /**
221 * Return the start and end indices (inclusive) of the translation component of the
222 * exponential map parameterization
223 * @return a pair of [start, end] indices into the tangent space vector
224 */
225 static std::pair<size_t, size_t> TranslationInterval() {
226 return {3, 5};
227 }
228
229 /// @}
230
231private:
232
233#if GTSAM_ENABLE_BOOST_SERIALIZATION
234 /** Serialization function */
235 friend class boost::serialization::access;
236 template<class Archive>
237 void serialize(Archive & ar, const unsigned int /*version*/) {
238 ar & BOOST_SERIALIZATION_NVP(pose_);
239 }
240#endif
241};
242// end of class PinholeBase
243
244/**
245 * A Calibrated camera class [R|-R't], calibration K=I.
246 * If calibration is known, it is more computationally efficient
247 * to calibrate the measurements rather than try to predict in pixels.
248 * @ingroup geometry
249 * \nosubgrouping
250 */
251class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
252
253public:
254
255 inline constexpr static auto dimension = 6;
256
257 /// @name Standard Constructors
258 /// @{
259
260 /// default constructor
261 CalibratedCamera() {
262 }
263
264 /// construct with pose
265 explicit CalibratedCamera(const Pose3& pose) :
266 PinholeBase(pose) {
267 }
268
269 /// @}
270 /// @name Named Constructors
271 /// @{
272
273 // Create CalibratedCamera, with derivatives
274 static CalibratedCamera Create(const Pose3& pose,
275 OptionalJacobian<dimension, 6> H1 = {}) {
276 if (H1)
277 *H1 << I_6x6;
278 return CalibratedCamera(pose);
279 }
280
281 /**
282 * Create a level camera at the given 2D pose and height
283 * @param pose2 specifies the location and viewing direction
284 * @param height specifies the height of the camera (along the positive Z-axis)
285 * (theta 0 = looking in direction of positive X axis)
286 */
287 static CalibratedCamera Level(const Pose2& pose2, double height);
288
289 /**
290 * Create a camera at the given eye position looking at a target point in the scene
291 * with the specified up direction vector.
292 * @param eye specifies the camera position
293 * @param target the point to look at
294 * @param upVector specifies the camera up direction vector,
295 * doesn't need to be on the image plane nor orthogonal to the viewing axis
296 */
297 static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
298 const Point3& upVector);
299
300 /// @}
301 /// @name Advanced Constructors
302 /// @{
303
304 /// construct from vector
305 explicit CalibratedCamera(const Vector &v) :
306 PinholeBase(v) {
307 }
308
309 /// @}
310 /// @name Manifold
311 /// @{
312
313 /// move a cameras pose according to d
314 CalibratedCamera retract(const Vector& d) const;
315
316 /// Return canonical coordinate
317 Vector localCoordinates(const CalibratedCamera& T2) const;
318
319 /// print
320 void print(const std::string& s = "CalibratedCamera") const override {
321 PinholeBase::print(s);
322 }
323
324 inline size_t dim() const {
325 return dimension;
326 }
327
328 inline static size_t Dim() {
329 return dimension;
330 }
331
332 /// @}
333 /// @name Transformations and measurement functions
334 /// @{
335
336 /**
337 * @deprecated
338 * Use project2, which is more consistently named across Pinhole cameras
339 */
340 Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
341 {}, OptionalJacobian<2, 3> Dpoint = {}) const;
342
343 /// backproject a 2-dimensional point to a 3-dimensional point at given depth
344 Point3 backproject(const Point2& pn, double depth,
345 OptionalJacobian<3, 6> Dresult_dpose = {},
346 OptionalJacobian<3, 2> Dresult_dp = {},
347 OptionalJacobian<3, 1> Dresult_ddepth = {}) const {
348
349 Matrix32 Dpoint_dpn;
350 Matrix31 Dpoint_ddepth;
351 const Point3 point = BackprojectFromCamera(p: pn, depth,
352 Dpoint: Dresult_dp ? &Dpoint_dpn : 0,
353 Ddepth: Dresult_ddepth ? &Dpoint_ddepth : 0);
354
355 Matrix33 Dresult_dpoint;
356 const Point3 result = pose().transformFrom(point, Hself: Dresult_dpose,
357 Hpoint: (Dresult_ddepth ||
358 Dresult_dp) ? &Dresult_dpoint : 0);
359
360 if (Dresult_dp)
361 *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
362 if (Dresult_ddepth)
363 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
364
365 return result;
366 }
367
368 /**
369 * Calculate range to a landmark
370 * @param point 3D location of landmark
371 * @return range (double)
372 */
373 double range(const Point3& point,
374 OptionalJacobian<1, 6> Dcamera = {},
375 OptionalJacobian<1, 3> Dpoint = {}) const {
376 return pose().range(point, Hself: Dcamera, Hpoint: Dpoint);
377 }
378
379 /**
380 * Calculate range to another pose
381 * @param pose Other SO(3) pose
382 * @return range (double)
383 */
384 double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {},
385 OptionalJacobian<1, 6> Dpose = {}) const {
386 return this->pose().range(pose, Hself: Dcamera, Hpose: Dpose);
387 }
388
389 /**
390 * Calculate range to another camera
391 * @param camera Other camera
392 * @return range (double)
393 */
394 double range(const CalibratedCamera& camera, //
395 OptionalJacobian<1, 6> H1 = {}, //
396 OptionalJacobian<1, 6> H2 = {}) const {
397 return pose().range(pose: camera.pose(), Hself: H1, Hpose: H2);
398 }
399
400 /// @}
401
402private:
403
404 /// @name Advanced Interface
405 /// @{
406
407#if GTSAM_ENABLE_BOOST_SERIALIZATION
408 /** Serialization function */
409 friend class boost::serialization::access;
410 template<class Archive>
411 void serialize(Archive & ar, const unsigned int /*version*/) {
412 ar
413 & boost::serialization::make_nvp(n: "PinholeBase",
414 v&: boost::serialization::base_object<PinholeBase>(d&: *this));
415 }
416#endif
417
418 /// @}
419};
420
421// manifold traits
422template <>
423struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
424
425template <>
426struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
427
428// range traits, used in RangeFactor
429template <typename T>
430struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
431
432} // namespace gtsam
433