Ocean
Loading...
Searching...
No Matches
AnyCamera.h
Go to the documentation of this file.
1/*
2 * Copyright (c) Meta Platforms, Inc. and affiliates.
3 *
4 * This source code is licensed under the MIT license found in the
5 * LICENSE file in the root directory of this source tree.
6 */
7
8#ifndef META_OCEAN_MATH_ANY_CAMERA_H
9#define META_OCEAN_MATH_ANY_CAMERA_H
10
11#include "ocean/math/Math.h"
12#include "ocean/math/Camera.h"
16#include "ocean/math/Vector2.h"
17#include "ocean/math/Vector3.h"
18
19namespace Ocean
20{
21
22// Forward declaration.
23template <typename T> class AnyCameraT;
24
25/**
26 * Definition of an AnyCamera object with Scalar precision.
27 * @see AnyCameraT
28 * @ingroup math
29 */
31
32/**
33 * Definition of an AnyCamera object with double precision.
34 * @see AnyCameraT
35 * @ingroup math
36 */
38
39/**
40 * Definition of an AnyCamera object with float precision.
41 * @see AnyCameraT
42 * @ingroup math
43 */
45
46/**
47 * Definition of a shared pointer holding an AnyCamera object with Scalar precision.
48 * @tparam T the data type of the scalar to be used either 'float' or 'double'
49 * @see AnyCameraT
50 * @ingroup math
51 */
52template <typename T>
53using SharedAnyCameraT = std::shared_ptr<AnyCameraT<T>>;
54
55/**
56 * Definition of a shared pointer holding an AnyCamera object with Scalar precision.
57 * @see AnyCameraT
58 * @ingroup math
59 */
60using SharedAnyCamera = std::shared_ptr<AnyCamera>;
61
62/**
63 * Definition of a shared pointer holding an AnyCamera object with double precision.
64 * @see AnyCameraT
65 * @ingroup math
66 */
67using SharedAnyCameraD = std::shared_ptr<AnyCameraD>;
68
69/**
70 * Definition of a shared pointer holding an AnyCamera object with float precision.
71 * @see AnyCameraT
72 * @ingroup math
73 */
74using SharedAnyCameraF = std::shared_ptr<AnyCameraF>;
75
76/**
77 * Definition of a typename alias for vectors with shared AnyCameraT objects.
78 * @tparam T the data type of the scalar to be used either 'float' or 'double'
79 * @see VectorT2
80 * @ingroup math
81 */
82template <typename T>
83using SharedAnyCamerasT = std::vector<std::shared_ptr<AnyCameraT<T>>>;
84
85/**
86 * Definition of a vector holding AnyCamera objects.
87 * @see AnyCamera
88 * @ingroup math
89 */
91
92/**
93 * Definition of a vector holding AnyCameraD objects.
94 * @see AnyCameraD
95 * @ingroup math
96 */
98
99/**
100 * Definition of a vector holding AnyCameraF objects.
101 * @see AnyCameraF
102 * @ingroup math
103 */
105
106/**
107 * Definition of individual camera types.
108 * @ingroup math
109 */
110enum class AnyCameraType : uint32_t
111{
112 /// An invalid camera type.
113 INVALID = 0u,
114 /// A pinhole camera.
115 PINHOLE,
116 /// A fisheye camera.
117 FISHEYE
118};
119
120/**
121 * This class implements the abstract base class for all AnyCamera objects.
122 * A custom camera object can be implemented by
123 * - simply deriving a new class from this base class
124 * - using the helper class AnyCameraWrappingT which helps reducing the implementation effort
125 * @tparam T The data type of a scalar, 'float' or 'double'
126 * @ingroup math
127 */
128template <typename T>
129class AnyCameraT : public CameraT<T>
130{
131 public:
132
133 /// The scalar data type of this object.
134 using TScalar = T;
135
136 public:
137
138 /**
139 * Destructs the AnyCamera object.
140 */
141 virtual ~AnyCameraT() = default;
142
143 /**
144 * Returns the type of this camera.
145 * @return The camera's type
146 */
147 virtual AnyCameraType anyCameraType() const = 0;
148
149 /**
150 * Returns the name of this camera.
151 * @return The camera's name
152 */
153 virtual std::string name() const = 0;
154
155 /**
156 * Returns a copy of this camera object.
157 * The image resolution of the cloned camera must have the same aspect ratio as the current image resolution.
158 * @param width The width of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
159 * @param height the height of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
160 * @return New instance of this camera object
161 */
162 virtual std::unique_ptr<AnyCameraT<T>> clone(const unsigned int width = 0u, const unsigned int height = 0u) const = 0;
163
164 /**
165 * Returns a copy of this camera object with float precision.
166 * The image resolution of the cloned camera must have the same aspect ratio as the current image resolution.
167 * @param width The width of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
168 * @param height the height of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
169 * @return New instance of this camera object
170 */
171 virtual std::unique_ptr<AnyCameraT<float>> cloneToFloat(const unsigned int width = 0u, const unsigned int height = 0u) const = 0;
172
173 /**
174 * Returns a copy of this camera object with double precision.
175 * The image resolution of the cloned camera must have the same aspect ratio as the current image resolution.
176 * @param width The width of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
177 * @param height the height of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
178 * @return New instance of this camera object
179 */
180 virtual std::unique_ptr<AnyCameraT<double>> cloneToDouble(const unsigned int width = 0u, const unsigned int height = 0u) const = 0;
181
182 /**
183 * Returns the width of the camera image.
184 * @return Width of the camera image, in pixel, with range [0, infinity)
185 */
186 virtual unsigned int width() const = 0;
187
188 /**
189 * Returns the height of the camera image.
190 * @return Height of the camera image, in pixel, with range [0, infinity)
191 */
192 virtual unsigned int height() const = 0;
193
194 /**
195 * Returns the coordinate of the principal point of the camera image in the pixel domain.
196 * @return The 2D location of the principal point, with range (-infinity, infinity)x(-infinity, infinity)
197 */
198 virtual VectorT2<T> principalPoint() const = 0;
199
200 /**
201 * Returns the x-value of the principal point of the camera image in the pixel domain.
202 * @return x-value of the principal point, with range (-infinity, infinity)
203 */
204 virtual T principalPointX() const = 0;
205
206 /**
207 * Returns the y-value of the principal point of the camera image in the pixel domain.
208 * @return y-value of the principal point, with range (-infinity, infinity)
209 */
210 virtual T principalPointY() const = 0;
211
212 /**
213 * Returns the horizontal focal length parameter.
214 * @return Horizontal focal length parameter in pixel domain, with range (0, infinity)
215 */
216 virtual T focalLengthX() const = 0;
217
218 /**
219 * Returns the vertical focal length parameter.
220 * @return Vertical focal length parameter in pixel domain, with range (0, infinity)
221 */
222 virtual T focalLengthY() const = 0;
223
224 /**
225 * Returns the inverse horizontal focal length parameter.
226 * @return Inverse horizontal focal length parameter in pixel domain, with range (0, infinity)
227 */
228 virtual T inverseFocalLengthX() const = 0;
229
230 /**
231 * Returns the inverse vertical focal length parameter.
232 * @return Inverse vertical focal length parameter in pixel domain, with range (0, infinity)
233 */
234 virtual T inverseFocalLengthY() const = 0;
235
236 /**
237 * Returns the field of view in x direction of the camera.
238 * The fov is the sum of the left and right part of the camera.
239 * @return Field of view (in radian), with range (0, 2 * PI]
240 */
241 virtual T fovX() const = 0;
242
243 /**
244 * Returns the field of view in x direction of the camera.
245 * The fov is the sum of the top and bottom part of the camera.
246 * @return Field of view (in radian), with range (0, 2 * PI]
247 */
248 virtual T fovY() const = 0;
249
250 /**
251 * Returns whether a given 2D image point lies inside the camera frame.
252 * Optional an explicit border can be defined to allow points slightly outside the camera image, or further inside the image.<br>
253 * Defined a negative border size to allow image points outside the camera frame, or a positive border size to prevent points within the camera frame but close to the boundary.
254 * @param imagePoint Image point to be checked, must be valid
255 * @param signedBorder The optional border increasing or decreasing the rectangle in which the image point must be located, in pixels, with range (-infinity, std::min(width() / 2, height() / 2)
256 * @return True, if the image point lies in the ranges [0, width())x[0, height())
257 */
258 virtual bool isInside(const VectorT2<T>& imagePoint, const T signedBorder = T(0)) const = 0;
259
260 /**
261 * Projects a 3D object point into the camera frame.
262 * The projection is applied with a default camera pose, the camera is looking into the negative z-space with y-axis up.
263 * @param objectPoint The 3D object point to project, defined in world
264 * @return The projected 2D image point
265 */
266 virtual VectorT2<T> projectToImage(const VectorT3<T>& objectPoint) const = 0;
267
268 /**
269 * Projects a 3D object point into the camera frame.
270 * @param world_T_camera The camera pose, the default camera is looking into the negative z-space with y-axis up, transforming camera to world, must be valid
271 * @param objectPoint The 3D object point to project, defined in world
272 * @return The projected 2D image point
273 */
274 virtual VectorT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>& objectPoint) const = 0;
275
276 /**
277 * Projects several 3D object points into the camera frame at once.
278 * The projection is applied with a default camera pose, the camera is looking into the negative z-space with y-axis up.
279 * @param objectPoints The 3D object points to project, defined in world, must be valid
280 * @param size The number of object points, with range [1, infinity)
281 * @param imagePoints The resulting 2D image points, must be valid
282 */
283 virtual void projectToImage(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const = 0;
284
285 /**
286 * Projects several 3D object points into the camera frame at once.
287 * @param world_T_camera The camera pose, the default camera is looking into the negative z-space with y-axis up, transforming camera to world, must be valid
288 * @param objectPoints The 3D object points to project, defined in world, must be valid
289 * @param size The number of object points, with range [1, infinity)
290 * @param imagePoints The resulting 2D image points, must be valid
291 */
292 virtual void projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const = 0;
293
294 /**
295 * Projects a 3D object point into the camera frame.
296 * The projection is applied with a default (inverted) and flipped camera pose, the camera is looking into the positive z-space with y-axis down.
297 * @param objectPoint The 3D object point to project, defined in world
298 * @return The projected 2D image point
299 */
300 virtual VectorT2<T> projectToImageIF(const VectorT3<T>& objectPoint) const = 0;
301
302 /**
303 * Projects a 3D object point into the camera frame.
304 * @param flippedCamera_T_world The inverted and flipped camera pose, the default camera is looking into the positive z-space with y-axis down, transforming world to flipped camera, must be valid
305 * @param objectPoint The 3D object point to project, defined in world
306 * @return The projected 2D image point
307 */
309
310 /**
311 * Projects several 3D object points into the camera frame at once.
312 * The projection is applied with a default (inverted) and flipped camera pose, the camera is looking into the positive z-space with y-axis down.
313 * @param objectPoints The 3D object points to project, defined in world, must be valid
314 * @param size The number of object points, with range [1, infinity)
315 * @param imagePoints The resulting 2D image points, must be valid
316 */
317 virtual void projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const = 0;
318
319 /**
320 * Projects several 3D object points into the camera frame at once.
321 * @param flippedCamera_T_world The inverted and flipped camera pose, the default camera is looking into the positive z-space with y-axis down, transforming world to flipped camera, must be valid
322 * @param objectPoints The 3D object points to project, defined in world, must be valid
323 * @param size The number of object points, with range [1, infinity)
324 * @param imagePoints The resulting 2D image points, must be valid
325 */
326 virtual void projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const = 0;
327
328 /**
329 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
330 * The vector is determined for a default camera looking into the negative z-space with y-axis up.
331 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
332 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
333 * @return Vector pointing into the negative z-space
334 * @see vectorIF(), ray().
335 */
336 virtual VectorT3<T> vector(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector = true) const = 0;
337
338 /**
339 * Determines vectors starting at the camera's center and intersecting given 2D points in the image.
340 * The vectors are determined for a default camera looking into the negative z-space with y-axis up.
341 * @param distortedImagePoints 2D (distorted) positions within the image, with range [0, width())x[0, height()), must be valid
342 * @param size The number of provided points, with range [1, infinity)
343 * @param vectors The resulting vectors pointing into the negative z-space, one for each image point, must be valid
344 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
345 * @see vectorIF(), ray().
346 */
347 virtual void vector(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector = true) const = 0;
348
349 /**
350 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
351 * The vector is determined for a default camera looking into the positive z-space with y-axis down.
352 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
353 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
354 * @return Vector pointing into the positive z-space
355 */
356 virtual VectorT3<T> vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector = true) const = 0;
357
358 /**
359 * Returns vectors starting at the camera's center and intersecting a given 2D points in the image.
360 * The vectors are determined for a default camera looking into the positive z-space with y-axis down.
361 * @param distortedImagePoints 2D (distorted) positions within the image, with range [0, width())x[0, height()), must be valid
362 * @param size The number of provided points, with range [1, infinity)
363 * @param vectors The resulting vectors pointing into the positive z-space, one for each image point, must be valid
364 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
365 */
366 virtual void vectorIF(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector = true) const = 0;
367
368 /**
369 * Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
370 * The ray is determined for a default camera looking into the negative z-space with y-axis up.
371 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
372 * @param world_T_camera The pose of the camera, the extrinsic camera matrix, must be valid
373 * @return The specified ray with direction pointing into the camera's negative z-space
374 * @see vector().
375 */
377
378 /**
379 * Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
380 * The ray is determined for a default camera looking into the negative z-space with y-axis up.
381 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
382 * @return The specified ray with direction pointing into the camera's negative z-space
383 * @see vector().
384 */
385 virtual LineT3<T> ray(const VectorT2<T>& distortedImagePoint) const = 0;
386
387 /**
388 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
389 * The resulting jacobian matrix has the following layout:
390 * <pre>
391 * | dfu / dx, dfu / dy, dfu / dz |
392 * | dfv / dx, dfv / dy, dfv / dz |
393 * with projection function
394 * q = f(p)
395 * q_u = fu(p), q_y = fv(p)
396 * with 2D image point q = (q_u, q_v) and 3D object point p = (x, y, z)
397 * </pre>
398 * @param flippedCameraObjectPoint The 3D object point defined in relation to the inverted and flipped camera pose (camera looking into the positive z-space with y-axis pointing down).
399 * @param jx The resulting first row of the Jacobian matrix, must contain three elements, must be valid
400 * @param jy The resulting second row of the Jacobian matrix, must contain three elements, must be valid
401 * @see pointJacobian2nx3IF().
402 */
403 virtual void pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const = 0;
404
405 /**
406 * Calculates the 2n x 3 jacobian matrix for the 3D object point projection into the camera frame.
407 * The resulting jacobian matrix has the following layout:
408 * <pre>
409 * | dfu / dx, dfu / dy, dfu / dz | <- for object point 0
410 * | dfv / dx, dfv / dy, dfv / dz |
411 * | ... |
412 * | dfu / dx, dfu / dy, dfu / dz | <- for object point n - 1
413 * | dfv / dx, dfv / dy, dfv / dz |
414 * with projection function
415 * q = f(p)
416 * q_u = fu(p), q_y = fv(p)
417 * with 2D image point q = (q_u, q_v) and 3D object point p = (x, y, z)
418 * </pre>
419 * @param flippedCameraObjectPoints The 3D object points defined in relation to the inverted and flipped camera pose (camera looking into the positive z-space with y-axis pointing down).
420 * @param numberObjectPoints The number of given 3D object points, with range [1, infinity)
421 * @param jacobians The resulting 2n x 3 Jacobian matrix, with 2 * numberObjectPoints * 3 elements, must be valid
422 * @see pointJacobian2x3IF().
423 */
424 virtual void pointJacobian2nx3IF(const VectorT3<T>* flippedCameraObjectPoints, const size_t numberObjectPoints, T* jacobians) const = 0;
425
426 /**
427 * Returns whether two camera objects are identical up to a given epsilon.
428 * The image resolution must always be identical.
429 * @param anyCamera The second camera to be used for comparison, can be invalid
430 * @param eps The epsilon threshold to be used, with range [0, infinity)
431 * @return True, if so
432 */
433 virtual bool isEqual(const AnyCameraT<T>& anyCamera, const T eps = NumericT<T>::eps()) const = 0;
434
435 /**
436 * Returns whether this camera is valid.
437 * @return True, if so
438 */
439 virtual bool isValid() const = 0;
440
441 /**
442 * Converts an AnyCamera object with arbitrary scalar type to another AnyCamera object with arbitrary scalar type.
443 * In case both scalar types are identical, the object is simply returned.
444 * In case both scalar types are different, a clone is returned.
445 * @param anyCamera The AnyCamera object to be converted, can be nullptr
446 * @return The resulting AnyCamera object
447 * @tparam U The scalar data type of the given AnyCamera object
448 */
449 template <typename U>
450 static std::shared_ptr<AnyCameraT<T>> convert(const std::shared_ptr<AnyCameraT<U>>& anyCamera);
451
452 protected:
453
454 /**
455 * Protected default constructor.
456 */
457 AnyCameraT() = default;
458
459 /**
460 * Protected copy constructor.
461 * @param anyCamera The object to copy
462 */
464
465 /**
466 * Disabled move constructor.
467 */
469
470 /**
471 * Disabled assign operator.
472 * @return Reference to this object
473 */
474 AnyCameraT& operator=(const AnyCameraT&) = delete;
475
476 /**
477 * Disabled move operator.
478 * @return Reference to this object
479 */
481};
482
483// Forward declaration.
484template <typename T> class CameraProjectionCheckerT;
485
486/**
487 * Definition of an ProjectionChecker object with Scalar precision.
488 * @see CameraProjectionCheckerT
489 * @ingroup math
490 */
492
493/**
494 * Definition of an ProjectionChecker object with double precision.
495 * @see CameraProjectionCheckerT
496 * @ingroup math
497 */
499
500/**
501 * Definition of an ProjectionChecker object with float precision.
502 * @see CameraProjectionCheckerT
503 * @ingroup math
504 */
506
507/**
508 * This class implements a helper class allowing to check whether a 3D object point projects into the camera image.
509 * The checker uses normalized coordinates when verifying the projection behavior to avoid numerical issues when object points project far outside the image area.<br>
510 * In contrast to AnyCamera::ptojectToImageIF() + AnyCamera::isInside(), the checker is more precise but also more expensive.
511 * @tparam T The data type of a scalar, 'float' or 'double'
512 * @ingroup math
513 */
514template <typename T>
516{
517 public:
518
519 /**
520 * Default constructor creating an invalid object.
521 */
523
524 /**
525 * Creates a new checker object for a specified camera model.
526 * @param camera The camera model defining the projection, must be valid
527 * @param segmentSteps The number of segments to be used to determine the camera boundary, with range [2, infinity)
528 */
529 explicit CameraProjectionCheckerT(const SharedAnyCameraT<T>& camera, const size_t segmentSteps = 10);
530
531 /**
532 * Returns whether a 3D object point is located in front of the camera and projects into the camera image.
533 * @param flippedCamera_T_world The inverted and flipped camera pose, the default flipped camera is looking into the positive z-space with y-axis down, transforming world to flipped camera, must be valid
534 * @param objectPoint The 3D object point to be checked, defined in world
535 * @param imagePoint Optional resulting 2D projected image point inside the camera image, nullptr if not of interest
536 * @return True, if the object point projects into the camera image; False, if the object point is behind the camera or projects outside the camera image
537 */
538 bool projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint, VectorT2<T>* imagePoint = nullptr) const;
539
540 /**
541 * Returns the camera model of this checker.
542 * @return The checker's camera model, nullptr if no camera model has been set
543 */
544 const SharedAnyCameraT<T>& camera() const;
545
546 /**
547 * Returns the 2D line segments defined in the camera's normalized image plane defining the camera's boundary.
548 * @return The camera boundary segments
549 */
551
552 /**
553 * Returns whether this checker holds a valid camera model and is ready to be used.
554 * @return True, if so
555 */
556 bool isValid() const;
557
558 protected:
559
560 /**
561 * Determines the camera boundary of a given camera model in normalized image coordinates.
562 * @param camera The camera model for which the boundary will be determined, must be valid
563 * @param cameraBoundarySegments The resulting 2D line segments defining the camera's boundary
564 * @param segmentSteps The number of segments to be used to determine the camera boundary, with range [1, infinity)
565 * @return True, if succeeded
566 */
567 static bool determineCameraBoundary(const AnyCameraT<T>& camera, FiniteLinesT2<T>& cameraBoundarySegments, const size_t segmentSteps);
568
569 /**
570 * Returns whether a given normalized image point lies inside the camera's boundary.
571 * @param cameraBoundarySegments The 2D line segments defining the camera's boundary, at least three
572 * @param imagePoint The normalized image point to be checked
573 * @return True, if if so
574 */
575 static bool isInside(const FiniteLinesT2<T>& cameraBoundarySegments, const VectorT2<T>& imagePoint);
576
577 protected:
578
579 /// The actual camera model this checker is based on.
581
582 /// The 2D line segments defined in the camera's normalized image plane defining the camera's boundary.
584};
585
586/**
587 * This class implements a specialized AnyCamera object wrapping the actual camera model.
588 * The class is a helper class to simplify the implementation of specialized AnyCamera objects.
589 * @tparam T The data type of a scalar, 'float' or 'double'
590 * @tparam TCameraWrapper The data type of the class actually wrapping the camera object
591 * @ingroup math
592 */
593template <typename T, typename TCameraWrapper>
595 public AnyCameraT<T>,
596 public TCameraWrapper
597{
598 public:
599
600 /// The scalar data type of this object.
601 using TScalar = T;
602
603 /// The class which is actually wrapping the camera object.
605
606 /// The actual camera object wrapped by this class.
607 using ActualCamera = typename TCameraWrapper::ActualCamera;
608
609 public:
610
611 /**
612 * Creates a new AnyCamera object wrapping the actual camera model.
613 * @param actualCamera The actual camera object to be wrapped
614 */
615 explicit AnyCameraWrappingT(ActualCamera&& actualCamera);
616
617 /**
618 * Creates a new AnyCamera object wrapping the actual camera model.
619 * @param actualCamera The actual camera object to be wrapped
620 */
621 explicit AnyCameraWrappingT(const ActualCamera& actualCamera);
622
623 /**
624 * Returns the type of this camera.
625 * @return The camera's type
626 */
627 AnyCameraType anyCameraType() const override;
628
629 /**
630 * Returns the name of this camera.
631 * @return The camera's name
632 */
633 std::string name() const override;
634
635 /**
636 * Returns a copy of this camera object.
637 * The image resolution of the cloned camera must have the same aspect ratio as the current image resolution.
638 * @param width The width of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
639 * @param height the height of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
640 * @return New instance of this camera object
641 */
642 std::unique_ptr<AnyCameraT<T>> clone(const unsigned int width = 0u, const unsigned int height = 0u) const override;
643
644 /**
645 * Returns a copy of this camera object with float precision.
646 * The image resolution of the cloned camera must have the same aspect ratio as the current image resolution.
647 * @param width The width of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
648 * @param height the height of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
649 * @return New instance of this camera object
650 */
651 std::unique_ptr<AnyCameraT<float>> cloneToFloat(const unsigned int width = 0u, const unsigned int height = 0u) const override;
652
653 /**
654 * Returns a copy of this camera object with double precision.
655 * The image resolution of the cloned camera must have the same aspect ratio as the current image resolution.
656 * @param width The width of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
657 * @param height the height of the cloned camera in pixel, with range [1, infinity), 0 to use the current image resolution
658 * @return New instance of this camera object
659 */
660 std::unique_ptr<AnyCameraT<double>> cloneToDouble(const unsigned int width = 0u, const unsigned int height = 0u) const override;
661
662 /**
663 * Returns the width of the camera image.
664 * @return Width of the camera image, in pixel, with range [0, infinity)
665 */
666 unsigned int width() const override;
667
668 /**
669 * Returns the height of the camera image.
670 * @return Height of the camera image, in pixel, with range [0, infinity)
671 */
672 unsigned int height() const override;
673
674 /**
675 * Returns the coordinate of the principal point of the camera image in the pixel domain.
676 * @return The 2D location of the principal point, with range (-infinity, infinity)x(-infinity, infinity)
677 */
678 VectorT2<T> principalPoint() const override;
679
680 /**
681 * Returns the x-value of the principal point of the camera image in the pixel domain.
682 * @return x-value of the principal point, with range (-infinity, infinity)
683 */
684 T principalPointX() const override;
685
686 /**
687 * Returns the y-value of the principal point of the camera image in the pixel domain.
688 * @return y-value of the principal point, with range (-infinity, infinity)
689 */
690 T principalPointY() const override;
691
692 /**
693 * Returns the horizontal focal length parameter.
694 * @return Horizontal focal length parameter in pixel domain, with range (0, infinity)
695 */
696 T focalLengthX() const override;
697
698 /**
699 * Returns the vertical focal length parameter.
700 * @return Vertical focal length parameter in pixel domain, with range (0, infinity)
701 */
702 T focalLengthY() const override;
703
704 /**
705 * Returns the inverse horizontal focal length parameter.
706 * @return Inverse horizontal focal length parameter in pixel domain, with range (0, infinity)
707 */
708 T inverseFocalLengthX() const override;
709
710 /**
711 * Returns the inverse vertical focal length parameter.
712 * @return Inverse vertical focal length parameter in pixel domain, with range (0, infinity)
713 */
714 T inverseFocalLengthY() const override;
715
716 /**
717 * Returns the field of view in x direction of the camera.
718 * The fov is the sum of the left and right part of the camera.
719 * @return Field of view (in radian), with range (0, 2 * PI]
720 */
721 T fovX() const override;
722
723 /**
724 * Returns the field of view in x direction of the camera.
725 * The fov is the sum of the top and bottom part of the camera.
726 * @return Field of view (in radian), with range (0, 2 * PI]
727 */
728 T fovY() const override;
729
730 /**
731 * Returns whether a given 2D image point lies inside the camera frame.
732 * Optional an explicit border can be defined to allow points slightly outside the camera image, or further inside the image.<br>
733 * Defined a negative border size to allow image points outside the camera frame, or a positive border size to prevent points within the camera frame but close to the boundary.
734 * @param imagePoint Image point to be checked, must be valid
735 * @param signedBorder The optional border increasing or decreasing the rectangle in which the image point must be located, in pixels, with range (-infinity, std::min(width() / 2, height() / 2)
736 * @return True, if the image point lies in the ranges [0, width())x[0, height())
737 */
738 bool isInside(const VectorT2<T>& imagePoint, const T signedBorder = T(0)) const override;
739
740 /**
741 * Projects a 3D object point into the camera frame.
742 * The projection is applied with a default camera pose, the camera is looking into the negative z-space with y-axis up.
743 * @param objectPoint The 3D object point to project, defined in world
744 * @return The projected 2D image point
745 */
746 VectorT2<T> projectToImage(const VectorT3<T>& objectPoint) const override;
747
748 /**
749 * Projects a 3D object point into the camera frame.
750 * @param world_T_camera The camera pose, the default camera is looking into the negative z-space with y-axis up, transforming camera to world, must be valid
751 * @param objectPoint The 3D object point to project, defined in world
752 * @return The projected 2D image point
753 */
754 VectorT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>& objectPoint) const override;
755
756 /**
757 * Projects several 3D object points into the camera frame at once.
758 * The projection is applied with a default camera pose, the camera is looking into the negative z-space with y-axis up.
759 * @param objectPoints The 3D object points to project, defined in world, must be valid
760 * @param size The number of object points, with range [1, infinity)
761 * @param imagePoints The resulting 2D image points, must be valid
762 */
763 void projectToImage(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const override;
764
765 /**
766 * Projects several 3D object points into the camera frame at once.
767 * @param world_T_camera The camera pose, the default camera is looking into the negative z-space with y-axis up, transforming camera to world, must be valid
768 * @param objectPoints The 3D object points to project, defined in world, must be valid
769 * @param size The number of object points, with range [1, infinity)
770 * @param imagePoints The resulting 2D image points, must be valid
771 */
772 void projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const override;
773
774 /**
775 * Projects a 3D object point into the camera frame.
776 * The projection is applied with a default (inverted) and flipped camera pose, the camera is looking into the positive z-space with y-axis down.
777 * @param objectPoint The 3D object point to project, defined in world
778 * @return The projected 2D image point
779 */
780 VectorT2<T> projectToImageIF(const VectorT3<T>& objectPoint) const override;
781
782 /**
783 * Projects a 3D object point into the camera frame.
784 * @param flippedCamera_T_world The inverted and flipped camera pose, the default camera is looking into the positive z-space with y-axis down, transforming world to flipped camera, must be valid
785 * @param objectPoint The 3D object point to project, defined in world
786 * @return The projected 2D image point
787 */
789
790 /**
791 * Projects several 3D object points into the camera frame at once.
792 * The projection is applied with a default (inverted) and flipped camera pose, the camera is looking into the positive z-space with y-axis down.
793 * @param objectPoints The 3D object points to project, defined in world, must be valid
794 * @param size The number of object points, with range [1, infinity)
795 * @param imagePoints The resulting 2D image points, must be valid
796 */
797 void projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const override;
798
799 /**
800 * Projects several 3D object points into the camera frame at once.
801 * @param flippedCamera_T_world The inverted and flipped camera pose, the default camera is looking into the positive z-space with y-axis down, transforming world to flipped camera, must be valid
802 * @param objectPoints The 3D object points to project, defined in world, must be valid
803 * @param size The number of object points, with range [1, infinity)
804 * @param imagePoints The resulting 2D image points, must be valid
805 */
806 void projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const override;
807
808 /**
809 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
810 * The vector is determined for the default camera looking into the negative z-space with y-axis up.
811 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
812 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
813 * @return Vector pointing into the negative z-space
814 * @see vectorIF(), ray().
815 */
816 VectorT3<T> vector(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector = true) const override;
817
818 /**
819 * Determines vectors starting at the camera's center and intersecting given 2D points in the image.
820 * The vectors are determined for a default camera looking into the negative z-space with y-axis up.
821 * @param distortedImagePoints 2D (distorted) positions within the image, with range [0, width())x[0, height()), must be valid
822 * @param size The number of provided points, with range [1, infinity)
823 * @param vectors The resulting vectors pointing into the negative z-space, one for each image point, must be valid
824 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
825 * @see vectorIF(), ray().
826 */
827 void vector(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector = true) const override;
828
829 /**
830 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
831 * The vector is determined for the default camera looking into the positive z-space with y-axis down.
832 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
833 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
834 * @return Vector pointing into the positive z-space
835 */
836 VectorT3<T> vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector = true) const override;
837
838 /**
839 * Returns vectors starting at the camera's center and intersecting a given 2D points in the image.
840 * The vectors are determined for a default camera looking into the positive z-space with y-axis down.
841 * @param distortedImagePoints 2D (distorted) positions within the image, with range [0, width())x[0, height()), must be valid
842 * @param size The number of provided points, with range [1, infinity)
843 * @param vectors The resulting vectors pointing into the positive z-space, one for each image point, must be valid
844 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
845 */
846 void vectorIF(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector = true) const override;
847
848 /**
849 * Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
850 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
851 * @param world_T_camera The pose of the camera, the extrinsic camera matrix, must be valid
852 * @return The specified ray with direction pointing into the camera's negative z-space
853 * @see vector().
854 */
856
857 /**
858 * Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
859 * @param distortedImagePoint 2D (distorted) position within the image, with range [0, width())x[0, height())
860 * @return The specified ray with direction pointing into the camera's negative z-space
861 * @see vector().
862 */
863 LineT3<T> ray(const VectorT2<T>& distortedImagePoint) const override;
864
865 /**
866 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
867 * The resulting jacobian matrix has the following layout:
868 * <pre>
869 * | dfu / dx, dfu / dy, dfu / dz |
870 * | dfv / dx, dfv / dy, dfv / dz |
871 * with projection function
872 * q = f(p)
873 * q_u = fu(p), q_y = fv(p)
874 * with 2D image point q = (q_u, q_v) and 3D object point p = (x, y, z)
875 * </pre>
876 * @param flippedCameraObjectPoint The 3D object point defined in relation to the inverted and flipped camera pose (camera looking into the positive z-space with y-axis pointing down).
877 * @param jx The resulting first row of the Jacobian matrix, must contain three elements, must be valid
878 * @param jy The resulting second row of the Jacobian matrix, must contain three elements, must be valid
879 */
880 void pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const override;
881
882 /**
883 * Calculates the 2n x 3 jacobian matrix for the 3D object point projection into the camera frame.
884 * The resulting jacobian matrix has the following layout:
885 * <pre>
886 * | dfu / dx, dfu / dy, dfu / dz | <- for object point 0
887 * | dfv / dx, dfv / dy, dfv / dz |
888 * | ... |
889 * | dfu / dx, dfu / dy, dfu / dz | <- for object point n - 1
890 * | dfv / dx, dfv / dy, dfv / dz |
891 * with projection function
892 * q = f(p)
893 * q_u = fu(p), q_y = fv(p)
894 * with 2D image point q = (q_u, q_v) and 3D object point p = (x, y, z)
895 * </pre>
896 * @param flippedCameraObjectPoints The 3D object points defined in relation to the inverted and flipped camera pose (camera looking into the positive z-space with y-axis pointing down).
897 * @param numberObjectPoints The number of given 3D object points, with range [1, infinity)
898 * @param jacobians The resulting 2n x 3 Jacobian matrix, with 2 * numberObjectPoints * 3 elements, must be valid
899 */
900 void pointJacobian2nx3IF(const VectorT3<T>* flippedCameraObjectPoints, const size_t numberObjectPoints, T* jacobians) const override;
901
902 /**
903 * Returns whether two camera objects are identical up to a given epsilon.
904 * The image resolution must always be identical.
905 * @param anyCamera The second camera to be used for comparison, can be invalid
906 * @param eps The epsilon threshold to be used, with range [0, infinity)
907 * @return True, if so
908 */
909 bool isEqual(const AnyCameraT<T>& anyCamera, const T eps = NumericT<T>::eps()) const override;
910
911 /**
912 * Returns whether this camera is valid.
913 * @return True, if so
914 */
915 bool isValid() const override;
916};
917
918/**
919 * This class implements a wrapper for an actual camera object.
920 * - TCameraWrapperBase implements the wrapper functions necessary for the individual camera models.
921 * - CameraWrapperT implements some additional functions necessary to fully implement all necessary functions for AnyCameraT.
922 * @tparam T The data type of a scalar, 'float' or 'double'
923 * @tparam TCameraWrapperBase The base class implementing all functions necessary for the wrapped camera object.
924 * @ingroup math
925 */
926template <typename T, typename TCameraWrapperBase>
928{
929 public:
930
931 /**
932 * Definition of the actual camera object which is wrapped in this class.
933 */
934 using typename TCameraWrapperBase::ActualCamera;
935
936 public:
937
938 /**
939 * Creates a new CameraWrapperT object wrapping the actual camera model.
940 * @param actualCamera The actual camera object to be wrapped
941 */
942 explicit CameraWrapperT(ActualCamera&& actualCamera);
943
944 /**
945 * Creates a new CameraWrapperT object wrapping the actual camera model.
946 * @param actualCamera The actual camera object to be wrapped
947 */
948 explicit CameraWrapperT(const ActualCamera& actualCamera);
949
950 /**
951 * Returns the coordinate of the principal point of the camera image in the pixel domain.
952 * @see AnyCameraT::principalPoint().
953 */
954 inline VectorT2<T> principalPoint() const;
955
956 /**
957 * Returns the field of view in x direction of the camera.
958 * @see AnyCameraT::fovX().
959 */
960 inline T fovX() const;
961
962 /**
963 * Returns the field of view in x direction of the camera.
964 * @see AnyCameraT::fovY().
965 */
966 inline T fovY() const;
967
968 /**
969 * Returns whether a given 2D image point lies inside the camera frame.
970 * @see AnyCameraT::isInside().
971 */
972 inline bool isInside(const VectorT2<T>& imagePoint, const T signedBorder = T(0)) const;
973
974 /**
975 * Projects a 3D object point into the camera frame.
976 * @see projectToImage().
977 */
978 inline VectorT2<T> projectToImage(const VectorT3<T>& objectPoint) const;
979
980 /**
981 * Projects a 3D object point into the camera frame.
982 * @see projectToImage().
983 */
984 inline VectorT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>& objectPoint) const;
985
986 /**
987 * Projects several 3D object points into the camera frame at once.
988 * @see projectToImage().
989 */
990 inline void projectToImage(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
991
992 /**
993 * Projects several 3D object points into the camera frame at once.
994 * @see projectToImage().
995 */
996 inline void projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
997
998 /**
999 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
1000 * @see AnyCameraT::vector().
1001 */
1002 inline VectorT3<T> vector(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const;
1003
1004 /**
1005 * Determines vectors starting at the camera's center and intersecting given 2D points in the image.
1006 * @see AnyCameraT::vector().
1007 */
1008 inline void vector(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const;
1009
1010 /**
1011 * Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
1012 * @see ray().
1013 */
1015
1016 /**
1017 * Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
1018 * @see ray().
1019 */
1020 inline LineT3<T> ray(const VectorT2<T>& distortedImagePoint) const;
1021
1022 /**
1023 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
1024 * @see AnyCameraT::pointJacobian2nx3IF().
1025 */
1026 inline void pointJacobian2nx3IF(const VectorT3<T>* flippedCameraObjectPoints, const size_t numberObjectPoints, T* jacobians) const;
1027};
1028
1029/**
1030 * This class implements the base wrapper around Ocean's pinhole camera profile.
1031 * The class can be used as 'TCameraWrapperBase' in 'CameraWrapperT' to create a full wrapper class e.g., 'CameraWrapperT<T, CameraWrapperBasePinholeT<T>>'.
1032 * @tparam T The data type of a scalar, 'float' or 'double'
1033 * @ingroup math
1034 */
1035template <typename T>
1037{
1038 public:
1039
1040 /**
1041 * Definition of the actual camera object wrapped by this class.
1042 */
1044
1045 /**
1046 * Definition of the parent WrappedCamera class using this base class.
1047 */
1049
1050 public:
1051
1052 /**
1053 * Creates a new CameraWrapperBasePinholeT object wrapping the actual camera model.
1054 * @param actualCamera The actual camera object to be wrapped
1055 */
1057
1058 /**
1059 * Creates a new CameraWrapperBasePinholeT object wrapping the actual camera model.
1060 * @param actualCamera The actual camera object to be wrapped
1061 */
1063
1064 /**
1065 * Returns the actual camera object wrapped in this class.
1066 * @return The wrapped camera object
1067 */
1068 inline const ActualCamera& actualCamera() const;
1069
1070 /**
1071 * Returns the width of the camera image.
1072 * @see AnyCameraT::width().
1073 */
1074 inline unsigned int width() const;
1075
1076 /**
1077 * Returns the height of the camera image.
1078 * @see AnyCameraT::height().
1079 */
1080 inline unsigned int height() const;
1081
1082 /**
1083 * Returns the x-value of the principal point of the camera image in the pixel domain.
1084 * @see AnyCameraT::principalPointX().
1085 */
1086 inline T principalPointX() const;
1087
1088 /**
1089 * Returns the y-value of the principal point of the camera image in the pixel domain.
1090 * @see AnyCameraT::principalPointY().
1091 */
1092 inline T principalPointY() const;
1093
1094 /**
1095 * Returns the horizontal focal length parameter.
1096 * @see AnyCameraT::focalLengthX().
1097 */
1098 inline T focalLengthX() const;
1099
1100 /**
1101 * Returns the vertical focal length parameter.
1102 * @see AnyCameraT::focalLengthY().
1103 */
1104 inline T focalLengthY() const;
1105
1106 /**
1107 * Returns the inverse horizontal focal length parameter.
1108 * @see AnyCameraT::inverseFocalLengthX().
1109 */
1110 inline T inverseFocalLengthX() const;
1111
1112 /**
1113 * Returns the inverse vertical focal length parameter.
1114 * @see AnyCameraT::inverseFocalLengthY().
1115 */
1116 inline T inverseFocalLengthY() const;
1117
1118 /**
1119 * Projects a 3D object point into the camera frame.
1120 * @see AnyCameraT::projectToImageIF().
1121 */
1122 inline VectorT2<T> projectToImageIF(const VectorT3<T>& objectPoint) const;
1123
1124 /**
1125 * Projects a 3D object point into the camera frame.
1126 * @see AnyCameraT::projectToImageIF().
1127 */
1128 inline VectorT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint) const;
1129
1130 /**
1131 * Projects several 3D object points into the camera frame at once.
1132 * @see AnyCameraT::projectToImageIF().
1133 */
1134 inline void projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
1135
1136 /**
1137 * Projects several 3D object points into the camera frame at once.
1138 * @see AnyCameraT::projectToImageIF().
1139 */
1140 inline void projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
1141
1142 /**
1143 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
1144 * @see AnyCameraT::vectorIF().
1145 */
1146 inline VectorT3<T> vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const;
1147
1148 /**
1149 * Returns vectors starting at the camera's center and intersecting a given 2D points in the image.
1150 * @see AnyCameraT::vectorIF().
1151 */
1152 inline void vectorIF(const VectorT2<T>* distortedImagePoint, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const;
1153
1154 /**
1155 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
1156 * @see AnyCameraT::pointJacobian2x3IF().
1157 */
1158 inline void pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const;
1159
1160 /**
1161 * Returns whether two camera objects are identical up to a given epsilon.
1162 * @see AnyCameraT::isEqual().
1163 */
1164 inline bool isEqual(const CameraWrapperBasePinholeT<T>& basePinhole, const T eps = NumericT<T>::eps()) const;
1165
1166 /**
1167 * Returns whether this camera is valid.
1168 * @see AnyCameraT::isValid().
1169 */
1170 inline bool isValid() const;
1171
1172 /**
1173 * Returns a copy of the actual camera object.
1174 * @return New instance of the actual camera object used in this wrapper.
1175 * @tparam U The scalar data type of the resulting cloned object, either 'float' or 'double'
1176 */
1177 template <typename U>
1178 inline std::unique_ptr<AnyCameraT<U>> clone(const unsigned int width = 0u, const unsigned int height = 0u) const;
1179
1180 /**
1181 * Returns the type of this camera.
1182 * @see AnyCameraT::anyCameraType().
1183 */
1184 static inline AnyCameraType anyCameraType();
1185
1186 /**
1187 * Returns the name of this camera.
1188 * @see AnyCameraT::name().
1189 */
1190 static inline std::string name();
1191
1192 protected:
1193
1194 /// The actual pinhole camera.
1196};
1197
1198/**
1199 * This class implements the base wrapper around Ocean's fisheye camera profile.
1200 * The class can be used as 'TCameraWrapperBase' in 'CameraWrapperT' to create a full wrapper class e.g., 'CameraWrapperT<T, CameraWrapperBaseFisheyeT<T>>'.
1201 * @tparam T The data type of a scalar, 'float' or 'double'
1202 * @ingroup math
1203 */
1204template <typename T>
1206{
1207 public:
1208
1209 /**
1210 * Definition of the actual camera object wrapped by this class.
1211 */
1213
1214 /**
1215 * Definition of the parent WrappedCamera class using this base class.
1216 */
1218
1219 public:
1220
1221 /**
1222 * Creates a new CameraWrapperBaseFisheyeT object wrapping the actual camera model.
1223 * @param actualCamera The actual camera object to be wrapped
1224 */
1226
1227 /**
1228 * Creates a new CameraWrapperBaseFisheyeT object wrapping the actual camera model.
1229 * @param actualCamera The actual camera object to be wrapped
1230 */
1232
1233 /**
1234 * Returns the actual camera object wrapped in this class.
1235 * @return The wrapped camera object
1236 */
1237 inline const ActualCamera& actualCamera() const;
1238
1239 /**
1240 * Returns the width of the camera image.
1241 * @see AnyCameraT::width().
1242 */
1243 inline unsigned int width() const;
1244
1245 /**
1246 * Returns the height of the camera image.
1247 * @see AnyCameraT::height().
1248 */
1249 inline unsigned int height() const;
1250
1251 /**
1252 * Returns the coordinate of the principal point of the camera image in the pixel domain.
1253 * @see AnyCameraT::principalPoint().
1254 */
1255 inline VectorT2<T> principalPoint() const;
1256
1257 /**
1258 * Returns the x-value of the principal point of the camera image in the pixel domain.
1259 * @see AnyCameraT::principalPointX().
1260 */
1261 inline T principalPointX() const;
1262
1263 /**
1264 * Returns the y-value of the principal point of the camera image in the pixel domain.
1265 * @see AnyCameraT::principalPointY().
1266 */
1267 inline T principalPointY() const;
1268
1269 /**
1270 * Returns the horizontal focal length parameter.
1271 * @see AnyCameraT::focalLengthX().
1272 */
1273 inline T focalLengthX() const;
1274
1275 /**
1276 * Returns the vertical focal length parameter.
1277 * @see AnyCameraT::focalLengthY().
1278 */
1279 inline T focalLengthY() const;
1280
1281 /**
1282 * Returns the inverse horizontal focal length parameter.
1283 * @see AnyCameraT::inverseFocalLengthX().
1284 */
1285 inline T inverseFocalLengthX() const;
1286
1287 /**
1288 * Returns the inverse vertical focal length parameter.
1289 * @see AnyCameraT::inverseFocalLengthY().
1290 */
1291 inline T inverseFocalLengthY() const;
1292
1293 /**
1294 * Projects a 3D object point into the camera frame.
1295 * @see AnyCameraT::projectToImageIF().
1296 */
1297 inline VectorT2<T> projectToImageIF(const VectorT3<T>& objectPoint) const;
1298
1299 /**
1300 * Projects a 3D object point into the camera frame.
1301 * @see AnyCameraT::projectToImageIF().
1302 */
1303 inline VectorT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint) const;
1304
1305 /**
1306 * Projects several 3D object points into the camera frame at once.
1307 * @see AnyCameraT::projectToImageIF().
1308 */
1309 inline void projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
1310
1311 /**
1312 * Projects several 3D object points into the camera frame at once.
1313 * @see AnyCameraT::projectToImageIF().
1314 */
1315 inline void projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
1316
1317 /**
1318 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
1319 * @see AnyCameraT::vectorIF().
1320 */
1321 inline VectorT3<T> vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const;
1322
1323 /**
1324 * Returns vectors starting at the camera's center and intersecting a given 2D points in the image.
1325 * @see AnyCameraT::vectorIF().
1326 */
1327 inline void vectorIF(const VectorT2<T>* distortedImagePoint, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const;
1328
1329 /**
1330 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
1331 * @see AnyCameraT::pointJacobian2x3IF().
1332 */
1333 inline void pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const;
1334
1335 /**
1336 * Returns whether two camera objects are identical up to a given epsilon.
1337 * @see AnyCameraT::isEqual().
1338 */
1339 inline bool isEqual(const CameraWrapperBaseFisheyeT<T>& baseFisheye, const T eps = NumericT<T>::eps()) const;
1340
1341 /**
1342 * Returns whether this camera is valid.
1343 * @return True, if so
1344 */
1345 inline bool isValid() const;
1346
1347 /**
1348 * Returns a copy of the actual camera object.
1349 * @return New instance of the actual camera object used in this wrapper.
1350 */
1351 template <typename U>
1352 inline std::unique_ptr<AnyCameraT<U>> clone(const unsigned int width = 0u, const unsigned int height = 0u) const;
1353
1354 /**
1355 * Returns the type of this camera.
1356 * @see AnyCamera::anyCameraType().
1357 */
1358 static inline AnyCameraType anyCameraType();
1359
1360 /**
1361 * Returns the name of this camera.
1362 * @see AnyCamera::name().
1363 */
1364 static inline std::string name();
1365
1366 protected:
1367
1368 /// The actual fisheye camera object.
1370};
1371
1372/**
1373 * This class implements invalid camera profiles, e.g. when no intrinsic information is available.
1374 * @tparam T The data type of a 'Scalar', 'float' or 'double'
1375 * @ingroup math
1376 */
1377template <typename T>
1379{
1380 public:
1381
1382 /**
1383 * Creates an invalid camera
1384 * @param reason The reason why no valid camera is available, must be valid
1385 */
1386 explicit InvalidCameraT(const std::string& reason);
1387
1388 /**
1389 * Returns the reason of this invalid camera
1390 * @return The reason
1391 */
1392 const std::string& reason() const;
1393
1394 protected:
1395
1396 /// The reason why no valid camera is available.
1397 std::string reason_;
1398};
1399
1400/**
1401 * Definition of an invalid camera object based with element precision 'Scalar'.
1402 * @see AnyCameraT, AnyCameraInvalidT.
1403 * @ingroup math
1404 */
1406
1407/**
1408 * Definition of an invalid camera object based with element precision 'double'.
1409 * @see AnyCameraT, AnyCameraInvalidT.
1410 * @ingroup math
1411 */
1413
1414/**
1415 * Definition of an invalid camera object based with element precision 'float'.
1416 * @see AnyCameraT, AnyCameraInvalidT.
1417 * @ingroup math
1418 */
1420
1421/**
1422 * This class implements the base wrapper around an invalid camera profile.
1423 * The class can be used as 'TCameraWrapperBase' in 'CameraWrapperT' to create a full wrapper class e.g., 'CameraWrapperT<T, CameraWrapperBaseInvalidT<T>>'.
1424 * @tparam T The data type of a scalar, 'float' or 'double'
1425 * @ingroup math
1426 */
1427template <typename T>
1429{
1430 public:
1431
1432 /**
1433 * Definition of the actual camera object wrapped by this class.
1434 */
1436
1437 /**
1438 * Definition of the parent WrappedCamera class using this base class.
1439 */
1441
1442 public:
1443
1444 /**
1445 * Creates a new CameraWrapperBaseInvalidT object wrapping the actual camera model.
1446 * @param actualCamera The actual camera object to be wrapped
1447 */
1449
1450 /**
1451 * Creates a new CameraWrapperBaseInvalidT object wrapping the actual camera model.
1452 * @param actualCamera The actual camera object to be wrapped
1453 */
1455
1456 /**
1457 * Returns the actual camera object wrapped in this class.
1458 * @return The wrapped camera object
1459 */
1460 inline const ActualCamera& actualCamera() const;
1461
1462 /**
1463 * Returns the width of the camera image.
1464 * @see AnyCameraT::width().
1465 */
1466 inline unsigned int width() const;
1467
1468 /**
1469 * Returns the height of the camera image.
1470 * @see AnyCameraT::height().
1471 */
1472 inline unsigned int height() const;
1473
1474 /**
1475 * Returns the coordinate of the principal point of the camera image in the pixel domain.
1476 * @see AnyCameraT::principalPoint().
1477 */
1478 inline VectorT2<T> principalPoint() const;
1479
1480 /**
1481 * Returns the x-value of the principal point of the camera image in the pixel domain.
1482 * @see AnyCameraT::principalPointX().
1483 */
1484 inline T principalPointX() const;
1485
1486 /**
1487 * Returns the y-value of the principal point of the camera image in the pixel domain.
1488 * @see AnyCameraT::principalPointY().
1489 */
1490 inline T principalPointY() const;
1491
1492 /**
1493 * Returns the horizontal focal length parameter.
1494 * @see AnyCameraT::focalLengthX().
1495 */
1496 inline T focalLengthX() const;
1497
1498 /**
1499 * Returns the vertical focal length parameter.
1500 * @see AnyCameraT::focalLengthY().
1501 */
1502 inline T focalLengthY() const;
1503
1504 /**
1505 * Returns the inverse horizontal focal length parameter.
1506 * @see AnyCameraT::inverseFocalLengthX().
1507 */
1508 inline T inverseFocalLengthX() const;
1509
1510 /**
1511 * Returns the inverse vertical focal length parameter.
1512 * @see AnyCameraT::inverseFocalLengthY().
1513 */
1514 inline T inverseFocalLengthY() const;
1515
1516 /**
1517 * Projects a 3D object point into the camera frame.
1518 * @see AnyCameraT::projectToImageIF().
1519 */
1520 inline VectorT2<T> projectToImageIF(const VectorT3<T>& objectPoint) const;
1521
1522 /**
1523 * Projects a 3D object point into the camera frame.
1524 * @see AnyCameraT::projectToImageIF().
1525 */
1526 inline VectorT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint) const;
1527
1528 /**
1529 * Projects several 3D object points into the camera frame at once.
1530 * @see AnyCameraT::projectToImageIF().
1531 */
1532 inline void projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
1533
1534 /**
1535 * Projects several 3D object points into the camera frame at once.
1536 * @see AnyCameraT::projectToImageIF().
1537 */
1538 inline void projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const;
1539
1540 /**
1541 * Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
1542 * @see AnyCameraT::vectorIF().
1543 */
1544 inline VectorT3<T> vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const;
1545
1546 /**
1547 * Returns vectors starting at the camera's center and intersecting a given 2D points in the image.
1548 * @see AnyCameraT::vectorIF().
1549 */
1550 inline void vectorIF(const VectorT2<T>* distortedImagePoint, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const;
1551
1552 /**
1553 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
1554 * @see AnyCameraT::pointJacobian2x3IF().
1555 */
1556 inline void pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const;
1557
1558 /**
1559 * Returns whether two camera objects are identical up to a given epsilon.
1560 * @see AnyCameraT::isEqual().
1561 */
1562 inline bool isEqual(const CameraWrapperBaseInvalidT<T>& baseInvalid, const T eps = NumericT<T>::eps()) const;
1563
1564 /**
1565 * Returns whether this camera is valid.
1566 * @return True, if so
1567 */
1568 inline bool isValid() const;
1569
1570 /**
1571 * Returns a copy of the actual camera object.
1572 * @return New instance of the actual camera object used in this wrapper.
1573 */
1574 template <typename U>
1575 inline std::unique_ptr<AnyCameraT<U>> clone(const unsigned int width = 0u, const unsigned int height = 0u) const;
1576
1577 /**
1578 * Returns the type of this camera.
1579 * @see AnyCamera::anyCameraType().
1580 */
1581 static inline AnyCameraType anyCameraType();
1582
1583 /**
1584 * Returns the name of this camera.
1585 * @see AnyCamera::name().
1586 */
1587 static inline std::string name();
1588
1589 protected:
1590
1591 /// The actual invalid camera.
1593};
1594
1595/**
1596 * Definition of an AnyCamera object based on Ocean's pinhole camera class with template parameter to define the element precision.
1597 * @tparam T The scalar data type
1598 * @see AnyCameraT, CameraWrapperBasePinholeT, AnyCameraPinhole, AnyCameraPinholeD, AnyCameraPinholeF.
1599 * @ingroup math
1600 */
1601template <typename T>
1603
1604/**
1605 * Definition of an AnyCamera object based on Ocean's pinhole camera class with element precision 'Scalar'.
1606 * @see AnyCameraT, AnyCameraPinholeT.
1607 * @ingroup math
1608 */
1610
1611/**
1612 * Definition of an AnyCamera object based on Ocean's pinhole camera class with element precision 'double'.
1613 * @see AnyCameraT, AnyCameraPinholeT.
1614 * @ingroup math
1615 */
1617
1618/**
1619 * Definition of an AnyCamera object based on Ocean's pinhole camera class with element precision 'float'.
1620 * @see AnyCameraT, AnyCameraPinholeT.
1621 * @ingroup math
1622 */
1624
1625/**
1626 * Definition of an AnyCamera object based on Ocean's fisheye camera class with template parameter to define the element precision.
1627 * @tparam T The scalar data type
1628 * @see AnyCameraT, CameraWrapperBaseFisheyeT, AnyCameraFisheye, AnyCameraFisheyeD, AnyCameraFisheyeF.
1629 * @ingroup math
1630 */
1631template <typename T>
1633
1634/**
1635 * Definition of an AnyCamera object based on Ocean's fisheye camera class with element precision 'Scalar'.
1636 * @see AnyCameraT, AnyCameraFisheyeT.
1637 * @ingroup math
1638 */
1640
1641/**
1642 * Definition of an AnyCamera object based on Ocean's fisheye camera class with element precision 'double'.
1643 * @see AnyCameraT, AnyCameraFisheyeT.
1644 * @ingroup math
1645 */
1647
1648/**
1649 * Definition of an AnyCamera object based on Ocean's fisheye camera class with element precision 'float'.
1650 * @see AnyCameraT, AnyCameraFisheyeT.
1651 * @ingroup math
1652 */
1654
1655/**
1656 * Definition of an AnyCamera object based on an invalid (by design) camera with template parameter to define the element precision.
1657 * @tparam T The scalar data type
1658 * @see AnyCameraT, CameraWrapperBaseFisheyeT, AnyCameraInvalid, AnyCameraInvalidD, AnyCameraInvalidF.
1659 * @ingroup math
1660 */
1661template <typename T>
1663
1664/**
1665 * Definition of an AnyCamera object based on an invalid (by design) camera with element precision 'Scalar'.
1666 * @see AnyCameraT, AnyCameraInvalidT.
1667 * @ingroup math
1668 */
1670
1671/**
1672 * Definition of an AnyCamera object based on an invalid (by design) camera with element precision 'double'.
1673 * @see AnyCameraT, AnyCameraInvalidT.
1674 * @ingroup math
1675 */
1677
1678/**
1679 * Definition of an AnyCamera object based on an invalid (by design) camera with element precision 'float'.
1680 * @see AnyCameraT, AnyCameraInvalidT.
1681 * @ingroup math
1682 */
1684
1685template <typename T>
1687{
1688 ocean_assert(camera != nullptr && camera->isValid() && segmentSteps >= 1);
1689
1690 if (camera != nullptr && camera->isValid() && segmentSteps >= 1)
1691 {
1692 FiniteLinesT2<T> cameraBoundarySegments;
1693
1694 if (determineCameraBoundary(*camera, cameraBoundarySegments, segmentSteps))
1695 {
1696 camera_ = camera;
1697 cameraBoundarySegments_ = std::move(cameraBoundarySegments);
1698 }
1699 }
1700}
1701
1702template <typename T>
1703bool CameraProjectionCheckerT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint, VectorT2<T>* imagePoint) const
1704{
1705 ocean_assert(camera_ != nullptr);
1706 ocean_assert(camera_->isValid());
1707 ocean_assert(flippedCamera_T_world.isValid());
1708
1709 const VectorT3<T> cameraObjectPointIF = flippedCamera_T_world * objectPoint;
1710
1711 if (cameraObjectPointIF.z() <= NumericT<T>::eps())
1712 {
1713 return false;
1714 }
1715
1716 const T invZ = T(1) / cameraObjectPointIF.z();
1717
1718 const VectorT2<T> normalizedImagePoint(cameraObjectPointIF.x() * invZ, cameraObjectPointIF.y() * invZ);
1719
1720 if (!isInside(cameraBoundarySegments_, normalizedImagePoint))
1721 {
1722 return false;
1723 }
1724
1725 if (imagePoint != nullptr)
1726 {
1727 *imagePoint = camera_->projectToImageIF(cameraObjectPointIF);
1728
1729 ocean_assert_accuracy(camera_->isInside(*imagePoint, T(std::max(camera_->width(), camera_->height())) * T(-0.1)));
1730 }
1731
1732 return true;
1733}
1734
1735template <typename T>
1737{
1738 return camera_;
1739}
1740
1741template <typename T>
1743{
1744 return cameraBoundarySegments_;
1745}
1746
1747template <typename T>
1749{
1750 ocean_assert(camera_ == nullptr || !cameraBoundarySegments_.empty());
1751
1752 return camera_ != nullptr;
1753}
1754
1755template <typename T>
1756bool CameraProjectionCheckerT<T>::determineCameraBoundary(const AnyCameraT<T>& camera, FiniteLinesT2<T>& cameraBoundarySegments, const size_t segmentSteps)
1757{
1758 ocean_assert(camera.isValid());
1759 if (!camera.isValid())
1760 {
1761 return false;
1762 }
1763
1764 ocean_assert(segmentSteps >= 1);
1765 if (segmentSteps < 1)
1766 {
1767 return false;
1768 }
1769
1770 const std::array<VectorT2<T>, 4> corners =
1771 {
1772 VectorT2<T>(T(0), T(0)),
1773 VectorT2<T>(T(0), T(camera.height() - 1u)),
1774 VectorT2<T>(T(camera.width() - 1u), T(camera.height() - 1u)),
1775 VectorT2<T>(T(camera.width() - 1u), T(0))
1776 };
1777
1778 VectorsT2<T> normalizedImagePoints;
1779 normalizedImagePoints.reserve(corners.size() * segmentSteps);
1780
1781 for (size_t nCorner = 0; nCorner < corners.size(); ++nCorner)
1782 {
1783 const VectorT2<T>& corner0 = corners[nCorner];
1784 const VectorT2<T>& corner1 = corners[(nCorner + 1u) % corners.size()];
1785
1786 for (size_t nStep = 0; nStep < segmentSteps; ++nStep)
1787 {
1788 const T factor = T(nStep) / T(segmentSteps);
1789
1790 const VectorT2<T> distortedImagePoint = corner0 * (T(1) - factor) + corner1 * factor;
1791
1792 VectorT3<T> objectPoint = camera.vectorIF(distortedImagePoint);
1793 ocean_assert(objectPoint.z() >= NumericT<T>::eps());
1794
1795 const VectorT2<T> normalizedImagePoint = objectPoint.xy() / objectPoint.z();
1796
1797 normalizedImagePoints.emplace_back(normalizedImagePoint.x(), normalizedImagePoint.y());
1798 }
1799 }
1800
1801 ocean_assert(normalizedImagePoints.size() >= 3);
1802 ocean_assert(normalizedImagePoints.size() == segmentSteps * corners.size());
1803
1804 ocean_assert(cameraBoundarySegments.empty());
1805 cameraBoundarySegments.clear();
1806
1807 cameraBoundarySegments.reserve(normalizedImagePoints.size());
1808
1809 for (size_t n = 1; n < normalizedImagePoints.size(); ++n)
1810 {
1811 cameraBoundarySegments.emplace_back(normalizedImagePoints[n - 1], normalizedImagePoints[n]);
1812 }
1813
1814 cameraBoundarySegments.emplace_back(normalizedImagePoints.back(), normalizedImagePoints.front());
1815
1816 return true;
1817}
1818
1819template <typename T>
1820bool CameraProjectionCheckerT<T>::isInside(const FiniteLinesT2<T>& cameraBoundarySegments, const VectorT2<T>& imagePoint)
1821{
1822 ocean_assert(cameraBoundarySegments.size() >= 3);
1823
1824 size_t counter = 0;
1825
1826 for (const FiniteLineT2<T>& cameraBoundarySegment : cameraBoundarySegments)
1827 {
1828 // let's check whether the point is above or below the line segment
1829
1830 const bool segmentTopDown = cameraBoundarySegment.point0().y() < cameraBoundarySegment.point1().y();
1831
1832 if (segmentTopDown)
1833 {
1834 if (cameraBoundarySegment.point1().y() < imagePoint.y() || imagePoint.y() < cameraBoundarySegment.point0().y())
1835 {
1836 continue;
1837 }
1838 }
1839 else
1840 {
1841 if (cameraBoundarySegment.point0().y() < imagePoint.y() || imagePoint.y() < cameraBoundarySegment.point1().y())
1842 {
1843 continue;
1844 }
1845 }
1846
1847 if (cameraBoundarySegment.isOnLine(imagePoint))
1848 {
1849 // the point is on the line segment, so we know the point is inside the camera boundary
1850
1851 return true;
1852 }
1853
1854 if (cameraBoundarySegment.isLeftOfLine(imagePoint) == segmentTopDown)
1855 {
1856 // the point is on the left side of the line segment, we only count points on the right side
1857 continue;
1858 }
1859
1860 ++counter;
1861 }
1862
1863 return counter % 2 == 1;
1864}
1865
1866template <>
1867template <>
1868inline std::shared_ptr<AnyCameraT<float>> AnyCameraT<float>::convert(const std::shared_ptr<AnyCameraT<double>>& anyCamera)
1869{
1870 if (anyCamera)
1871 {
1872 return anyCamera->cloneToFloat();
1873 }
1874
1875 return nullptr;
1876}
1877
1878template <>
1879template <>
1880inline std::shared_ptr<AnyCameraT<double>> AnyCameraT<double>::convert(const std::shared_ptr<AnyCameraT<float>>& anyCamera)
1881{
1882 if (anyCamera)
1883 {
1884 return anyCamera->cloneToDouble();
1885 }
1886
1887 return nullptr;
1888}
1889
1890template <typename T>
1891template <typename U>
1892std::shared_ptr<AnyCameraT<T>> AnyCameraT<T>::convert(const std::shared_ptr<AnyCameraT<U>>& anyCamera)
1893{
1894 static_assert(std::is_same<T, U>::value, "Invalid data types!");
1895
1896 return anyCamera;
1897}
1898
1899template <typename T, typename TCameraWrapper>
1901 TCameraWrapper(std::move(actualCamera))
1902{
1903 // nothing to do here
1904}
1905
1906template <typename T, typename TCameraWrapper>
1908 TCameraWrapper(actualCamera)
1909{
1910 // nothing to do here
1911}
1912
1913template <typename T, typename TCameraWrapper>
1915{
1916 return TCameraWrapper::anyCameraType();
1917}
1918
1919template <typename T, typename TCameraWrapper>
1921{
1922 return TCameraWrapper::name();
1923}
1924
1925template <typename T, typename TCameraWrapper>
1926std::unique_ptr<AnyCameraT<T>> AnyCameraWrappingT<T, TCameraWrapper>::clone(const unsigned int width, const unsigned int height) const
1927{
1928 return TCameraWrapper::template clone<T>(width, height);
1929}
1930
1931template <typename T, typename TCameraWrapper>
1932std::unique_ptr<AnyCameraT<float>> AnyCameraWrappingT<T, TCameraWrapper>::cloneToFloat(const unsigned int width, const unsigned int height) const
1933{
1934 return TCameraWrapper::template clone<float>(width, height);
1935}
1936
1937template <typename T, typename TCameraWrapper>
1938std::unique_ptr<AnyCameraT<double>> AnyCameraWrappingT<T, TCameraWrapper>::cloneToDouble(const unsigned int width, const unsigned int height) const
1939{
1940 return TCameraWrapper::template clone<double>(width, height);
1941}
1942
1943template <typename T, typename TCameraWrapper>
1945{
1946 return TCameraWrapper::width();
1947}
1948
1949template <typename T, typename TCameraWrapper>
1951{
1952 return TCameraWrapper::height();
1953}
1954
1955template <typename T, typename TCameraWrapper>
1957{
1958 return TCameraWrapper::principalPoint();
1959}
1960
1961template <typename T, typename TCameraWrapper>
1963{
1964 return TCameraWrapper::principalPointX();
1965}
1966
1967template <typename T, typename TCameraWrapper>
1969{
1970 return TCameraWrapper::principalPointY();
1971}
1972
1973template <typename T, typename TCameraWrapper>
1975{
1976 return TCameraWrapper::focalLengthX();
1977}
1978
1979template <typename T, typename TCameraWrapper>
1981{
1982 return TCameraWrapper::focalLengthY();
1983}
1984
1985template <typename T, typename TCameraWrapper>
1987{
1988 return TCameraWrapper::inverseFocalLengthX();
1989}
1990
1991template <typename T, typename TCameraWrapper>
1993{
1994 return TCameraWrapper::inverseFocalLengthY();
1995}
1996
1997template <typename T, typename TCameraWrapper>
1999{
2000 return TCameraWrapper::fovX();
2001}
2002
2003template <typename T, typename TCameraWrapper>
2005{
2006 return TCameraWrapper::fovY();
2007}
2008
2009template <typename T, typename TCameraWrapper>
2010bool AnyCameraWrappingT<T, TCameraWrapper>::isInside(const VectorT2<T>& imagePoint, const T signedBorder) const
2011{
2012 return TCameraWrapper::isInside(imagePoint, signedBorder);
2013}
2014
2015template <typename T, typename TCameraWrapper>
2017{
2018 return TCameraWrapper::projectToImage(objectPoint);
2019}
2020
2021template <typename T, typename TCameraWrapper>
2023{
2024 return TCameraWrapper::projectToImage(world_T_camera, objectPoint);
2025}
2026
2027template <typename T, typename TCameraWrapper>
2029{
2030 return TCameraWrapper::projectToImageIF(objectPoint);
2031}
2032
2033template <typename T, typename TCameraWrapper>
2035{
2036 return TCameraWrapper::projectToImageIF(flippedCamera_T_world, objectPoint);
2037}
2038
2039template <typename T, typename TCameraWrapper>
2040void AnyCameraWrappingT<T, TCameraWrapper>::projectToImage(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2041{
2042 return TCameraWrapper::projectToImage(objectPoints, size, imagePoints);
2043}
2044
2045template <typename T, typename TCameraWrapper>
2046void AnyCameraWrappingT<T, TCameraWrapper>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2047{
2048 return TCameraWrapper::projectToImage(world_T_camera, objectPoints, size, imagePoints);
2049}
2050
2051template <typename T, typename TCameraWrapper>
2052void AnyCameraWrappingT<T, TCameraWrapper>::projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2053{
2054 return TCameraWrapper::projectToImageIF(objectPoints, size, imagePoints);
2055}
2056
2057template <typename T, typename TCameraWrapper>
2058void AnyCameraWrappingT<T, TCameraWrapper>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2059{
2060 return TCameraWrapper::projectToImageIF(flippedCamera_T_world, objectPoints, size, imagePoints);
2061}
2062
2063template <typename T, typename TCameraWrapper>
2064VectorT3<T> AnyCameraWrappingT<T, TCameraWrapper>::vector(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const
2065{
2066 return TCameraWrapper::vector(distortedImagePoint, makeUnitVector);
2067}
2068
2069template <typename T, typename TCameraWrapper>
2070void AnyCameraWrappingT<T, TCameraWrapper>::vector(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const
2071{
2072 return TCameraWrapper::vector(distortedImagePoints, size, vectors, makeUnitVector);
2073}
2074
2075template <typename T, typename TCameraWrapper>
2076VectorT3<T> AnyCameraWrappingT<T, TCameraWrapper>::vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const
2077{
2078 return TCameraWrapper::vectorIF(distortedImagePoint, makeUnitVector);
2079}
2080
2081template <typename T, typename TCameraWrapper>
2082void AnyCameraWrappingT<T, TCameraWrapper>::vectorIF(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const
2083{
2084 return TCameraWrapper::vectorIF(distortedImagePoints, size, vectors, makeUnitVector);
2085}
2086
2087template <typename T, typename TCameraWrapper>
2088LineT3<T> AnyCameraWrappingT<T, TCameraWrapper>::ray(const VectorT2<T>& distortedImagePoint, const HomogenousMatrixT4<T>& world_T_camera) const
2089{
2090 return TCameraWrapper::ray(distortedImagePoint, world_T_camera);
2091}
2092
2093template <typename T, typename TCameraWrapper>
2095{
2096 return TCameraWrapper::ray(distortedImagePoint);
2097}
2098
2099template <typename T, typename TCameraWrapper>
2100void AnyCameraWrappingT<T, TCameraWrapper>::pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const
2101{
2102 return TCameraWrapper::pointJacobian2x3IF(flippedCameraObjectPoint, jx, jy);
2103}
2104
2105template <typename T, typename TCameraWrapper>
2106void AnyCameraWrappingT<T, TCameraWrapper>::pointJacobian2nx3IF(const VectorT3<T>* flippedCameraObjectPoints, const size_t numberObjectPoints, T* jacobians) const
2107{
2108 return TCameraWrapper::pointJacobian2nx3IF(flippedCameraObjectPoints, numberObjectPoints, jacobians);
2109}
2110
2111template <typename T, typename TCameraWrapper>
2112bool AnyCameraWrappingT<T, TCameraWrapper>::isEqual(const AnyCameraT<T>& anyCamera, const T eps) const
2113{
2114 ocean_assert(eps >= T(0));
2115
2116 if (isValid() != anyCamera.isValid())
2117 {
2118 // one camera is value, one is not valid
2119 return false;
2120 }
2121
2122 if (!isValid())
2123 {
2124 // both cameras are invalid
2125 return true;
2126 }
2127
2128 if (name() != anyCamera.name())
2129 {
2130 return false;
2131 }
2132
2133 return TCameraWrapper::isEqual((const AnyCameraWrappingT<T, TCameraWrapper>&)(anyCamera), eps);
2134}
2135
2136template <typename T, typename TCameraWrapper>
2138{
2139 return TCameraWrapper::isValid();
2140}
2141
2142template <typename T, typename TCameraWrapperBase>
2144 TCameraWrapperBase(std::move(actualCamera))
2145{
2146 // nothing to do here
2147}
2148
2149template <typename T, typename TCameraWrapperBase>
2151 TCameraWrapperBase(actualCamera)
2152{
2153 // nothing to do here
2154}
2155
2156template <typename T, typename TCameraWrapperBase>
2158{
2159 return VectorT2<T>(TCameraWrapperBase::principalPointX(), TCameraWrapperBase::principalPointY());
2160}
2161
2162template <typename T, typename TCameraWrapperBase>
2164{
2165 ocean_assert(TCameraWrapperBase::isValid());
2166
2167 /**
2168 * x = Fx * X / Z + mx
2169 *
2170 * (x - mx) / Fx = X / Z
2171 */
2172
2173 if (NumericT<T>::isEqualEps(TCameraWrapperBase::focalLengthX()))
2174 {
2175 return T(0);
2176 }
2177
2178 const T leftAngle = NumericT<T>::abs(NumericT<T>::atan(-TCameraWrapperBase::principalPointX() * TCameraWrapperBase::inverseFocalLengthX()));
2179
2180 if (T(TCameraWrapperBase::width()) <= TCameraWrapperBase::principalPointX())
2181 {
2182 ocean_assert(false && "Invalid principal point");
2183 return T(2) * leftAngle;
2184 }
2185
2186 const T rightAngle = NumericT<T>::atan((T(TCameraWrapperBase::width()) - TCameraWrapperBase::principalPointX()) * TCameraWrapperBase::inverseFocalLengthX());
2187
2188 return leftAngle + rightAngle;
2189}
2190
2191template <typename T, typename TCameraWrapperBase>
2193{
2194 ocean_assert(TCameraWrapperBase::isValid());
2195
2196 /**
2197 * y = Fy * Y / Z + my
2198 *
2199 * (y - my) / Fy = Y / Z
2200 */
2201
2202 if (NumericT<T>::isEqualEps(TCameraWrapperBase::focalLengthY()))
2203 {
2204 return T(0);
2205 }
2206
2207 const T topAngle = NumericT<T>::abs(NumericT<T>::atan(-TCameraWrapperBase::principalPointY() * TCameraWrapperBase::inverseFocalLengthY()));
2208
2209 if (T(TCameraWrapperBase::height()) <= TCameraWrapperBase::principalPointY())
2210 {
2211 ocean_assert(false && "Invalid principal point");
2212 return T(2) * topAngle;
2213 }
2214
2215 const T bottomAngle = NumericT<T>::atan((T(TCameraWrapperBase::height()) - TCameraWrapperBase::principalPointY()) * TCameraWrapperBase::inverseFocalLengthY());
2216
2217 return topAngle + bottomAngle;
2218}
2219
2220template <typename T, typename TCameraWrapperBase>
2221inline bool CameraWrapperT<T, TCameraWrapperBase>::isInside(const VectorT2<T>& imagePoint, const T signedBorder) const
2222{
2223 ocean_assert(TCameraWrapperBase::isValid());
2224
2225 const unsigned int cameraWidth = TCameraWrapperBase::width();
2226 const unsigned int cameraHeight = TCameraWrapperBase::height();
2227
2228 ocean_assert(signedBorder < T(std::min(cameraWidth / 2u, cameraHeight / 2u)));
2229
2230 return imagePoint.x() >= signedBorder && imagePoint.y() >= signedBorder
2231 && imagePoint.x() < T(cameraWidth) - signedBorder && imagePoint.y() < T(cameraHeight) - signedBorder;
2232}
2233
2234template <typename T, typename TCameraWrapperBase>
2236{
2237 return TCameraWrapperBase::projectToImageIF(VectorT3<T>(objectPoint.x(), -objectPoint.y(), -objectPoint.z()));
2238}
2239
2240template <typename T, typename TCameraWrapperBase>
2242{
2243 return TCameraWrapperBase::projectToImageIF(CameraT<T>::standard2InvertedFlipped(world_T_camera), objectPoint);
2244}
2245
2246template <typename T, typename TCameraWrapperBase>
2247inline void CameraWrapperT<T, TCameraWrapperBase>::projectToImage(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2248{
2249 ocean_assert(size == 0 || objectPoints != nullptr);
2250 ocean_assert(size == 0 || imagePoints != nullptr);
2251
2252 for (size_t n = 0; n < size; ++n)
2253 {
2254 const VectorT3<T>& objectPoint = objectPoints[n];
2255 imagePoints[n] = TCameraWrapperBase::projectToImageIF(VectorT3<T>(objectPoint.x(), -objectPoint.y(), -objectPoint.z()));
2256 }
2257}
2258
2259template <typename T, typename TCameraWrapperBase>
2260inline void CameraWrapperT<T, TCameraWrapperBase>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2261{
2262 return TCameraWrapperBase::projectToImageIF(CameraT<T>::standard2InvertedFlipped(world_T_camera), objectPoints, size, imagePoints);
2263}
2264
2265template <typename T, typename TCameraWrapperBase>
2266inline VectorT3<T> CameraWrapperT<T, TCameraWrapperBase>::vector(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const
2267{
2268 const VectorT3<T> localVectorIF(TCameraWrapperBase::vectorIF(distortedImagePoint, makeUnitVector));
2269
2270 return VectorT3<T>(localVectorIF.x(), -localVectorIF.y(), -localVectorIF.z());
2271}
2272
2273template <typename T, typename TCameraWrapperBase>
2274inline void CameraWrapperT<T, TCameraWrapperBase>::vector(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const
2275{
2276 TCameraWrapperBase::vectorIF(distortedImagePoints, size, vectors, makeUnitVector);
2277
2278 for (size_t n = 0; n < size; ++n)
2279 {
2280 const VectorT3<T>& localVectorIF = vectors[n];
2281
2282 vectors[n] = VectorT3<T>(localVectorIF.x(), -localVectorIF.y(), -localVectorIF.z());
2283 }
2284}
2285
2286template <typename T, typename TCameraWrapperBase>
2287inline LineT3<T> CameraWrapperT<T, TCameraWrapperBase>::ray(const VectorT2<T>& distortedImagePoint, const HomogenousMatrixT4<T>& world_T_camera) const
2288{
2289 ocean_assert(TCameraWrapperBase::isValid() && world_T_camera.isValid());
2290
2291 return LineT3<T>(world_T_camera.translation(), world_T_camera.rotationMatrix(vector(distortedImagePoint, true /*makeUnitVector*/)));
2292}
2293
2294template <typename T, typename TCameraWrapperBase>
2296{
2297 ocean_assert(TCameraWrapperBase::isValid());
2298
2299 return LineT3<T>(VectorT3<T>(0, 0, 0), vector(distortedImagePoint, true /*makeUnitVector*/));
2300}
2301
2302template <typename T, typename TCameraWrapperBase>
2303inline void CameraWrapperT<T, TCameraWrapperBase>::pointJacobian2nx3IF(const VectorT3<T>* flippedCameraObjectPoints, const size_t numberObjectPoints, T* jacobians) const
2304{
2305 ocean_assert(flippedCameraObjectPoints != nullptr);
2306 ocean_assert(numberObjectPoints >= 1);
2307 ocean_assert(jacobians != nullptr);
2308
2309 for (size_t n = 0; n < numberObjectPoints; ++n)
2310 {
2311 TCameraWrapperBase::pointJacobian2x3IF(flippedCameraObjectPoints[n], jacobians + 0, jacobians + 3);
2312 jacobians += 6;
2313 }
2314}
2315
2316template <typename T>
2318 actualCamera_(std::move(actualCamera))
2319{
2320 // nothing to do here
2321}
2322
2323template <typename T>
2325 actualCamera_(actualCamera)
2326{
2327 // nothing to do here
2328}
2329
2330template <typename T>
2332{
2333 return actualCamera_;
2334}
2335
2336template <typename T>
2337inline unsigned int CameraWrapperBasePinholeT<T>::width() const
2338{
2339 return actualCamera_.width();
2340}
2341
2342template <typename T>
2343inline unsigned int CameraWrapperBasePinholeT<T>::height() const
2344{
2345 return actualCamera_.height();
2346}
2347
2348template <typename T>
2350{
2351 return T(actualCamera_.principalPointX());
2352}
2353
2354template <typename T>
2356{
2357 return T(actualCamera_.principalPointY());
2358}
2359
2360template <typename T>
2362{
2363 return T(actualCamera_.focalLengthX());
2364}
2365
2366template <typename T>
2368{
2369 return T(actualCamera_.focalLengthY());
2370}
2371
2372template <typename T>
2374{
2375 return T(actualCamera_.inverseFocalLengthX());
2376}
2377
2378template <typename T>
2380{
2381 return T(actualCamera_.inverseFocalLengthY());
2382}
2383
2384template <typename T>
2386{
2387 return VectorT2<T>(actualCamera_.template projectToImageIF<true>(HomogenousMatrixT4<T>(true), objectPoint, true));
2388}
2389
2390template <typename T>
2391inline VectorT2<T> CameraWrapperBasePinholeT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint) const
2392{
2393 return VectorT2<T>(actualCamera_.template projectToImageIF<true>(flippedCamera_T_world, objectPoint, true));
2394}
2395
2396template <typename T>
2397inline void CameraWrapperBasePinholeT<T>::projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2398{
2399 ocean_assert(size == 0 || objectPoints != nullptr);
2400 ocean_assert(size == 0 || imagePoints != nullptr);
2401
2402 for (size_t n = 0; n < size; ++n)
2403 {
2404 imagePoints[n] = projectToImageIF(objectPoints[n]);
2405 }
2406}
2407
2408template <typename T>
2409inline void CameraWrapperBasePinholeT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2410{
2411 ocean_assert(size == 0 || objectPoints != nullptr);
2412 ocean_assert(size == 0 || imagePoints != nullptr);
2413
2414 for (size_t n = 0; n < size; ++n)
2415 {
2416 imagePoints[n] = projectToImageIF(flippedCamera_T_world, objectPoints[n]);
2417 }
2418}
2419
2420template <typename T>
2421inline VectorT3<T> CameraWrapperBasePinholeT<T>::vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const
2422{
2423 const VectorT2<T> undistortedImagePoint(actualCamera_.template undistort<true>(distortedImagePoint));
2424
2425 return VectorT3<T>(actualCamera_.vectorIF(undistortedImagePoint, makeUnitVector));
2426}
2427
2428template <typename T>
2429inline void CameraWrapperBasePinholeT<T>::vectorIF(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const
2430{
2431 ocean_assert(distortedImagePoints != nullptr && size > 0);
2432 ocean_assert(vectors != nullptr);
2433
2434 for (size_t n = 0; n < size; ++n)
2435 {
2436 vectors[n] = vectorIF(distortedImagePoints[n], makeUnitVector);
2437 }
2438}
2439
2440template <typename T>
2441inline void CameraWrapperBasePinholeT<T>::pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const
2442{
2443 ocean_assert(jx != nullptr && jy != nullptr);
2444 actualCamera_.template pointJacobian2x3IF<T, true>(flippedCameraObjectPoint, jx, jy);
2445}
2446
2447template <typename T>
2448inline bool CameraWrapperBasePinholeT<T>::isEqual(const CameraWrapperBasePinholeT<T>& basePinhole, const T eps) const
2449{
2450 ocean_assert(eps >= T(0));
2451
2452 return actualCamera_.isEqual(basePinhole.actualCamera_, eps);
2453}
2454
2455template <typename T>
2457{
2458 return actualCamera_.isValid();
2459}
2460
2461template <typename T>
2462template <typename U>
2463inline std::unique_ptr<AnyCameraT<U>> CameraWrapperBasePinholeT<T>::clone(const unsigned int width, const unsigned int height) const
2464{
2465 ocean_assert(actualCamera_.isValid());
2466
2467 if constexpr (std::is_same<T, U>::value)
2468 {
2469 if ((width == 0u && height == 0u) || (width == actualCamera_.width() && height == actualCamera_.height()))
2470 {
2471 return std::make_unique<AnyCameraWrappingT<U, CameraWrapperT<U, CameraWrapperBasePinholeT<U>>>>(actualCamera_);
2472 }
2473
2474 const unsigned int validWidth = (height * actualCamera_.width() + actualCamera_.height() / 2u) / actualCamera_.height();
2475 const unsigned int validHeight = (width * actualCamera_.height() + actualCamera_.width() / 2u) / actualCamera_.width();
2476
2477 if (!NumericT<unsigned int>::isEqual(width, validWidth, 1u) && !NumericT<unsigned int>::isEqual(height, validHeight, 1u)) // either of the valid width/height needs to be close by 1 pixel
2478 {
2479 ocean_assert(false && "Wrong aspect ratio!");
2480 return nullptr;
2481 }
2482
2483 return std::make_unique<AnyCameraWrappingT<U, CameraWrapperT<U, CameraWrapperBasePinholeT<U>>>>(PinholeCameraT<U>(width, height, actualCamera_));
2484 }
2485 else
2486 {
2487 const PinholeCameraT<U> convertedPinholeCamera(actualCamera_);
2488
2489 if ((width == 0u && height == 0u) || (width == actualCamera_.width() && height == actualCamera_.height()))
2490 {
2491 return std::make_unique<AnyCameraWrappingT<U, CameraWrapperT<U, CameraWrapperBasePinholeT<U>>>>(convertedPinholeCamera);
2492 }
2493
2494 const unsigned int validWidth = (height * actualCamera_.width() + actualCamera_.height() / 2u) / actualCamera_.height();
2495 const unsigned int validHeight = (width * actualCamera_.height() + actualCamera_.width() / 2u) / actualCamera_.width();
2496
2497 if (!NumericT<unsigned int>::isEqual(width, validWidth, 1u) && !NumericT<unsigned int>::isEqual(height, validHeight, 1u)) // either of the valid width/height needs to be close by 1 pixel
2498 {
2499 ocean_assert(false && "Wrong aspect ratio!");
2500 return nullptr;
2501 }
2502
2503 return std::make_unique<AnyCameraWrappingT<U, CameraWrapperT<U, CameraWrapperBasePinholeT<U>>>>(PinholeCameraT<U>(width, height, convertedPinholeCamera));
2504 }
2505}
2506
2507template <typename T>
2512
2513template <typename T>
2515{
2516 return std::string("Ocean Pinhole");
2517}
2518
2519template <typename T>
2521 actualCamera_(std::move(camera))
2522{
2523 // nothing to do here
2524}
2525
2526template <typename T>
2528 actualCamera_(camera)
2529{
2530 // nothing to do here
2531}
2532
2533template <typename T>
2535{
2536 return actualCamera_;
2537}
2538
2539template <typename T>
2540inline unsigned int CameraWrapperBaseFisheyeT<T>::width() const
2541{
2542 return actualCamera_.width();
2543}
2544
2545template <typename T>
2546inline unsigned int CameraWrapperBaseFisheyeT<T>::height() const
2547{
2548 return actualCamera_.height();
2549}
2550
2551template <typename T>
2553{
2554 return actualCamera_.principalPoint();
2555}
2556
2557template <typename T>
2559{
2560 return actualCamera_.principalPointX();
2561}
2562
2563template <typename T>
2565{
2566 return actualCamera_.principalPointY();
2567}
2568
2569template <typename T>
2571{
2572 return actualCamera_.focalLengthX();
2573}
2574
2575template <typename T>
2577{
2578 return actualCamera_.focalLengthY();
2579}
2580
2581template <typename T>
2583{
2584 return actualCamera_.inverseFocalLengthX();
2585}
2586
2587template <typename T>
2589{
2590 return actualCamera_.inverseFocalLengthY();
2591}
2592
2593template <typename T>
2595{
2596 return actualCamera_.projectToImageIF(objectPoint);
2597}
2598
2599template <typename T>
2600inline VectorT2<T> CameraWrapperBaseFisheyeT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint) const
2601{
2602 return actualCamera_.projectToImageIF(flippedCamera_T_world, objectPoint);
2603}
2604
2605template <typename T>
2606inline void CameraWrapperBaseFisheyeT<T>::projectToImageIF(const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2607{
2608 ocean_assert(size == 0 || objectPoints != nullptr);
2609 ocean_assert(size == 0 || imagePoints != nullptr);
2610
2611 for (size_t n = 0; n < size; ++n)
2612 {
2613 imagePoints[n] = projectToImageIF(objectPoints[n]);
2614 }
2615}
2616
2617template <typename T>
2618inline void CameraWrapperBaseFisheyeT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* objectPoints, const size_t size, VectorT2<T>* imagePoints) const
2619{
2620 ocean_assert(size == 0 || objectPoints != nullptr);
2621 ocean_assert(size == 0 || imagePoints != nullptr);
2622
2623 for (size_t n = 0; n < size; ++n)
2624 {
2625 imagePoints[n] = projectToImageIF(flippedCamera_T_world, objectPoints[n]);
2626 }
2627}
2628
2629template <typename T>
2630inline VectorT3<T> CameraWrapperBaseFisheyeT<T>::vectorIF(const VectorT2<T>& distortedImagePoint, const bool makeUnitVector) const
2631{
2632 return actualCamera_.vectorIF(distortedImagePoint, makeUnitVector);
2633}
2634
2635template <typename T>
2636inline void CameraWrapperBaseFisheyeT<T>::vectorIF(const VectorT2<T>* distortedImagePoints, const size_t size, VectorT3<T>* vectors, const bool makeUnitVector) const
2637{
2638 ocean_assert(distortedImagePoints != nullptr && size > 0);
2639 ocean_assert(vectors != nullptr);
2640
2641 for (size_t n = 0; n < size; ++n)
2642 {
2643 vectors[n] = vectorIF(distortedImagePoints[n], makeUnitVector);
2644 }
2645}
2646
2647template <typename T>
2648inline void CameraWrapperBaseFisheyeT<T>::pointJacobian2x3IF(const VectorT3<T>& flippedCameraObjectPoint, T* jx, T* jy) const
2649{
2650 actualCamera_.pointJacobian2x3IF(flippedCameraObjectPoint, jx, jy);
2651}
2652
2653template <typename T>
2654inline bool CameraWrapperBaseFisheyeT<T>::isEqual(const CameraWrapperBaseFisheyeT& baseFisheye, const T eps) const
2655{
2656 ocean_assert(eps >= T(0));
2657
2658 return actualCamera_.isEqual(baseFisheye.actualCamera_, eps);
2659}
2660
2661template <typename T>
2663{
2664 return actualCamera_.isValid();
2665}
2666
2667template <typename T>
2668template <typename U>
2669inline std::unique_ptr<AnyCameraT<U>> CameraWrapperBaseFisheyeT<T>::clone(const unsigned int width, const unsigned int height) const
2670{
2671 ocean_assert(actualCamera_.isValid());
2672
2673 if ((width == 0u && height == 0u) || (width == actualCamera_.width() && height == actualCamera_.height()))
2674 {
2675 return std::make_unique<AnyCameraWrappingT<U, CameraWrapperT<U, CameraWrapperBaseFisheyeT<U>>>>(FisheyeCameraT<U>(actualCamera_));
2676 }
2677
2678 const unsigned int validWidth = (height * actualCamera_.width() + actualCamera_.height() / 2u) / actualCamera_.height();
2679
2680 if (!NumericT<unsigned int>::isEqual(width, validWidth, 1u))
2681 {
2682 ocean_assert(false && "Wrong aspect ratio!");
2683 return nullptr;
2684 }
2685
2686 const T xFactor = T(width) / T(actualCamera_.width());
2687 const T yFactor = T(height) / T(actualCamera_.height());
2688
2689 const U newPrincipalX = U(actualCamera_.principalPointX() * xFactor);
2690 const U newPrincipalY = U(actualCamera_.principalPointY() * yFactor);
2691
2692 const U newFocalLengthX = U(actualCamera_.focalLengthX() * xFactor);
2693 const U newFocalLengthY = U(actualCamera_.focalLengthY() * yFactor);
2694
2695 U radialDistortion[6];
2696 for (unsigned int n = 0u; n < 6u; ++n)
2697 {
2698 radialDistortion[n] = U(actualCamera_.radialDistortion()[n]);
2699 }
2700
2701 const U tangentialDistortion[2] =
2702 {
2703 U(actualCamera_.tangentialDistortion()[0]),
2704 U(actualCamera_.tangentialDistortion()[1])
2705 };
2706
2707 return std::make_unique<AnyCameraWrappingT<U, CameraWrapperT<U, CameraWrapperBaseFisheyeT<U>>>>(FisheyeCameraT<U>(width, height, newFocalLengthX, newFocalLengthY, newPrincipalX, newPrincipalY, radialDistortion, tangentialDistortion));
2708}
2709
2710template <typename T>
2715
2716template <typename T>
2718{
2719 return std::string("Ocean Fisheye");
2720}
2721
2722template <typename T>
2724 actualCamera_(std::move(camera))
2725{
2726 // nothing to do here
2727}
2728
2729template <typename T>
2731 actualCamera_(camera)
2732{
2733 // nothing to do here
2734}
2735
2736template <typename T>
2738{
2739 Log::error() << "Invalid camera: " << actualCamera_.reason();
2740
2741 ocean_assert(false && "This function must never be called.");
2742
2743 return actualCamera_;
2744}
2745
2746template <typename T>
2747inline unsigned int CameraWrapperBaseInvalidT<T>::width() const
2748{
2749 Log::error() << "Invalid camera: " << actualCamera_.reason();
2750
2751 ocean_assert(false && "This function must never be called.");
2752
2753 return (unsigned int)(-1);
2754}
2755
2756template <typename T>
2757inline unsigned int CameraWrapperBaseInvalidT<T>::height() const
2758{
2759 Log::error() << "Invalid camera: " << actualCamera_.reason();
2760
2761 ocean_assert(false && "This function must never be called.");
2762
2763 return (unsigned int)(-1);
2764}
2765
2766template <typename T>
2768{
2769 Log::error() << "Invalid camera: " << actualCamera_.reason();
2770
2771 ocean_assert(false && "This function must never be called.");
2772
2774}
2775
2776template <typename T>
2778{
2779 Log::error() << "Invalid camera: " << actualCamera_.reason();
2780
2781 ocean_assert(false && "This function must never be called.");
2782
2783 return NumericT<T>::minValue();
2784}
2785
2786template <typename T>
2788{
2789 Log::error() << "Invalid camera: " << actualCamera_.reason();
2790
2791 ocean_assert(false && "This function must never be called.");
2792
2793 return NumericT<T>::minValue();
2794}
2795
2796template <typename T>
2798{
2799 Log::error() << "Invalid camera: " << actualCamera_.reason();
2800
2801 ocean_assert(false && "This function must never be called.");
2802
2803 return NumericT<T>::minValue();
2804}
2805
2806template <typename T>
2808{
2809 Log::error() << "Invalid camera: " << actualCamera_.reason();
2810
2811 ocean_assert(false && "This function must never be called.");
2812
2813 return NumericT<T>::minValue();
2814}
2815
2816template <typename T>
2818{
2819 Log::error() << "Invalid camera: " << actualCamera_.reason();
2820
2821 ocean_assert(false && "This function must never be called.");
2822
2823 return NumericT<T>::minValue();
2824}
2825
2826template <typename T>
2828{
2829 Log::error() << "Invalid camera: " << actualCamera_.reason();
2830
2831 ocean_assert(false && "This function must never be called.");
2832
2833 return NumericT<T>::minValue();
2834}
2835
2836template <typename T>
2838{
2839 Log::error() << "Invalid camera: " << actualCamera_.reason();
2840
2841 ocean_assert(false && "This function must never be called.");
2842
2844}
2845
2846template <typename T>
2847inline VectorT2<T> CameraWrapperBaseInvalidT<T>::projectToImageIF(const HomogenousMatrixT4<T>& /*flippedCamera_T_world*/, const VectorT3<T>& /*objectPoint*/) const
2848{
2849 Log::error() << "Invalid camera: " << actualCamera_.reason();
2850
2851 ocean_assert(false && "This function must never be called.");
2852
2854}
2855
2856template <typename T>
2857inline void CameraWrapperBaseInvalidT<T>::projectToImageIF(const VectorT3<T>* /*objectPoints*/, const size_t /*size*/, VectorT2<T>* /*imagePoints*/) const
2858{
2859 Log::error() << "Invalid camera: " << actualCamera_.reason();
2860
2861 ocean_assert(false && "This function must never be called.");
2862
2863}
2864
2865template <typename T>
2866inline void CameraWrapperBaseInvalidT<T>::projectToImageIF(const HomogenousMatrixT4<T>& /*flippedCamera_T_world*/, const VectorT3<T>* /*objectPoints*/, const size_t /*size*/, VectorT2<T>* /*imagePoints*/) const
2867{
2868 Log::error() << "Invalid camera: " << actualCamera_.reason();
2869
2870 ocean_assert(false && "This function must never be called.");
2871}
2872
2873template <typename T>
2874inline VectorT3<T> CameraWrapperBaseInvalidT<T>::vectorIF(const VectorT2<T>& /*distortedImagePoint*/, const bool /*makeUnitVector*/) const
2875{
2876 Log::error() << "Invalid camera: " << actualCamera_.reason();
2877
2878 ocean_assert(false && "This function must never be called.");
2879
2881}
2882
2883template <typename T>
2884inline void CameraWrapperBaseInvalidT<T>::vectorIF(const VectorT2<T>* /*distortedImagePoints*/, const size_t /*size*/, VectorT3<T>* /*vectors*/, const bool /*makeUnitVector*/) const
2885{
2886 Log::error() << "Invalid camera: " << actualCamera_.reason();
2887
2888 ocean_assert(false && "This function must never be called.");
2889}
2890
2891template <typename T>
2892inline void CameraWrapperBaseInvalidT<T>::pointJacobian2x3IF(const VectorT3<T>& /*flippedCameraObjectPoint*/, T* /*jx*/, T* /*jy*/) const
2893{
2894 Log::error() << "Invalid camera: " << actualCamera_.reason();
2895
2896 ocean_assert(false && "This function must never be called.");
2897}
2898
2899template <typename T>
2900inline bool CameraWrapperBaseInvalidT<T>::isEqual(const CameraWrapperBaseInvalidT& /*baseFisheye*/, const T /*eps*/) const
2901{
2902 Log::error() << "Invalid camera: " << actualCamera_.reason();
2903
2904 ocean_assert(false && "This function must never be called.");
2905
2906 return false;
2907}
2908
2909template <typename T>
2911{
2912 return false;
2913}
2914
2915template <typename T>
2916template <typename U>
2917inline std::unique_ptr<AnyCameraT<U>> CameraWrapperBaseInvalidT<T>::clone(const unsigned int /*width*/, const unsigned int /*height*/) const
2918{
2919 Log::error() << "Invalid camera: " << actualCamera_.reason();
2920
2921 ocean_assert(false && "This function must never be called.");
2922
2923 return nullptr;
2924}
2925
2926template <typename T>
2931
2932template <typename T>
2934{
2935 return std::string("Invalid camera");
2936}
2937
2938template <typename T>
2939InvalidCameraT<T>::InvalidCameraT(const std::string& reason) :
2940 reason_(reason)
2941{
2942 // nothing to do here
2943}
2944
2945template <typename T>
2946const std::string& InvalidCameraT<T>::reason() const
2947{
2948 return reason_;
2949}
2950
2951}
2952
2953#endif // META_OCEAN_MATH_ANY_CAMERA_H
This class implements the abstract base class for all AnyCamera objects.
Definition AnyCamera.h:130
virtual ~AnyCameraT()=default
Destructs the AnyCamera object.
virtual VectorT3< T > vector(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector=true) const =0
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
AnyCameraT(AnyCameraT< T > &&)=delete
Disabled move constructor.
virtual LineT3< T > ray(const VectorT2< T > &distortedImagePoint) const =0
Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
virtual unsigned int width() const =0
Returns the width of the camera image.
virtual T focalLengthX() const =0
Returns the horizontal focal length parameter.
virtual VectorT2< T > projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const VectorT3< T > &objectPoint) const =0
Projects a 3D object point into the camera frame.
virtual void vectorIF(const VectorT2< T > *distortedImagePoints, const size_t size, VectorT3< T > *vectors, const bool makeUnitVector=true) const =0
Returns vectors starting at the camera's center and intersecting a given 2D points in the image.
virtual void projectToImage(const VectorT3< T > *objectPoints, const size_t size, VectorT2< T > *imagePoints) const =0
Projects several 3D object points into the camera frame at once.
virtual VectorT2< T > principalPoint() const =0
Returns the coordinate of the principal point of the camera image in the pixel domain.
virtual std::unique_ptr< AnyCameraT< double > > cloneToDouble(const unsigned int width=0u, const unsigned int height=0u) const =0
Returns a copy of this camera object with double precision.
virtual T inverseFocalLengthY() const =0
Returns the inverse vertical focal length parameter.
AnyCameraT & operator=(AnyCameraT &&)=delete
Disabled move operator.
virtual void projectToImageIF(const VectorT3< T > *objectPoints, const size_t size, VectorT2< T > *imagePoints) const =0
Projects several 3D object points into the camera frame at once.
virtual std::unique_ptr< AnyCameraT< T > > clone(const unsigned int width=0u, const unsigned int height=0u) const =0
Returns a copy of this camera object.
virtual T fovX() const =0
Returns the field of view in x direction of the camera.
virtual VectorT2< T > projectToImage(const VectorT3< T > &objectPoint) const =0
Projects a 3D object point into the camera frame.
virtual T inverseFocalLengthX() const =0
Returns the inverse horizontal focal length parameter.
virtual T focalLengthY() const =0
Returns the vertical focal length parameter.
virtual void pointJacobian2x3IF(const VectorT3< T > &flippedCameraObjectPoint, T *jx, T *jy) const =0
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
AnyCameraT & operator=(const AnyCameraT &)=delete
Disabled assign operator.
T TScalar
The scalar data type of this object.
Definition AnyCamera.h:134
virtual unsigned int height() const =0
Returns the height of the camera image.
virtual VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const =0
Projects a 3D object point into the camera frame.
virtual T fovY() const =0
Returns the field of view in x direction of the camera.
virtual T principalPointY() const =0
Returns the y-value of the principal point of the camera image in the pixel domain.
virtual void projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const VectorT3< T > *objectPoints, const size_t size, VectorT2< T > *imagePoints) const =0
Projects several 3D object points into the camera frame at once.
virtual bool isEqual(const AnyCameraT< T > &anyCamera, const T eps=NumericT< T >::eps()) const =0
Returns whether two camera objects are identical up to a given epsilon.
virtual VectorT3< T > vectorIF(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector=true) const =0
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
virtual bool isValid() const =0
Returns whether this camera is valid.
virtual LineT3< T > ray(const VectorT2< T > &distortedImagePoint, const HomogenousMatrixT4< T > &world_T_camera) const =0
Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
virtual VectorT2< T > projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint) const =0
Projects a 3D object point into the camera frame.
virtual AnyCameraType anyCameraType() const =0
Returns the type of this camera.
AnyCameraT()=default
Protected default constructor.
virtual void vector(const VectorT2< T > *distortedImagePoints, const size_t size, VectorT3< T > *vectors, const bool makeUnitVector=true) const =0
Determines vectors starting at the camera's center and intersecting given 2D points in the image.
virtual void pointJacobian2nx3IF(const VectorT3< T > *flippedCameraObjectPoints, const size_t numberObjectPoints, T *jacobians) const =0
Calculates the 2n x 3 jacobian matrix for the 3D object point projection into the camera frame.
virtual std::string name() const =0
Returns the name of this camera.
AnyCameraT(const AnyCameraT< T > &anyCamera)=default
Protected copy constructor.
static std::shared_ptr< AnyCameraT< T > > convert(const std::shared_ptr< AnyCameraT< U > > &anyCamera)
Converts an AnyCamera object with arbitrary scalar type to another AnyCamera object with arbitrary sc...
Definition AnyCamera.h:1892
virtual std::unique_ptr< AnyCameraT< float > > cloneToFloat(const unsigned int width=0u, const unsigned int height=0u) const =0
Returns a copy of this camera object with float precision.
virtual void projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > *objectPoints, const size_t size, VectorT2< T > *imagePoints) const =0
Projects several 3D object points into the camera frame at once.
virtual bool isInside(const VectorT2< T > &imagePoint, const T signedBorder=T(0)) const =0
Returns whether a given 2D image point lies inside the camera frame.
virtual T principalPointX() const =0
Returns the x-value of the principal point of the camera image in the pixel domain.
This class implements a specialized AnyCamera object wrapping the actual camera model.
Definition AnyCamera.h:597
VectorT2< T > principalPoint() const override
Returns the coordinate of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:1956
void pointJacobian2nx3IF(const VectorT3< T > *flippedCameraObjectPoints, const size_t numberObjectPoints, T *jacobians) const override
Calculates the 2n x 3 jacobian matrix for the 3D object point projection into the camera frame.
Definition AnyCamera.h:2106
VectorT2< T > projectToImage(const VectorT3< T > &objectPoint) const override
Projects a 3D object point into the camera frame.
Definition AnyCamera.h:2016
bool isEqual(const AnyCameraT< T > &anyCamera, const T eps=NumericT< T >::eps()) const override
Returns whether two camera objects are identical up to a given epsilon.
Definition AnyCamera.h:2112
unsigned int width() const override
Returns the width of the camera image.
Definition AnyCamera.h:1944
unsigned int height() const override
Returns the height of the camera image.
Definition AnyCamera.h:1950
std::unique_ptr< AnyCameraT< float > > cloneToFloat(const unsigned int width=0u, const unsigned int height=0u) const override
Returns a copy of this camera object with float precision.
Definition AnyCamera.h:1932
std::unique_ptr< AnyCameraT< T > > clone(const unsigned int width=0u, const unsigned int height=0u) const override
Returns a copy of this camera object.
Definition AnyCamera.h:1926
T focalLengthY() const override
Returns the vertical focal length parameter.
Definition AnyCamera.h:1980
bool isValid() const override
Returns whether this camera is valid.
Definition AnyCamera.h:2137
AnyCameraType anyCameraType() const override
Returns the type of this camera.
Definition AnyCamera.h:1914
T inverseFocalLengthX() const override
Returns the inverse horizontal focal length parameter.
Definition AnyCamera.h:1986
T principalPointY() const override
Returns the y-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:1968
AnyCameraWrappingT(ActualCamera &&actualCamera)
Creates a new AnyCamera object wrapping the actual camera model.
Definition AnyCamera.h:1900
VectorT3< T > vector(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector=true) const override
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2064
VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const override
Projects a 3D object point into the camera frame.
Definition AnyCamera.h:2028
T TScalar
The scalar data type of this object.
Definition AnyCamera.h:601
T focalLengthX() const override
Returns the horizontal focal length parameter.
Definition AnyCamera.h:1974
T fovX() const override
Returns the field of view in x direction of the camera.
Definition AnyCamera.h:1998
bool isInside(const VectorT2< T > &imagePoint, const T signedBorder=T(0)) const override
Returns whether a given 2D image point lies inside the camera frame.
Definition AnyCamera.h:2010
std::string name() const override
Returns the name of this camera.
Definition AnyCamera.h:1920
LineT3< T > ray(const VectorT2< T > &distortedImagePoint, const HomogenousMatrixT4< T > &world_T_camera) const override
Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2088
VectorT3< T > vectorIF(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector=true) const override
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2076
typename TCameraWrapper::ActualCamera ActualCamera
The actual camera object wrapped by this class.
Definition AnyCamera.h:607
T principalPointX() const override
Returns the x-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:1962
std::unique_ptr< AnyCameraT< double > > cloneToDouble(const unsigned int width=0u, const unsigned int height=0u) const override
Returns a copy of this camera object with double precision.
Definition AnyCamera.h:1938
void pointJacobian2x3IF(const VectorT3< T > &flippedCameraObjectPoint, T *jx, T *jy) const override
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Definition AnyCamera.h:2100
T fovY() const override
Returns the field of view in x direction of the camera.
Definition AnyCamera.h:2004
T inverseFocalLengthY() const override
Returns the inverse vertical focal length parameter.
Definition AnyCamera.h:1992
This class implements a helper class allowing to check whether a 3D object point projects into the ca...
Definition AnyCamera.h:516
const SharedAnyCameraT< T > & camera() const
Returns the camera model of this checker.
Definition AnyCamera.h:1736
static bool determineCameraBoundary(const AnyCameraT< T > &camera, FiniteLinesT2< T > &cameraBoundarySegments, const size_t segmentSteps)
Determines the camera boundary of a given camera model in normalized image coordinates.
Definition AnyCamera.h:1756
CameraProjectionCheckerT()=default
Default constructor creating an invalid object.
bool isValid() const
Returns whether this checker holds a valid camera model and is ready to be used.
Definition AnyCamera.h:1748
FiniteLinesT2< T > cameraBoundarySegments_
The 2D line segments defined in the camera's normalized image plane defining the camera's boundary.
Definition AnyCamera.h:583
const FiniteLinesT2< T > & cameraBoundarySegments() const
Returns the 2D line segments defined in the camera's normalized image plane defining the camera's bou...
Definition AnyCamera.h:1742
SharedAnyCameraT< T > camera_
The actual camera model this checker is based on.
Definition AnyCamera.h:580
static bool isInside(const FiniteLinesT2< T > &cameraBoundarySegments, const VectorT2< T > &imagePoint)
Returns whether a given normalized image point lies inside the camera's boundary.
Definition AnyCamera.h:1820
bool projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint, VectorT2< T > *imagePoint=nullptr) const
Returns whether a 3D object point is located in front of the camera and projects into the camera imag...
Definition AnyCamera.h:1703
This class implements the base class for all cameras.
Definition Camera.h:54
This class implements the base wrapper around Ocean's fisheye camera profile.
Definition AnyCamera.h:1206
T principalPointX() const
Returns the x-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2558
T inverseFocalLengthX() const
Returns the inverse horizontal focal length parameter.
Definition AnyCamera.h:2582
ActualCamera actualCamera_
The actual fisheye camera object.
Definition AnyCamera.h:1369
unsigned int height() const
Returns the height of the camera image.
Definition AnyCamera.h:2546
void pointJacobian2x3IF(const VectorT3< T > &flippedCameraObjectPoint, T *jx, T *jy) const
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Definition AnyCamera.h:2648
unsigned int width() const
Returns the width of the camera image.
Definition AnyCamera.h:2540
bool isValid() const
Returns whether this camera is valid.
Definition AnyCamera.h:2662
FisheyeCameraT< T > ActualCamera
Definition of the actual camera object wrapped by this class.
Definition AnyCamera.h:1212
T inverseFocalLengthY() const
Returns the inverse vertical focal length parameter.
Definition AnyCamera.h:2588
static AnyCameraType anyCameraType()
Returns the type of this camera.
Definition AnyCamera.h:2711
VectorT3< T > vectorIF(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector) const
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2630
const ActualCamera & actualCamera() const
Returns the actual camera object wrapped in this class.
Definition AnyCamera.h:2534
VectorT2< T > principalPoint() const
Returns the coordinate of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2552
T principalPointY() const
Returns the y-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2564
static std::string name()
Returns the name of this camera.
Definition AnyCamera.h:2717
T focalLengthX() const
Returns the horizontal focal length parameter.
Definition AnyCamera.h:2570
T focalLengthY() const
Returns the vertical focal length parameter.
Definition AnyCamera.h:2576
VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const
Projects a 3D object point into the camera frame.
Definition AnyCamera.h:2594
CameraWrapperBaseFisheyeT(ActualCamera &&actualCamera)
Creates a new CameraWrapperBaseFisheyeT object wrapping the actual camera model.
Definition AnyCamera.h:2520
std::unique_ptr< AnyCameraT< U > > clone(const unsigned int width=0u, const unsigned int height=0u) const
Returns a copy of the actual camera object.
Definition AnyCamera.h:2669
bool isEqual(const CameraWrapperBaseFisheyeT< T > &baseFisheye, const T eps=NumericT< T >::eps()) const
Returns whether two camera objects are identical up to a given epsilon.
Definition AnyCamera.h:2654
This class implements the base wrapper around an invalid camera profile.
Definition AnyCamera.h:1429
bool isEqual(const CameraWrapperBaseInvalidT< T > &baseInvalid, const T eps=NumericT< T >::eps()) const
Returns whether two camera objects are identical up to a given epsilon.
Definition AnyCamera.h:2900
InvalidCameraT< T > ActualCamera
Definition of the actual camera object wrapped by this class.
Definition AnyCamera.h:1435
VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const
Projects a 3D object point into the camera frame.
Definition AnyCamera.h:2837
CameraWrapperBaseInvalidT(ActualCamera &&actualCamera)
Creates a new CameraWrapperBaseInvalidT object wrapping the actual camera model.
Definition AnyCamera.h:2723
unsigned int height() const
Returns the height of the camera image.
Definition AnyCamera.h:2757
const ActualCamera & actualCamera() const
Returns the actual camera object wrapped in this class.
Definition AnyCamera.h:2737
static std::string name()
Returns the name of this camera.
Definition AnyCamera.h:2933
ActualCamera actualCamera_
The actual invalid camera.
Definition AnyCamera.h:1592
T principalPointX() const
Returns the x-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2777
VectorT2< T > principalPoint() const
Returns the coordinate of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2767
T inverseFocalLengthX() const
Returns the inverse horizontal focal length parameter.
Definition AnyCamera.h:2817
bool isValid() const
Returns whether this camera is valid.
Definition AnyCamera.h:2910
static AnyCameraType anyCameraType()
Returns the type of this camera.
Definition AnyCamera.h:2927
std::unique_ptr< AnyCameraT< U > > clone(const unsigned int width=0u, const unsigned int height=0u) const
Returns a copy of the actual camera object.
Definition AnyCamera.h:2917
T inverseFocalLengthY() const
Returns the inverse vertical focal length parameter.
Definition AnyCamera.h:2827
void pointJacobian2x3IF(const VectorT3< T > &flippedCameraObjectPoint, T *jx, T *jy) const
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Definition AnyCamera.h:2892
VectorT3< T > vectorIF(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector) const
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2874
T focalLengthY() const
Returns the vertical focal length parameter.
Definition AnyCamera.h:2807
T principalPointY() const
Returns the y-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2787
T focalLengthX() const
Returns the horizontal focal length parameter.
Definition AnyCamera.h:2797
unsigned int width() const
Returns the width of the camera image.
Definition AnyCamera.h:2747
This class implements the base wrapper around Ocean's pinhole camera profile.
Definition AnyCamera.h:1037
T inverseFocalLengthX() const
Returns the inverse horizontal focal length parameter.
Definition AnyCamera.h:2373
void pointJacobian2x3IF(const VectorT3< T > &flippedCameraObjectPoint, T *jx, T *jy) const
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Definition AnyCamera.h:2441
PinholeCameraT< T > ActualCamera
Definition of the actual camera object wrapped by this class.
Definition AnyCamera.h:1043
unsigned int height() const
Returns the height of the camera image.
Definition AnyCamera.h:2343
bool isValid() const
Returns whether this camera is valid.
Definition AnyCamera.h:2456
T principalPointY() const
Returns the y-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2355
T inverseFocalLengthY() const
Returns the inverse vertical focal length parameter.
Definition AnyCamera.h:2379
static std::string name()
Returns the name of this camera.
Definition AnyCamera.h:2514
ActualCamera actualCamera_
The actual pinhole camera.
Definition AnyCamera.h:1195
const ActualCamera & actualCamera() const
Returns the actual camera object wrapped in this class.
Definition AnyCamera.h:2331
unsigned int width() const
Returns the width of the camera image.
Definition AnyCamera.h:2337
CameraWrapperBasePinholeT(ActualCamera &&actualCamera)
Creates a new CameraWrapperBasePinholeT object wrapping the actual camera model.
Definition AnyCamera.h:2317
VectorT3< T > vectorIF(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector) const
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2421
VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const
Projects a 3D object point into the camera frame.
Definition AnyCamera.h:2385
static AnyCameraType anyCameraType()
Returns the type of this camera.
Definition AnyCamera.h:2508
T principalPointX() const
Returns the x-value of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2349
bool isEqual(const CameraWrapperBasePinholeT< T > &basePinhole, const T eps=NumericT< T >::eps()) const
Returns whether two camera objects are identical up to a given epsilon.
Definition AnyCamera.h:2448
std::unique_ptr< AnyCameraT< U > > clone(const unsigned int width=0u, const unsigned int height=0u) const
Returns a copy of the actual camera object.
Definition AnyCamera.h:2463
T focalLengthY() const
Returns the vertical focal length parameter.
Definition AnyCamera.h:2367
T focalLengthX() const
Returns the horizontal focal length parameter.
Definition AnyCamera.h:2361
This class implements a wrapper for an actual camera object.
Definition AnyCamera.h:928
LineT3< T > ray(const VectorT2< T > &distortedImagePoint, const HomogenousMatrixT4< T > &world_T_camera) const
Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2287
T fovY() const
Returns the field of view in x direction of the camera.
Definition AnyCamera.h:2192
T fovX() const
Returns the field of view in x direction of the camera.
Definition AnyCamera.h:2163
CameraWrapperT(ActualCamera &&actualCamera)
Creates a new CameraWrapperT object wrapping the actual camera model.
Definition AnyCamera.h:2143
VectorT3< T > vector(const VectorT2< T > &distortedImagePoint, const bool makeUnitVector) const
Returns a vector starting at the camera's center and intersecting a given 2D point in the image.
Definition AnyCamera.h:2266
void pointJacobian2nx3IF(const VectorT3< T > *flippedCameraObjectPoints, const size_t numberObjectPoints, T *jacobians) const
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Definition AnyCamera.h:2303
bool isInside(const VectorT2< T > &imagePoint, const T signedBorder=T(0)) const
Returns whether a given 2D image point lies inside the camera frame.
Definition AnyCamera.h:2221
VectorT2< T > principalPoint() const
Returns the coordinate of the principal point of the camera image in the pixel domain.
Definition AnyCamera.h:2157
VectorT2< T > projectToImage(const VectorT3< T > &objectPoint) const
Projects a 3D object point into the camera frame.
Definition AnyCamera.h:2235
This class implements an finite line in 2D space.
Definition FiniteLine2.h:82
Class representing a fisheye camera.
Definition FisheyeCamera.h:106
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
SquareMatrixT3< T > rotationMatrix() const
Returns the rotation matrix of the transformation.
Definition HomogenousMatrix4.h:1493
VectorT3< T > translation() const
Returns the translation of the transformation.
Definition HomogenousMatrix4.h:1381
bool isValid() const
Returns whether this matrix is a valid homogeneous transformation.
Definition HomogenousMatrix4.h:1806
This class implements invalid camera profiles, e.g.
Definition AnyCamera.h:1379
const std::string & reason() const
Returns the reason of this invalid camera.
Definition AnyCamera.h:2946
std::string reason_
The reason why no valid camera is available.
Definition AnyCamera.h:1397
InvalidCameraT(const std::string &reason)
Creates an invalid camera.
Definition AnyCamera.h:2939
This class implements an infinite line in 3D space.
Definition Line3.h:68
static MessageObject error()
Returns the message for error messages.
Definition Messenger.h:1074
This class provides basic numeric functionalities.
Definition Numeric.h:57
static constexpr T minValue()
Returns the min scalar value.
Definition Numeric.h:3253
static T atan(const T value)
Returns the arctangent of a given value.
Definition Numeric.h:1616
static T abs(const T value)
Returns the absolute value of a given value.
Definition Numeric.h:1220
static bool isEqual(const T first, const T second)
Returns whether two values are equal up to a small epsilon.
Definition Numeric.h:2389
Definition of a pinhole camera model.
Definition PinholeCamera.h:114
This class implements a vector with two elements.
Definition Vector2.h:96
const T & x() const noexcept
Returns the x value.
Definition Vector2.h:710
const T & y() const noexcept
Returns the y value.
Definition Vector2.h:722
This class implements a vector with three elements.
Definition Vector3.h:97
const T & y() const noexcept
Returns the y value.
Definition Vector3.h:824
const T & x() const noexcept
Returns the x value.
Definition Vector3.h:812
const T & z() const noexcept
Returns the z value.
Definition Vector3.h:836
std::shared_ptr< AnyCameraD > SharedAnyCameraD
Definition of a shared pointer holding an AnyCamera object with double precision.
Definition AnyCamera.h:67
std::shared_ptr< AnyCamera > SharedAnyCamera
Definition of a shared pointer holding an AnyCamera object with Scalar precision.
Definition AnyCamera.h:60
std::vector< FiniteLineT2< T > > FiniteLinesT2
Definition of a typename alias for vectors with FiniteLineT2 objects.
Definition FiniteLine2.h:50
std::vector< VectorT2< T > > VectorsT2
Definition of a typename alias for vectors with VectorT2 objects.
Definition Vector2.h:57
SharedAnyCamerasT< float > SharedAnyCamerasF
Definition of a vector holding AnyCameraF objects.
Definition AnyCamera.h:104
SharedAnyCamerasT< double > SharedAnyCamerasD
Definition of a vector holding AnyCameraD objects.
Definition AnyCamera.h:97
AnyCameraType
Definition of individual camera types.
Definition AnyCamera.h:111
std::shared_ptr< AnyCameraF > SharedAnyCameraF
Definition of a shared pointer holding an AnyCamera object with float precision.
Definition AnyCamera.h:74
std::vector< std::shared_ptr< AnyCameraT< T > > > SharedAnyCamerasT
Definition of a typename alias for vectors with shared AnyCameraT objects.
Definition AnyCamera.h:83
SharedAnyCamerasT< Scalar > SharedAnyCameras
Definition of a vector holding AnyCamera objects.
Definition AnyCamera.h:90
std::shared_ptr< AnyCameraT< T > > SharedAnyCameraT
Definition of a shared pointer holding an AnyCamera object with Scalar precision.
Definition AnyCamera.h:53
@ FISHEYE
A fisheye camera.
@ PINHOLE
A pinhole camera.
@ INVALID
An invalid camera type.
The namespace covering the entire Ocean framework.
Definition Accessor.h:15