Ocean
Loading...
Searching...
No Matches
PinholeCamera.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_PINHOLE_CAMERA_H
9#define META_OCEAN_MATH_PINHOLE_CAMERA_H
10
11#include "ocean/math/Math.h"
12#include "ocean/math/Box2.h"
13#include "ocean/math/Box3.h"
14#include "ocean/math/Camera.h"
15#include "ocean/math/HomogenousMatrix4.h"
16#include "ocean/math/Line2.h"
17#include "ocean/math/Line3.h"
18#include "ocean/math/Lookup2.h"
19#include "ocean/math/Quaternion.h"
20#include "ocean/math/Rotation.h"
21#include "ocean/math/SquareMatrix3.h"
22#include "ocean/math/SquareMatrix4.h"
23#include "ocean/math/Triangle2.h"
24#include "ocean/math/Triangle3.h"
25#include "ocean/math/Vector2.h"
26#include "ocean/math/Vector3.h"
27
28namespace Ocean
29{
30
31// Forward declaration.
32template <typename T> class PinholeCameraT;
33
34/**
35 * Definition of an pinhole camera object with Scalar precision.
36 * @see PinholeCameraT
37 * @ingroup math
38 */
39typedef PinholeCameraT<Scalar> PinholeCamera;
40
41/**
42 * Definition of an pinhole camera object with double precision.
43 * @see PinholeCameraT
44 * @ingroup math
45 */
46typedef PinholeCameraT<double> PinholeCameraD;
47
48/**
49 * Definition of an pinhole camera object with float precision.
50 * @see PinholeCameraT
51 * @ingroup math
52 */
53typedef PinholeCameraT<float> PinholeCameraF;
54
55/**
56 * Definition of a typename alias for vectors with PinholeCameraT objects.
57 * @see PinholeCameraT
58 * @ingroup math
59 */
60template <typename T>
61using PinholeCamerasT = std::vector<PinholeCameraT<T>>;
62
63/**
64 * Definition of a vector holding pinhole camera objects.
65 * @see PinholeCamera
66 * @ingroup math
67 */
68typedef PinholeCamerasT<Scalar> PinholeCameras;
69
70/**
71 * Definition of a vector holding PinholeCameraD objects.
72 * @see PinholeCameraD
73 * @ingroup math
74 */
75typedef PinholeCamerasT<double> PinholeCamerasD;
76
77/**
78 * Definition of a vector holding PinholeCameraF objects.
79 * @see PinholeCameraF
80 * @ingroup math
81 */
82typedef PinholeCamerasT<float> PinholeCamerasF;
83
84/**
85 * Definition of a pinhole camera model.<br>
86 * The class holds the intrinsic and distortion parameters of a camera.<br>
87 * <pre>
88 * The camera holds:
89 * 1) Width and height of the camera image.
90 * 2) Intrinsic camera matrix:
91 * | Fx 0 mx |
92 * | 0 Fy my |
93 * | 0 0 1 |
94 * with mx and my as principal point,
95 * and with Fx = f / sx, Fy = f / sy, with focus f and pixel sizes sx and sy.
96 * 3) Two radial distortion parameters k1, and k2 for r^2 and r^4:
97 * 4) Two tangential distortion parameters p1 and p2.
98 * An distortion free (undistorted) image point is transformed to the corresponding distorted image position as follows:
99 * x' = x + x * (k1 * r^2 + k2 * r^4) + p1 * 2 * x * y + p2 * (r^2 + 2 * x^2),
100 * y' = y + y * (k1 * r^2 + k2 * r^4) + p2 * 2 * x * y + p1 * (r^2 + 2 * y^2).
101 * With: Undistorted and normalized image coordinates (x, y) and distorted and normalized image coordinates (x', y'),
102 * r^2 is defined by: r^2 = (x^2 + y^2), while r^4 = (x^2 + y^2)^2.
103 * The normalized image coordinate is obtained from a 3D object point by application of the perspective division.
104 * Normalized image coordinates may be transformed into the pixel coordinate system by applying the focal length and the principal point.
105 * </pre>
106 * This class provides several distort, undistort or projection functions of image points or object points.<br>
107 * Points lying outside the camera frame, may be distorted or undistorted due to the given position or due to the nearest position lying on the camera frame's border.<br>
108 * The larger the distance to the principal point of the camera, the more irregular the un-/distortion of the distortion model of this camera.<br>
109 * @tparam T The data type of a scalar, 'float' or 'double'
110 * @ingroup math
111 */
112template <typename T>
113class OCEAN_MATH_EXPORT PinholeCameraT : public CameraT<T>
114{
115 template <typename U> friend class PinholeCameraT;
116
117 public:
118
119 /// The scalar data type of this object.
120 typedef T TScalar;
121
122 /// Definition of a pair of distortion values.
123 typedef std::pair<T, T> DistortionPair;
124
125 /**
126 * Definition of individual optimization strategies for camera parameters.
127 */
128 enum OptimizationStrategy
129 {
130 /// No optimization.
131 OS_NONE = 0,
132 /// Optimization of one focal length parameter (the same/identical parameter for horizontal and vertical focal length).
133 OS_FOCAL_LENGTH = 1,
134 /// Optimization of two focal length parameters: Horizontal focal length and vertical focal length.
135 OS_FOCAL_LENGTHS = 2,
136 /// Optimization of the four (basic) intrinsic camera parameters: Horizontal focal length, vertical focal length, horizontal principal point, vertical principal point.
137 OS_INTRINSIC_PARAMETERS = 4,
138 /// Optimization of six parameters: Horizontal focal length, vertical focal length, two radial distortion parameters, two tangential distortion parameters.
139 OS_FOCAL_LENGTHS_DISTORTION = 6,
140 /// Optimization of all camera parameters while the horizontal and vertical focal length parameter is identical.
141 OS_SYMMETRIC_INTRINSIC_PARAMETERS_DISTORTIONS = 7,
142 /// Optimization of all 8 intrinsic camera parameters including the distortion parameters: (2x focal length, 2x principal point, 2x radial distortion, 2x tangential distortion).
143 OS_INTRINSIC_PARAMETERS_DISTORTIONS = 8,
144 /// Optimization of four distortion parameters: Two radial distortion parameters, two tangential distortion parameters.
145 OS_DISTORTION = 0x1000 | 4,
146 /// Optimization of four (basic) intrinsic camera parameters: Horizontal focal length, vertical focal length, horizontal principal point, vertical principal point and two radial distortion parameters.
147 OS_INTRINSIC_PARAMETERS_RADIAL_DISTORTION = 0x1000 | 6
148 };
149
150 /**
151 * This class encapsulates a lookup table for camera distortion offsets allowing for faster un-distortion of image points (which is than an approximated position only).
152 */
153 class OCEAN_MATH_EXPORT DistortionLookup
154 {
155 protected:
156
157 /**
158 * Definition of a lookup table for 2D vectors.
159 */
160 typedef LookupCenter2<VectorT2<T>, T> LookupTable;
161
162 public:
163
164 /**
165 * Creates an invalid lookup object.
166 */
167 inline DistortionLookup();
168
169 /**
170 * Creates an lookup object for a given camera.
171 * @param camera The camera profile for that the lookup table is determined
172 * @param binSize The size of the lookup bins for the camera distortion, horizontal and vertical in pixel, with range [1u, min(camera.width(), camera.height())]
173 */
174 explicit DistortionLookup(const PinholeCameraT<T>& camera, const unsigned int binSize);
175
176 /**
177 * Returns the undistorted image point for a given (distorted) image point (by application of a bilinear interpolation).
178 * @param distortedImagePoint The distorted image point for that the undistorted image point is returned
179 * @return The approximation of the undistorted image point
180 */
181 inline VectorT2<T> undistortedImagePoint(const VectorT2<T>& distortedImagePoint) const;
182
183 /**
184 * Returns the offset that needs to be added to an distorted image point so that it would be undistorted (by application of a bilinear interpolation).
185 * @param distortedImagePoint The distorted image point for that the offset is returned
186 * @return The approximation of the offset
187 */
188 inline VectorT2<T> undistortionOffset(const VectorT2<T>& distortedImagePoint) const;
189
190 /**
191 * Returns the undistorted image point for a given (distorted) image point (by application of a bicubic interpolation).
192 * @param distortedImagePoint The distorted image point for that the undistorted image point is returned
193 * @return The approximation of the undistorted image point
194 */
195 inline VectorT2<T> undistortedImagePointBicubic(const VectorT2<T>& distortedImagePoint) const;
196
197 /**
198 * Returns the offset that needs to be added to an distorted image point so that it would be undistorted (by application of a bicubic interpolation).
199 * @param distortedImagePoint The distorted image point for that the offset is returned
200 * @return The approximation of the offset
201 */
202 inline VectorT2<T> undistortionOffsetBicubic(const VectorT2<T>& distortedImagePoint) const;
203
204 protected:
205
206 /// The distortion lookup table.
207 LookupTable distortionLookupTable;
208 };
209
210 public:
211
212 /**
213 * Standard constructor.
214 * Creates a new PinholeCameraT<T> object with all internal parameters as zero.
215 * The resulting camera object is invalid.<br>
216 */
217 PinholeCameraT() = default;
218
219 /**
220 * Creates a new camera object with specified frame dimension and intrinsic camera parameters best matching to a given reference camera profile with different frame dimension.
221 * Beware: The dimension aspect ratio between the new camera and the given reference camera profile should be almost similar.
222 * @param width The width of the camera dimension in pixel, with range [1, infinity)
223 * @param height The height of the camera dimension in pixel, with range [1, infinity)
224 * @param camera The camera profile that is used to adopt the intrinsic (and distortion) parameters
225 */
226 PinholeCameraT(const unsigned int width, const unsigned int height, const PinholeCameraT<T>& camera);
227
228 /**
229 * Creates a new sub-frame camera profile based on a camera profile of the entire camera frame.
230 * @param subFrameLeft The horizontal start position of the sub-frame within the original camera frame, in pixel, with range (-infinity, infinity)
231 * @param subFrameTop The vertical start position of the sub-frame within the original camera frame, in pixel, with range (-infinity, infinity)
232 * @param subFrameWidth The width of the sub-frame in pixel, with range [1, infinity)
233 * @param subFrameHeight The height of the sub-frame in pixel, with range [1, infinity)
234 * @param camera The original camera profile for which a sub-frame camera profile will be created
235 */
236 PinholeCameraT(const T subFrameLeft, const T subFrameTop, const unsigned int subFrameWidth, const unsigned int subFrameHeight, const PinholeCameraT<T>& camera);
237
238 /**
239 * Creates a new PinholeCameraT<T> object by it's given intrinsic parameters.
240 * @param width The width of the camera image, in pixel, with range [1, infinity)
241 * @param height The height of the camera image, in pixel, with range [1, infinity)
242 * @param focalX The focal parameter of the horizontal axis
243 * @param focalY The focal parameter of the vertical axis
244 * @param principalX The principal point of the horizontal axis (in pixel)
245 * @param principalY The principal point of the vertical axis (in pixel)
246 */
247 PinholeCameraT(const unsigned int width, const unsigned int height, const T focalX, const T focalY, const T principalX, const T principalY);
248
249 /**
250 * Creates a new PinholeCameraT<T> object by it's given intrinsic parameters.
251 * @param width The width of the camera image, in pixel, with range [1, infinity)
252 * @param height The height of the camera image, in pixel, with range [1, infinity)
253 * @param focalX The focal parameter of the horizontal axis
254 * @param focalY The focal parameter of the vertical axis
255 * @param principalX The principal point of the horizontal axis (in pixel)
256 * @param principalY The principal point of the vertical axis (in pixel)
257 * @param radial The pair of radial distortion parameter for r^2 and r^4
258 * @param tangential The tangential distortion parameters
259 */
260 PinholeCameraT(const unsigned int width, const unsigned int height, const T focalX, const T focalY, const T principalX, const T principalY, const DistortionPair& radial, const DistortionPair& tangential);
261
262 /**
263 * Creates a new PinholeCameraT<T> object by it's given intrinsic parameters.
264 * @param width The width of the camera image, in pixel, with range [1, infinity)
265 * @param height The height of the camera image, in pixel, with range [1, infinity)
266 * @param parameters The four to eight intrinsic and distortion parameters of the camera focalLegnthX(), focalLengthY(), principalPointX(), principalPointY(), radialDistortion().first, radialDistortion().second, tangentialDistortion().first, tangentialDistortion().second
267 * @param radialDistortion True, if parameter[4] and parameter[5] exist and store radial distortion parameters, must be 'True' if tangentialDistortion is 'True'
268 * @param tangentialDistortion True, if parameter[6] and parameter[7] exist and store tangential distortion parameters
269 */
270 PinholeCameraT(const unsigned int width, const unsigned int height, const T* parameters, const bool radialDistortion = true, const bool tangentialDistortion = true);
271
272 /**
273 * Creates a new PinholeCameraT<T> object by a given projection matrix with the intrinsic camera parameters.
274 * @param intrinsic The matrix with intrinsic camera parameter.
275 */
276 explicit PinholeCameraT(const SquareMatrixT3<T>& intrinsic);
277
278 /**
279 * Creates a new PinholeCameraT<T> object by the given intrinsic camera matrix and width and height of the camera.
280 * @param intrinsic The intrinsic camera matrix
281 * @param width The width of the camera image, in pixel, with range [1, infinity)
282 * @param height The height of the camera image, in pixel, with range [1, infinity)
283 */
284 PinholeCameraT(const SquareMatrixT3<T>& intrinsic, const unsigned int width, const unsigned int height);
285
286 /**
287 * Creates a new PinholeCameraT<T> object by the given intrinsic camera matrix, the width and height and the radial distortion parameters.
288 * @param intrinsic The intrinsic camera matrix
289 * @param width The width of the camera image, in pixel, with range [1, infinity)
290 * @param height The height of the camera image, in pixel, with range [1, infinity)
291 * @param radial The radial distortion parameters (r^2 and r^4)
292 * @param tangential The tangential distortion parameters
293 */
294 PinholeCameraT(const SquareMatrixT3<T>& intrinsic, const unsigned int width, const unsigned int height, const DistortionPair& radial, const DistortionPair& tangential);
295
296 /**
297 * Creates a new PinholeCameraT<T> object by the given width, height and field of view of a camera.
298 * This camera has no radial distortion.
299 * @param width The width of the camera image, in pixel, with range [1, infinity)
300 * @param height The height of the camera image, in pixel, with range [1, infinity)
301 * @param fovX The field of view in x-direction, in radian, with range (0, PI)
302 */
303 PinholeCameraT(const unsigned int width, const unsigned int height, const T fovX);
304
305 /**
306 * Creates a new PinholeCameraT<T> object by the given frame dimensions, the ideal field of view, and the camera's principal point.
307 * The provided field of view is expected to be the ideal field of view (if the camera would have a principal point in the perfect center of the image).<br>
308 * This camera has no radial distortion.
309 * @param width The width of the camera image, in pixel, with range [1, infinity)
310 * @param height The height of the camera image, in pixel, with range [1, infinity)
311 * @param fovX The camera's ideal field of view in x-direction, in radian, with range (0, PI)
312 * @param principalX The horizontal principal point within the camera frame, in pixel, with range (-infinity, infinity)
313 * @param principalY The vertical principal point within the camera frame, in pixel, with range (-infinity, infinity)
314 */
315 PinholeCameraT(const unsigned int width, const unsigned int height, const T fovX, const T principalX, const T principalY);
316
317 /**
318 * Copy constructor for a pinhole camera with difference element data type than T.
319 * @param pinholeCamera The pinhole camera profile to be copied
320 * @param copyDistortionParameters True, to copy the distortion parameters; False, create a new pinhole camera without distortion parameters
321 * @tparam U The element data type of the given pinhole camera
322 */
323 template <typename U>
324 explicit inline PinholeCameraT(const PinholeCameraT<U>& pinholeCamera, const bool copyDistortionParameters = true);
325
326 /**
327 * Returns the intrinsic camera matrix.
328 * @return The intrinsic camera matrix
329 */
330 inline const SquareMatrixT3<T>& intrinsic() const;
331
332 /**
333 * Returns the inverted intrinsic camera matrix.
334 * @return The inverted intrinsic camera matrix
335 */
336 inline const SquareMatrixT3<T>& invertedIntrinsic() const;
337
338 /**
339 * Returns the pair of radial distortion parameters.
340 * @return The radial distortion parameters for r^2 and r^4
341 */
342 inline const DistortionPair& radialDistortion() const;
343
344 /**
345 * Returns the pair of tangential distortion parameters.
346 * @return The tangential distortion parameters
347 */
348 inline const DistortionPair& tangentialDistortion() const;
349
350 /**
351 * Returns whether this camera object has specified distortion parameters.
352 * @return True, if so
353 */
354 inline bool hasDistortionParameters() const;
355
356 /**
357 * Returns the width of the camera image.
358 * @return The width of the camera image
359 */
360 inline unsigned int width() const;
361
362 /**
363 * Returns the height of the camera image.
364 * @return The height of the camera image
365 */
366 inline unsigned int height() const;
367
368 /**
369 * Returns the coordinate of the principal point of the camera image in the pixel domain.
370 * @return The 2D location of the principal point, with range [0, width)x[0, height)
371 */
372 inline VectorT2<T> principalPoint() const;
373
374 /**
375 * Returns the x-value of the principal point of the camera image in the pixel domain.
376 * @return The x-value of the principal point, with range [0, width)
377 */
378 inline T principalPointX() const;
379
380 /**
381 * Returns the y-value of the principal point of the camera image in the pixel domain.
382 * @return The y-value of the principal point, with range [0, height)
383 */
384 inline T principalPointY() const;
385
386 /**
387 * Returns the horizontal focal length parameter.
388 * @return The horizontal focal length parameter
389 */
390 inline T focalLengthX() const;
391
392 /**
393 * Returns the vertical focal length parameter.
394 * @return The vertical focal length parameter
395 */
396 inline T focalLengthY() const;
397
398 /**
399 * Returns the inverse horizontal focal length parameter.
400 * @return The inverse horizontal focal length parameter
401 */
402 inline T inverseFocalLengthX() const;
403
404 /**
405 * Returns the inverse vertical focal length parameter.
406 * @return The inverse vertical focal length parameter
407 */
408 inline T inverseFocalLengthY() const;
409
410 /**
411 * Returns the field of view in x direction of the camera.
412 * The fov is the sum of the left and right part of the camera.
413 * @return The field of view (in radian), with range (0, PI)
414 */
415 T fovX() const;
416
417 /**
418 * Returns the field of view in x direction of the camera.
419 * The fov is the sum of the top and bottom part of the camera.
420 * @return The field of view (in radian), with range (0, PI)
421 */
422 T fovY() const;
423
424 /**
425 * Returns the left field of view in x direction.
426 * @return The left field of view (in radian), with range (-PI, PI), negative if principal point is outside image region
427 */
428 T fovXLeft() const;
429
430 /**
431 * Returns the right field of view in x direction.
432 * @return The right field of view (in radian), with range (-PI, PI), negative if principal point is outside image region
433 */
434 T fovXRight() const;
435
436 /**
437 * Returns the top field of view in y direction.
438 * @return The top field of view (in radian), with range (-PI, PI), negative if principal point is outside image region
439 */
440 T fovYTop() const;
441
442 /**
443 * Returns the bottom field of view in y direction.
444 * @return The bottom field of view (in radian), with range (-PI, PI), negative if principal point is outside image region
445 */
446 T fovYBottom() const;
447
448 /**
449 * Returns the diagonal field of view of the camera
450 * @return The diagonal field of view (in radian), with range (0, PI)
451 */
452 T fovDiagonal() const;
453
454 /**
455 * Gets two rotation parameters of the viewing ray for a given undistorted 2D position in the camera image.
456 * @param undistortedPosition The undistorted 2D position [in pixel]
457 * @param angleX The horizontal angle for the viewing ray [in radian]
458 * @param angleY The vertical angle for the viewing ray [in radian]
459 * @return True, if succeeded
460 */
461 bool rotation(const VectorT2<T>& undistortedPosition, T& angleX, T& angleY) const;
462
463 /**
464 * Copies the elements of this camera to an array with 4 to 8 floating point values.
465 * The resulting values are stored in the following order: focalLegnthX(), focalLengthY(), principalPointX(), principalPointY(), radialDistortion().first, radialDistortion().second, tangentialDistortion().first, tangentialDistortion().second.
466 * @param arrayValues The array with 4 to 8 floating point values receiving the camera data
467 * @param copyRadialDistortion True, if the array holds at least 6 elements so that the radial parameters will be copied too, must be 'True' if tangentialDistortion is 'True'
468 * @param copyTangentialDistortion True, if the array holds at least 8 elements so that the tangential parameters will be copied too
469 */
470 void copyElements(T* arrayValues, const bool copyRadialDistortion = true, const bool copyTangentialDistortion = true) const;
471
472 /**
473 * Sets the intrinsic camera matrix.
474 * @param intrinsic The intrinsic camera matrix
475 * @return True, if the camera matrix is valid
476 */
477 bool setIntrinsic(const SquareMatrixT3<T>& intrinsic);
478
479 /**
480 * Sets the radial distortion parameters.
481 * @param radial The pair of radial distortion parameters for r^2 and r^4
482 */
483 inline void setRadialDistortion(const DistortionPair& radial);
484
485 /**
486 * Sets the tangential distortion parameters.
487 * @param tangential The pair of tangential distortion parameters
488 */
489 inline void setTangentialDistortion(const DistortionPair& tangential);
490
491 /**
492 * Applies a given (relative) zoom factor which mainly multiplies the focal length parameters by the given factor.
493 * @param relativeZoom The (realtive) zoom factor to apply, with range (0, infinity)
494 */
495 void applyZoomFactor(const T relativeZoom);
496
497 /**
498 * Checks whether the distortion of this camera is plausible.
499 * The check is based on two individual criteria.
500 * First, we check whether we can distort and un-distort the corner points of the camera frame to almost the same position in the camera frame.<br>
501 * Second we check whether the distortion is almost symmetric by checking that the distances between the distorted corner points of the camera frame and the (virtual) center of the camera frame are almost identical.
502 * @param symmetricFocalLengthRatio The symmetric tolerance ratio between the horizontal and vertical focal length values of the camera, with range (1, infinity)
503 * @param modelAccuracy The accuracy of the distortion model function, measuring the maximal offset between the distorted position of an undistorted position, in percent of the frame size, with range (0, 1)
504 * @param symmetricDistortionRatio The symmetric tolerance ratio between the camera frame's center and the distorted corner positions in percent, with range (1, infinity)
505 * @return True, if so
506 */
507 bool isDistortionPlausible(const T symmetricFocalLengthRatio = T(1.05), const T modelAccuracy = T(0.001), const T symmetricDistortionRatio = T(1.08)) const;
508
509 /**
510 * Returns the undistorted position of a given distorted position defined in pixel coordinates.<br>
511 * Beware: As the camera distortion model can not be inverted numerically, there is no guarantee that a given point can be undistorted for any given camera profile.
512 * @param distorted The distorted pixel position to be undistorted
513 * @param iterations The number of iterative calculations, with range [1, 100]
514 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
515 * @return The resulting undistorted pixel position
516 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
517 * @see distort().
518 */
519 template <bool tUseBorderDistortionIfOutside>
520 VectorT2<T> undistort(const VectorT2<T>& distorted, const unsigned int iterations = 10u, const T zoom = T(1)) const;
521
522 /**
523 * Returns the undistorted position of a given distorted position defined in pixel coordinates.<br>
524 * Beware: As the camera distortion model can not be inverted numerically, there is no guarantee that a given point can be undistorted for any given camera profile.
525 * This function applies a damping for the distortion outside the camera frame so that the quadratic and quartic radii do not have such a significant impact.
526 * @param distorted The distorted pixel position to be undistorted
527 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
528 * @param iterations The number of iterative calculations, with range [1, 100]
529 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
530 * @return The resulting undistorted pixel position
531 * @see distortDamped().
532 */
533 VectorT2<T> undistortDamped(const VectorT2<T>& distorted, const T dampingFactor = T(1), const unsigned int iterations = 10u, const T zoom = T(1)) const;
534
535 /**
536 * Returns the distorted position of a given undistorted position defined in pixel coordinates.<br>
537 * The distorted position is calculated by the usage of the internal distortion parameters of this camera object:
538 * <pre>
539 * dx = x + x * (k1 * r^2 + k2 * r^4) + p1 * 2 * x * y + p2 * (r^2 + 2 * x^2),
540 * dy = y + y * (k1 * r^2 + k2 * r^4) + p2 * 2 * x * y + p1 * (r^2 + 2 * y^2).
541 * </pre>
542 * With (dx, dy) the distorted normalized point coordinates, (x, y) the undistorted normalized point coordinates,<br>
543 * and dcx = Fx * dx + mx, dcy = Fy * dy + my the distorted pixel coordinates.
544 * @param undistorted The undistorted position to be distorted in pixel coordinates
545 * @return The resulting distorted position in pixel coordinates
546 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
547 * @see undistort().
548 */
549 template <bool tUseBorderDistortionIfOutside>
550 VectorT2<T> distort(const VectorT2<T>& undistorted) const;
551
552 /**
553 * Returns the distorted position of a given undistorted position defined in pixel coordinates.<br>
554 * This function applies a damping for the distortion outside the camera frame so that the quadratic and quartic radii do not have such a significant impact.<br>
555 * The damping has an asymptotic pattern and so that distortion is based on normalized coordinates never reaching a specified boundary.<br>
556 * The distorted position is calculated by the usage of the internal distortion parameters of this camera object:
557 * <pre>
558 * dx = x + x * (k1 * r^2 + k2 * r^4) + p1 * 2 * x * y + p2 * (r^2 + 2 * x^2),
559 * dy = y + y * (k1 * r^2 + k2 * r^4) + p2 * 2 * x * y + p1 * (r^2 + 2 * y^2).
560 * </pre>
561 * With (dx, dy) the distorted normalized point coordinates, (x, y) the undistorted normalized point coordinates,<br>
562 * and dcx = Fx * dx + mx, dcy = Fy * dy + my the distorted pixel coordinates.
563 * @param undistorted The undistorted position to be distorted in pixel coordinates
564 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
565 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
566 * @return The resulting distorted position in pixel coordinates
567 * @see undistortDamped().
568 */
569 VectorT2<T> distortDamped(const VectorT2<T>& undistorted, const T dampingFactor = T(1), const T zoom = T(1)) const;
570
571 /**
572 * Returns whether a given 2D image point lies inside the camera frame.
573 * Optional an explicit border can be defined to allow points slightly outside the camera image, or further inside the image.<br>
574 * 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.
575 * @param imagePoint The image point to be checked, must be valid
576 * @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)
577 * @return True, if the image point lies in the ranges [0, width())x[0, height())
578 */
579 inline bool isInside(const VectorT2<T>& imagePoint, const T signedBorder = T(0)) const;
580
581 /**
582 * Returns the viewing angle between two undistorted points on the camera's image plane.
583 * @param first The first undistorted point
584 * @param second The second undistorted point
585 * @return The viewing angle between the defined points (in radian)
586 * @see calculateCosBetween().
587 */
588 T calculateAngleBetween(const VectorT2<T>& first, const VectorT2<T>& second) const;
589
590 /**
591 * Returns the cosine of the viewing angle between two undistorted points on the camera's image plane.
592 * @param first The first undistorted point
593 * @param second The second undistorted point
594 * @return The cosine of viewing angle between the defined points
595 * @see calculateAngleBetween().
596 */
597 T calculateCosBetween(const VectorT2<T>& first, const VectorT2<T>& second) const;
598
599 /**
600 * Calculates the normalized image point corresponding to a given (distorted) image point.
601 * @param imagePoint The image point (that might be distorted)
602 * @param undistortImagePoint True, to force the un-distortion of the image point using the distortion parameters of this camera object
603 * @return The resulting normalized and (optional undistorted) image point
604 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
605 */
606 template <bool tUseBorderDistortionIfOutside>
607 VectorT2<T> imagePoint2normalizedImagePoint(const VectorT2<T>& imagePoint, const bool undistortImagePoint) const;
608
609 /**
610 * Calculates the normalized image point corresponding to a given (distorted) image point.
611 * This function applies a damping for the distortion outside the camera frame so that the quadratic and quartic radii do not have such a significant impact.
612 * @param imagePoint The image point (that might be distorted)
613 * @param undistortImagePoint True, to force the un-distortion of the image point using the distortion parameters of this camera object
614 * @return The resulting normalized and (optional undistorted) image point
615 */
616 inline VectorT2<T> imagePoint2normalizedImagePointDamped(const VectorT2<T>& imagePoint, const bool undistortImagePoint) const;
617
618 /**
619 * Calculates the image point corresponding to a given normalized image point.
620 * @param normalizedImagePoint The normalized image point (projected 3D object point without adjustment of focal length and principal point)
621 * @param distortImagePoint True, to force the distortion of the normalized image point using the distortion parameters of this camera object
622 * @return The resulting image point
623 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
624 */
625 template <bool tUseBorderDistortionIfOutside>
626 VectorT2<T> normalizedImagePoint2imagePoint(const VectorT2<T>& normalizedImagePoint, const bool distortImagePoint) const;
627
628 /**
629 * Calculates the image points corresponding to a set of given normalized image points.
630 * @param normalizedImagePoints The set of normalized image points (projected 3D object points without adjustment of focal length and principal point)
631 * @param numberNormalizedImagePoints The number of normalized image points
632 * @param distortImagePoints True, to force the distortion of the normalized image point using the distortion parameters of this camera object
633 * @param imagePoints The resulting image points, make sure that enough memory is provided
634 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
635 */
636 template <bool tUseBorderDistortionIfOutside>
637 void normalizedImagePoints2imagePoints(const VectorT2<T>* normalizedImagePoints, const size_t numberNormalizedImagePoints, const bool distortImagePoints, VectorT2<T>* imagePoints) const;
638
639 /**
640 * Projects a 3D object point to the 2D image plane of the camera by a given camera pose.
641 * This function may not apply the distortion parameters if the distortion-free projected image point lies outside the camera image.<br>
642 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
643 * The viewing direction of the camera is along the negative z-axis.<br>
644 * The extrinsic matrix will be flipped and inverted internally.<br>
645 * Further this function can apply a specific zoom to the intrinsic camera matrix.
646 * @param world_T_camera The pose of the camera, must be valid
647 * @param worldObjectPoint The 3D object point to project, defined in world
648 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
649 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
650 * @return The imagePoint Resulting 2D image plane point defined inside the camera pixel coordinate system
651 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
652 * @see projectToImageIF().
653 */
654 template <bool tUseBorderDistortionIfOutside>
655 inline VectorT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>& worldObjectPoint, const bool distortImagePoint, const T zoom = T(1)) const;
656
657 /**
658 * Projects a 3D box to the 2D image plane of the camera by a given camera pose.
659 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
660 * The viewing direction of the camera is along the negative z-axis.<br>
661 * The extrinsic matrix will be flipped and inverted internally.<br>
662 * Further this function can apply a specific zoom to the intrinsic camera matrix.
663 * @param world_T_camera The pose of the camera, must be valid
664 * @param worldObjectBox The 3D box to project, defined in world, must be valid
665 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
666 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
667 * @return The 2D image box defined inside the camera pixel coordinate system
668 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
669 * @see projectToImageIF().
670 */
671 template <bool tUseBorderDistortionIfOutside>
672 inline BoxT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const BoxT3<T>& worldObjectBox, const bool distortImagePoint, const T zoom = T(1)) const;
673
674 /**
675 * Projects a 3D triangle to the 2D image plane of the camera by a given camera pose.
676 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
677 * The viewing direction of the camera is along the negative z-axis.<br>
678 * The extrinsic matrix will be flipped and inverted internally.<br>
679 * Further this function can apply a specific zoom to the intrinsic camera matrix.
680 * @param world_T_camera The pose of the camera, must be valid
681 * @param worldObjectTriangle The 3D triangle to project, defined in world, must be valid
682 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
683 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
684 * @return The 2D image triangle defined inside the camera pixel coordinate system
685 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
686 * @see projectToImageIF().
687 */
688 template <bool tUseBorderDistortionIfOutside>
689 inline TriangleT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const TriangleT3<T>& worldObjectTriangle, const bool distortImagePoint, const T zoom = T(1)) const;
690
691 /**
692 * Projects a set of 3D object points onto an image plane of the camera by a given camera pose.
693 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
694 * The viewing direction of the camera is along the negative z-axis.<br>
695 * The extrinsic matrix will be flipped and inverted internally.<br>
696 * Further this function can apply a specific zoom to the intrinsic camera matrix.
697 * @param world_T_camera The pose of the camera, must be valid
698 * @param worldObjectPoints The 3D object points to project, defined in world
699 * @param numberObjectPoints The number of object points to project, with range [0, infinity)
700 * @param distortImagePoints True, to force the distortion of the image point using the distortion parameters of this camera object
701 * @param imagePoints The resulting image points, make sure that enough memory is provided
702 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
703 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
704 * @see projectToImageIF().
705 */
706 template <bool tUseBorderDistortionIfOutside>
707 inline void projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* worldObjectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T zoom = T(1)) const;
708
709 /**
710 * Projects a 3D line onto an image plane of the camera by a given camera pose.
711 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
712 * The viewing direction of the camera is along the negative z-axis.<br>
713 * The extrinsic matrix will be flipped and inverted internally.<br>
714 * Further this function can apply a specific zoom to the intrinsic camera matrix.
715 * @param world_T_camera The pose of the camera, must be valid
716 * @param worldLine The 3D line to be projected, defined in world, with unit length direction
717 * @param distortProjectedLine True, to distort the projected 3D line (a very rough approximation only)
718 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
719 * @return The projectedLine Resulting projected 2D line, an invalid line is returned if the projection fails
720 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
721 */
722 template <bool tUseBorderDistortionIfOutside>
723 inline LineT2<T> projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const LineT3<T>& worldLine, const bool distortProjectedLine, const T zoom = T(1)) const;
724
725 /**
726 * Projects a 3D object point to the 2D image plane of the camera by a given inverse camera pose.
727 * This function may not apply the distortion parameters if the (default) projected image point lies outside the camera image.<br>
728 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
729 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
730 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
731 * Further this function can apply a specific zoom to the intrinsic camera matrix.
732 * @param flippedCamera_T_world The inverted and flipped extrinsic camera matrix, must be valid
733 * @param objectPoint The 3D object point to project
734 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
735 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
736 * @return The imagePoint Resulting 2D image plane point defined inside the camera pixel coordinate system
737 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
738 * @see projectToImage().
739 */
740 template <bool tUseBorderDistortionIfOutside>
741 VectorT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint, const bool distortImagePoint, const T zoom = T(1)) const;
742
743 /**
744 * Projects a 3D object point to the 2D image plane of the camera by a given inverse camera pose.
745 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
746 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
747 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
748 * Further this function can apply a specific zoom to the intrinsic camera matrix.
749 * @param flippedCamera_T_world The inverted and flipped extrinsic camera matrix, must be valid
750 * @param worldObjectPoint The 3D object point to project, defined in world
751 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
752 * @return The 2D image plane point defined inside the camera pixel coordinate system
753 * @tparam tDistortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
754 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
755 * @see projectToImage().
756 */
757 template <bool tDistortImagePoint, bool tUseBorderDistortionIfOutside>
758 inline VectorT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, const T zoom = T(1)) const;
759
760 /**
761 * Projects a 3D box to the 2D image plane of the camera by a given inverse camera pose.
762 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
763 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
764 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
765 * Further this function can apply a specific zoom to the intrinsic camera matrix.
766 * @param flippedCamera_T_world The inverted and flipped extrinsic camera matrix, must be valid
767 * @param worldObjectBox The 3D box to project, defined in world, must be valid
768 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
769 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
770 * @return The 2D box defined inside the camera pixel coordinate system
771 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
772 * @see projectToImage().
773 */
774 template <bool tUseBorderDistortionIfOutside>
775 BoxT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const BoxT3<T>& worldObjectBox, const bool distortImagePoint, const T zoom = T(1)) const;
776
777 /**
778 * Projects a 3D triangle to the 2D image plane of the camera by a given extrinsic camera pose.
779 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
780 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
781 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
782 * Further this function can apply a specific zoom to the intrinsic camera matrix.
783 * @param flippedCamera_T_world The inverted and flipped extrinsic camera matrix, must be valid
784 * @param worldObjectTriangle The 3D triangle to project, defined in world, must be valid
785 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
786 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
787 * @return The 2D triangle defined inside the camera pixel coordinate system
788 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
789 * @see projectToImage().
790 */
791 template <bool tUseBorderDistortionIfOutside>
792 TriangleT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const TriangleT3<T>& worldObjectTriangle, const bool distortImagePoint, const T zoom = T(1)) const;
793
794 /**
795 * Transforms a normalized object point (a 3D object point transformed by the inverted and flipped extrinsic camera matrix) into the camera pixel coordinate system.
796 * Further this function can apply a specific zoom to the intrinsic camera matrix.
797 * @param normalizedObjectPoint The 2D normalized object point of a 3D object point
798 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
799 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
800 * @return The 2D image plane point defined inside the camera pixel coordinate system
801 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
802 * @see projectToImage().
803 */
804 template <bool tUseBorderDistortionIfOutside>
805 VectorT2<T> projectToImageIF(const VectorT2<T>& normalizedObjectPoint, const bool distortImagePoint, const T zoom = T(1)) const;
806
807 /**
808 * Projects a set of 3D object points onto an image plane of the camera by a given inverse camera pose.
809 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
810 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
811 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
812 * Further this function can apply a specific zoom to the intrinsic camera matrix.
813 * @param flippedCamera_T_world The inverted and flipped extrinsic camera matrix, must be valid
814 * @param worldObjectPoints The 3D object points to project, defined in world
815 * @param numberObjectPoints The number of object points to project, with range [0, infinity)
816 * @param distortImagePoints True, to force the distortion of the image point using the distortion parameters of this camera object
817 * @param imagePoints The resulting image points, make sure that enough memory is provided
818 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
819 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
820 * @see projectToImage().
821 */
822 template <bool tUseBorderDistortionIfOutside>
823 void projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* worldObjectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T zoom = T(1)) const;
824
825 /**
826 * Projects a 3D line onto an image plane of the camera by a given inverse camera pose.
827 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
828 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
829 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
830 * Further this function can apply a specific zoom to the intrinsic camera matrix.
831 * @param flippedCamera_T_world The inverted and flipped extrinsic camera matrix, must be valid
832 * @param worldLine The 3D line to be projected, with unit length direction, defined in world, must be valid
833 * @param distortProjectedLine True, to distort the projected 3D line (a very rough approximation only)
834 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
835 * @return The projectedLine Resulting projected 2D line, an invalid line is returned if the projection fails
836 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
837 */
838 template <bool tUseBorderDistortionIfOutside>
839 LineT2<T> projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const LineT3<T>& worldLine, const bool distortProjectedLine, const T zoom = T(1)) const;
840
841 /**
842 * Projects a 3D object point to the 2D image plane of the camera by a given extrinsic camera matrix.
843 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
844 * The viewing direction of the camera is along the negative z-axis.<br>
845 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
846 * Further this function can apply a specific zoom to the intrinsic camera matrix.
847 * @param extrinsic The extrinsic camera matrix
848 * @param objectPoint The 3D object point to project
849 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
850 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
851 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
852 * @return The imagePoint Resulting 2D image plane point defined inside the camera pixel coordinate system
853 * @see projectToImageDampedIF().
854 */
855 inline VectorT2<T> projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const VectorT3<T>& objectPoint, const bool distortImagePoint, const T dampingFactor = T(1), const T zoom = T(1)) const;
856
857 /**
858 * Projects a 3D box to the 2D image plane of the camera by a given inverse extrinsic camera matrix.
859 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
860 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
861 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
862 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
863 * Further this function can apply a specific zoom to the intrinsic camera matrix.
864 * @param extrinsic The extrinsic camera matrix
865 * @param objectBox The 3D box to project
866 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
867 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
868 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
869 * @return The 2D box defined inside the camera pixel coordinate system
870 * @see projectToImageDampedIF().
871 */
872 inline BoxT2<T> projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const BoxT3<T>& objectBox, const bool distortImagePoint, const T dampingFactor = T(1), const T zoom = T(1)) const;
873
874 /**
875 * Projects a 3D triangle to the 2D image plane of the camera by a given extrinsic camera matrix.
876 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
877 * The viewing direction of the camera is along the negative z-axis.<br>
878 * The extrinsic matrix will be flipped and inverted internally.<br>
879 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
880 * Further this function can apply a specific zoom to the intrinsic camera matrix.
881 * @param extrinsic The extrinsic camera matrix
882 * @param objectTriangle The 3D triangle to project
883 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
884 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
885 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
886 * @return The 2D image triangle defined inside the camera pixel coordinate system
887 * @see projectToImageDampedIF().
888 */
889 inline TriangleT2<T> projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const TriangleT3<T>& objectTriangle, const bool distortImagePoint, const T dampingFactor = T(1), const T zoom = T(1)) const;
890
891 /**
892 * Projects a set of 3D object points onto an image plane of the camera by a given extrinsic camera matrix.
893 * The extrinsic matrix transforms a 3D point given in camera coordinates into 3D world coordinates.<br>
894 * The viewing direction of the camera is along the negative z-axis.<br>
895 * The extrinsic matrix will be flipped and inverted internally.<br>
896 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
897 * Further this function can apply a specific zoom to the intrinsic camera matrix.
898 * @param extrinsic The extrinsic camera matrix
899 * @param objectPoints The set of 3D object points to project
900 * @param numberObjectPoints The number of object points to project
901 * @param distortImagePoints True, to force the distortion of the image point using the distortion parameters of this camera object
902 * @param imagePoints The resulting image points, make sure that enough memory is provided
903 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
904 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
905 * @see projectToImageIF().
906 */
907 inline void projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const VectorT3<T>* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T dampingFactor = T(1), const T zoom = T(1)) const;
908
909 /**
910 * Projects a 3D object point to the 2D image plane of the camera by a given inverse extrinsic camera matrix.
911 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
912 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
913 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
914 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
915 * Further this function can apply a specific zoom to the intrinsic camera matrix.
916 * @param iFlippedExtrinsic The inverted and flipped extrinsic camera matrix
917 * @param objectPoint The 3D object point to project
918 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
919 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
920 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
921 * @return The resulting 2D image plane point defined inside the camera pixel coordinate system
922 * @see projectToImageDamped().
923 */
924 VectorT2<T> projectToImageDampedIF(const HomogenousMatrixT4<T>& iFlippedExtrinsic, const VectorT3<T>& objectPoint, const bool distortImagePoint, const T dampingFactor = T(1), const T zoom = T(1)) const;
925
926 /**
927 * Projects a 3D box to the 2D image plane of the camera by a given inverse extrinsic camera matrix.
928 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
929 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
930 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
931 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
932 * Further this function can apply a specific zoom to the intrinsic camera matrix.
933 * @param iFlippedExtrinsic The inverted and flipped extrinsic camera matrix
934 * @param objectBox The 3D box to project
935 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
936 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
937 * @param zoom The optional zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
938 * @return The 2D box defined inside the camera pixel coordinate system
939 * @see projectToImageDamped().
940 */
941 BoxT2<T> projectToImageDampedIF(const HomogenousMatrixT4<T>& iFlippedExtrinsic, const BoxT3<T>& objectBox, const bool distortImagePoint, const T dampingFactor = T(1), const T zoom = T(1)) const;
942
943 /**
944 * Projects a 3D triangle to the 2D image plane of the camera by a given inverse extrinsic camera matrix.
945 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
946 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
947 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
948 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
949 * Further this function can apply a specific zoom to the intrinsic camera matrix.
950 * @param iFlippedExtrinsic The inverted and flipped extrinsic camera matrix
951 * @param objectTriangle The 3D triangle to project
952 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
953 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
954 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
955 * @return The 2D triangle defined inside the camera pixel coordinate system
956 * @see projectToImageDamped().
957 */
958 TriangleT2<T> projectToImageDampedIF(const HomogenousMatrixT4<T>& iFlippedExtrinsic, const TriangleT3<T>& objectTriangle, const bool distortImagePoint, const T dampingFactor = T(1), const T zoom = T(1)) const;
959
960 /**
961 * Projects a set of 3D object points onto an image plane of the camera by a given inverse extrinsic camera matrix.
962 * The inverse extrinsic matrix transforms a 3D point given in world coordinates into 3D camera coordinates.<br>
963 * The coordinate system of the camera is flipped meaning that the viewing direction is along the positive z-axis.<br>
964 * The flipped coordinate system can be received by a rotation around the x-axis by 180 degree.<br>
965 * Object points projecting outside the camera frame will be distorted (if desired) by application of a damping factor.<br>
966 * Further this function can apply a specific zoom to the intrinsic camera matrix.
967 * @param invertedFlippedExtrinsic The inverted and flipped extrinsic camera matrix
968 * @param objectPoints The set of 3D object points to project
969 * @param numberObjectPoints The number of object points to project
970 * @param distortImagePoints True, to force the distortion of the image point using the distortion parameters of this camera object
971 * @param imagePoints The resulting image points, make sure that enough memory is provided
972 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
973 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
974 * @see projectToImageDamped().
975 */
976 void projectToImageDampedIF(const HomogenousMatrixT4<T>& invertedFlippedExtrinsic, const VectorT3<T>* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T dampingFactor = T(1), const T zoom = T(1)) const;
977
978 /**
979 * Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given 2D point on the image plane.
980 * @param position The 2D position on the image plane, specified in the pixel domain
981 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
982 * @return The normalized vector with -Z direction
983 * @see vectorIF(), ray().
984 */
985 inline VectorT3<T> vector(const VectorT2<T>& position, const bool makeUnitVector = true) const;
986
987 /**
988 * Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given 2D point on the image plane.
989 * Further this function can apply a specific zoom to the intrinsic camera matrix.
990 * @param position The 2D position on the image plane, specified in the pixel domain
991 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
992 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
993 * @return The normalized vector with -Z direction
994 * @see vectorIF(), ray().
995 */
996 inline VectorT3<T> vector(const VectorT2<T>& position, const T zoom, const bool makeUnitVector = true) const;
997
998 /**
999 * Returns a vector starting at the camera's center and intersecting a given 2D point on the image plane.
1000 * The length of the vector is determined so that the vector (exactly) reaches a plane parallel to the image plane.<br>
1001 * The plane's normal is parallel to the z-axis while a positive distance locates the plane in the space of the negative z-axis.
1002 * @param position The 2D position on the image plane, specified in the pixel domain
1003 * @param distance The distance between the camera's center and the plane parallel to the image plane, with range (0, infinity)
1004 * @return The normalized vector with -Z direction
1005 */
1006 inline VectorT3<T> vectorToPlane(const VectorT2<T>& position, const T distance) const;
1007
1008 /**
1009 * Returns a vector starting at the camera's center and intersecting a given 2D point on the image plane.
1010 * Further this function can apply a specific zoom to the intrinsic camera matrix.<br>
1011 * The length of the vector is determined so that the vector (exactly) reaches a plane parallel to the image plane.<br>
1012 * The plane's normal is parallel to the z-axis while a positive distance locates the plane in the space of the negative z-axis.
1013 * @param position The 2D position on the image plane, specified in the pixel domain
1014 * @param distance The distance between the camera's center and the plane parallel to the image plane, with range (0, infinity)
1015 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
1016 * @return The normalized vector with -Z direction
1017 */
1018 inline VectorT3<T> vectorToPlane(const VectorT2<T>& position, const T distance, const T zoom) const;
1019
1020 /**
1021 * Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given 2D point on the image plane.
1022 * @param position The 2D position on the image plane, specified in the pixel domain
1023 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
1024 * @return The normalized vector with +Z direction
1025 */
1026 inline VectorT3<T> vectorIF(const VectorT2<T>& position, const bool makeUnitVector = true) const;
1027
1028 /**
1029 * Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given 2D point on the image plane.
1030 * Further this function can apply a specific zoom to the intrinsic camera matrix.
1031 * @param position The 2D position on the image plane, specified in the pixel domain
1032 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
1033 * @param makeUnitVector True, to return a vector with length 1; False, to return a vector with any length
1034 * @return The normalized vector with +Z direction
1035 */
1036 inline VectorT3<T> vectorIF(const VectorT2<T>& position, const T zoom, const bool makeUnitVector) const;
1037
1038 /**
1039 * Returns a ray starting at the camera's center and intersection a given 2D point on the image plane.
1040 * Further this function can apply a specific zoom to the intrinsic camera matrix.
1041 * @param position The 2D position on the image plane, specified in the pixel domain
1042 * @param world_T_camera The pose of the camera, the extrinsic camera matrix, must be valid
1043 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
1044 * @return The specified ray for a camera pointing towards the negative z-space
1045 * @see vector().
1046 */
1047 inline LineT3<T> ray(const VectorT2<T>& position, const HomogenousMatrixT4<T>& world_T_camera, const T zoom = T(1)) const;
1048
1049 /**
1050 * Returns a ray starting at the camera's center and intersection a given 2D point on the image plane.
1051 * Further this function can apply a specific zoom to the intrinsic camera matrix.
1052 * @param position The 2D position on the image plane, specified in the pixel domain
1053 * @param world_t_camera The translation (position) of the camera pose
1054 * @param world_Q_camera The standard rotation quaternion of the camera pose, must be valid
1055 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
1056 * @return The specified ray for a camera pointing towards the negative z-space
1057 * @see vector().
1058 */
1059 inline LineT3<T> ray(const VectorT2<T>& position, const VectorT3<T>& world_t_camera, const QuaternionT<T>& world_Q_camera, const T zoom = T(1)) const;
1060
1061 /**
1062 * Returns the 4x4 frustum projection matrix corresponding to this camera.
1063 * The frustum matrix is defined to point into negative z axis and does not provide any distortion parameters.<br>
1064 * @param nearDistance The positive distance to the near clipping plane
1065 * @param farDistance The positive distance to the far clipping plane
1066 * @return The resulting frustum projection matrix
1067 */
1068 SquareMatrixT4<T> frustumMatrix(const T nearDistance, const T farDistance) const;
1069
1070 /**
1071 * Returns a 4x4 homogenous transformation matrix (corresponding to a 3x4 matrix) that covers an extrinsic (inverted and flipped) camera matrix and the intrinsic projection matrix of this camera object.
1072 * Further this function can apply a specific zoom to the intrinsic camera matrix.
1073 * @param iFlippedExtrinsic The inverted and flipped extrinsic camera matrix
1074 * @param zoom The zoom factor of the camera, with range (0, infinity), with 1 the default zoom factor
1075 * @return The resulting transformation matrix
1076 */
1077 inline HomogenousMatrixT4<T> transformationMatrixIF(const HomogenousMatrixT4<T>& iFlippedExtrinsic, const T zoom = T(1)) const;
1078
1079 /**
1080 * Returns whether two camera profiles are identical up to a given epsilon.
1081 * The image resolution must always be identical.
1082 * @param camera The second camera profile to be used for comparison, can be invalid
1083 * @param eps The epsilon threshold to be used, with range [0, infinity)
1084 * @return True, if so
1085 */
1086 bool isEqual(const PinholeCameraT<T>& camera, const T eps = NumericT<T>::eps()) const;
1087
1088 /**
1089 * Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
1090 * The resulting jacobian matrix has the following layout:
1091 * <pre>
1092 * | dfu / dx, dfu / dy, dfu / dz |
1093 * | dfv / dx, dfv / dy, dfv / dz |
1094 * with projection function
1095 * q = f(p)
1096 * q_u = fu(p), q_y = fv(p)
1097 * with 2D image point q = (q_u, q_v) and 3D object point p = (x, y, z)
1098 * </pre>
1099 * @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).
1100 * @param jx The resulting first row of the Jacobian matrix, must contain three elements, must be valid
1101 * @param jy The resulting second row of the Jacobian matrix, must contain three elements, must be valid
1102 * @tparam U The data type of the scalar element either 'float' or 'double'
1103 * @tparam tUseDistortionParameters True, to use the camera profile's distortion parameters when calculating the Jacobian; False, to skip the distortion parameters
1104 */
1105 template <typename U, bool tUseDistortionParameters>
1106 inline void pointJacobian2x3IF(const VectorT3<U>& flippedCameraObjectPoint, U* jx, U* jy) const;
1107
1108 /**
1109 * Returns whether this camera is valid.
1110 * @return True, if so
1111 */
1112 inline bool isValid() const;
1113
1114 /**
1115 * Returns whether two camera objects are identical up to a small epsilon.
1116 * @param camera The second camera object
1117 * @return True, if so
1118 */
1119 bool operator==(const PinholeCameraT<T>& camera) const;
1120
1121 /**
1122 * Returns whether two camera objects are not identical up to a small epsilon.
1123 * @param camera The second camera object
1124 * @return True, if so
1125 */
1126 inline bool operator!=(const PinholeCameraT<T>& camera) const;
1127
1128 /**
1129 * Returns whether the camera holds valid parameters.
1130 * @return True, if so
1131 */
1132 explicit inline operator bool() const;
1133
1134 private:
1135
1136 /**
1137 * Determines the inverse of the intrinsic camera matrix.
1138 * This function must be invoked immediately after the intrinsic matrix has changed.
1139 */
1140 inline void calculateInverseIntrinsic();
1141
1142 /**
1143 * Returns the distorted position of a given undistorted normalized position.<br>
1144 * The distorted position is calculated by the usage of the internal distortion parameters of this camera object:
1145 * <pre>
1146 * x' = x + x * (k1 * r^2 + k2 * r^4) + p1 * 2 * x * y + p2 * (r^2 + 2 * x^2),
1147 * y' = y + y * (k1 * r^2 + k2 * r^4) + p2 * 2 * x * y + p1 * (r^2 + 2 * y^2).
1148 * </pre>
1149 * @param undistortedNormalized The undistorted normalized position to be distorted
1150 * @param invZoom The optional the inverse zoom factor (1/zoom) of the camera, with range (0, infinity), with 1 the default zoom factor
1151 * @return The resulting distorted normalized position
1152 * @tparam tUseBorderDistortionIfOutside True, to apply the distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False to apply the distortion from the given position
1153 * @see distort().
1154 */
1155 template <bool tUseBorderDistortionIfOutside>
1156 VectorT2<T> distortNormalized(const VectorT2<T>& undistortedNormalized, const T invZoom) const;
1157
1158 /**
1159 * Returns the distorted position of a given undistorted normalized position.<br>
1160 * This function applies a damping for the distortion outside the camera frame so that the quadratic and quartic radii do not have such a significant impact.<br>
1161 * The damping has an asymptotic pattern and so that distortion is based on normalized coordinates never reaching a specified boundary.<br>
1162 * The distorted position is calculated by the usage of the internal distortion parameters of this camera object:
1163 * <pre>
1164 * dx = x + x * (k1 * r^2 + k2 * r^4) + p1 * 2 * x * y + p2 * (r^2 + 2 * x^2),
1165 * dy = y + y * (k1 * r^2 + k2 * r^4) + p2 * 2 * x * y + p1 * (r^2 + 2 * y^2).
1166 * </pre>
1167 * With (dx, dy) the distorted normalized point coordinates and (x, y) the undistorted normalized point coordinates.
1168 * @param undistortedNormalized The undistorted normalized position to be distorted
1169 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
1170 * @param invZoom The optional the inverse zoom factor (1/zoom) of the camera, with range (0, infinity), with 1 the default zoom factor
1171 * @return The resulting distorted normalized position
1172 * @see distortDamped().
1173 */
1174 VectorT2<T> distortNormalizedDamped(const VectorT2<T>& undistortedNormalized, const T dampingFactor, const T invZoom) const;
1175
1176 /**
1177 * Determines the damped normalized coordinate for a given normalized coordinate.
1178 * The damping is applied to coordinates outside the camera frame.<br>
1179 * The damping has an asymptotic pattern so that a damped coordinate never reaches a boundary (specified by the dampingFactor).
1180 * @param normalized The normalized coordinate for which the damped coordinate will be calculated
1181 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
1182 * @param invZoom The optional the inverse zoom factor (1/zoom) of the camera, with range (0, infinity), with 1 the default zoom factor
1183 * @return The damped normalized coordinate
1184 */
1185 VectorT2<T> dampedNormalized(const VectorT2<T>& normalized, const T dampingFactor, const T invZoom) const;
1186
1187 /**
1188 * Determines the damped normalized coordinate for a given normalized coordinate.
1189 * The damping is applied to coordinates outside the camera frame.<br>
1190 * The damping has an asymptotic pattern so that a damped coordinate never reaches a boundary (specified by the dampingFactor).
1191 * @param normalized The normalized coordinate for which the damped coordinate will be calculated
1192 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
1193 * @param leftNormalizedBorder The left border of camera frame in normalized coordinates, respecting a possible zoom factor already
1194 * @param rightNormalizedBorder The right border of the camera frame in normalized coordinates, respecting a possible zoom factor already
1195 * @param topNormalizedBorder The top border of the camera frame in normalized coordinates, respecting a possible zoom factor already
1196 * @param bottomNormalizedBorder The bottom border of the camera frame in normalized coordinates, respecting a possible zoom factor already
1197 * @return The damped normalized coordinate
1198 */
1199 VectorT2<T> dampedNormalized(const VectorT2<T>& normalized, const T dampingFactor, const T leftNormalizedBorder, const T rightNormalizedBorder, const T topNormalizedBorder, const T bottomNormalizedBorder) const;
1200
1201 private:
1202
1203 /// Intrinsic camera matrix.
1204 SquareMatrixT3<T> intrinsics_ = SquareMatrixT3<T>(false);
1205
1206 /// Inverted intrinsic camera matrix.
1207 SquareMatrixT3<T> invertedIntrinsics_ = SquareMatrixT3<T>(false);
1208
1209 /// Width of the camera image (in pixels).
1210 unsigned int width_ = 0u;
1211
1212 /// Height of the camera image (in pixels).
1213 unsigned int height_ = 0u;
1214
1215 /// Pair of radial distortion parameters for r^2 and r^4.
1216 DistortionPair radialDistortion_ = DistortionPair(T(0), T(0));
1217
1218 /// Pair of tangential distortion parameters.
1219 DistortionPair tangentialDistortion_ = DistortionPair(T(0), T(0));
1220};
1221
1222template <typename T>
1223inline PinholeCameraT<T>::DistortionLookup::DistortionLookup()
1224{
1225 // nothing to do here
1226}
1227
1228template <typename T>
1229inline VectorT2<T> PinholeCameraT<T>::DistortionLookup::undistortedImagePoint(const VectorT2<T>& distortedImagePoint) const
1230{
1231 ocean_assert(distortionLookupTable);
1232 return distortionLookupTable.bilinearValue(distortedImagePoint.x(), distortedImagePoint.y()) + distortedImagePoint;
1233}
1234
1235template <typename T>
1236inline VectorT2<T> PinholeCameraT<T>::DistortionLookup::undistortionOffset(const VectorT2<T>& distortedImagePoint) const
1237{
1238 ocean_assert(distortionLookupTable);
1239 return distortionLookupTable.bilinearValue(distortedImagePoint.x(), distortedImagePoint.y());
1240}
1241
1242template <typename T>
1243inline VectorT2<T> PinholeCameraT<T>::DistortionLookup::undistortedImagePointBicubic(const VectorT2<T>& distortedImagePoint) const
1244{
1245 ocean_assert(distortionLookupTable);
1246 return distortionLookupTable.bicubicValue(distortedImagePoint.x(), distortedImagePoint.y()) + distortedImagePoint;
1247}
1248
1249template <typename T>
1250inline VectorT2<T> PinholeCameraT<T>::DistortionLookup::undistortionOffsetBicubic(const VectorT2<T>& distortedImagePoint) const
1251{
1252 ocean_assert(distortionLookupTable);
1253 return distortionLookupTable.bicubicValue(distortedImagePoint.x(), distortedImagePoint.y());
1254}
1255
1256template <typename T>
1257inline const SquareMatrixT3<T>& PinholeCameraT<T>::intrinsic() const
1258{
1259 return intrinsics_;
1260}
1261
1262template <typename T>
1267
1268template <typename T>
1273
1274template <typename T>
1279
1280template <typename T>
1281template <typename U>
1282inline PinholeCameraT<T>::PinholeCameraT(const PinholeCameraT<U>& pinholeCamera, const bool copyDistortionParameters) :
1283 PinholeCameraT<T>(SquareMatrixT3<T>(pinholeCamera.intrinsics_), pinholeCamera.width(), pinholeCamera.height())
1284{
1285 if (copyDistortionParameters)
1286 {
1287 radialDistortion_ = DistortionPair(T(pinholeCamera.radialDistortion_.first), T(pinholeCamera.radialDistortion_.second));
1288 tangentialDistortion_ = DistortionPair(T(pinholeCamera.tangentialDistortion_.first), T(pinholeCamera.tangentialDistortion_.second));
1289 }
1290}
1291
1292template <typename T>
1293inline bool PinholeCameraT<T>::hasDistortionParameters() const
1294{
1295 return radialDistortion_.first != 0 || radialDistortion_.second != 0
1296 || tangentialDistortion_.first != 0 || tangentialDistortion_.second != 0;
1297}
1298
1299template <typename T>
1300inline unsigned int PinholeCameraT<T>::width() const
1301{
1302 return width_;
1303}
1304
1305template <typename T>
1306inline unsigned int PinholeCameraT<T>::height() const
1307{
1308 return height_;
1309}
1310
1311template <typename T>
1312inline VectorT2<T> PinholeCameraT<T>::principalPoint() const
1313{
1314 return VectorT2<T>(principalPointX(), principalPointY());
1315}
1316
1317template <typename T>
1318inline T PinholeCameraT<T>::principalPointX() const
1319{
1320 return intrinsics_(6);
1321}
1322
1323template <typename T>
1324inline T PinholeCameraT<T>::principalPointY() const
1325{
1326 return intrinsics_(7);
1327}
1328
1329template <typename T>
1330inline T PinholeCameraT<T>::focalLengthX() const
1331{
1332 return intrinsics_(0);
1333}
1334
1335template <typename T>
1336inline T PinholeCameraT<T>::focalLengthY() const
1337{
1338 return intrinsics_(4);
1339}
1340
1341template <typename T>
1342inline T PinholeCameraT<T>::inverseFocalLengthX() const
1343{
1344 ocean_assert((std::is_same<T, float>::value) || intrinsics_.inverted() == invertedIntrinsics_);
1345 ocean_assert(NumericT<T>::isEqual(invertedIntrinsics_(0) * intrinsics_(0), 1));
1346
1347 return invertedIntrinsics_(0);
1348}
1349
1350template <typename T>
1351inline T PinholeCameraT<T>::inverseFocalLengthY() const
1352{
1353 ocean_assert((std::is_same<T, float>::value) || intrinsics_.inverted() == invertedIntrinsics_);
1354 ocean_assert(NumericT<T>::isEqual(invertedIntrinsics_(4) * intrinsics_(4), 1));
1355
1356 return invertedIntrinsics_(4);
1357}
1358
1359template <typename T>
1360inline void PinholeCameraT<T>::setRadialDistortion(const DistortionPair& radial)
1361{
1362 radialDistortion_ = radial;
1363}
1364
1365template <typename T>
1366inline void PinholeCameraT<T>::setTangentialDistortion(const DistortionPair& tangential)
1367{
1368 tangentialDistortion_ = tangential;
1369}
1370
1371template <typename T>
1372template <bool tUseBorderDistortionIfOutside>
1373VectorT2<T> PinholeCameraT<T>::undistort(const VectorT2<T>& distorted, const unsigned int iterations, const T zoom) const
1374{
1375 ocean_assert(iterations >= 1u && iterations <= 1000u && zoom > NumericT<T>::eps());
1376
1377 // check whether the camera is distortion free
1378 if (!hasDistortionParameters())
1379 {
1380 return distorted;
1381 }
1382
1383 const T invZoom = T(1) / zoom;
1384
1385 const VectorT2<T> nDistorted((distorted.x() - principalPointX()) * inverseFocalLengthX() * invZoom, (distorted.y() - principalPointY()) * inverseFocalLengthY() * invZoom);
1386
1387 const VectorT2<T> nMainOffset(distortNormalized<tUseBorderDistortionIfOutside>(nDistorted, invZoom) - nDistorted);
1388 VectorT2<T> nIntermediateUndistorted(nDistorted - nMainOffset);
1389
1390 unsigned int i = 0u;
1391
1392 while (i++ < iterations)
1393 {
1394 const VectorT2<T> nIntermediateDistorted(distortNormalized<tUseBorderDistortionIfOutside>(nIntermediateUndistorted, invZoom));
1395 const VectorT2<T> nIntermediateOffset(nDistorted - nIntermediateDistorted);
1396
1397 nIntermediateUndistorted = nIntermediateUndistorted + nIntermediateOffset * T(0.75);
1398
1399 const T offsetPixelX = NumericT<T>::abs(nIntermediateOffset.x() * focalLengthX());
1400 const T offsetPixelY = NumericT<T>::abs(nIntermediateOffset.y() * focalLengthY());
1401
1402 if (offsetPixelX < 0.05 && offsetPixelY < 0.05)
1403 {
1404 break;
1405 }
1406
1407 if (offsetPixelX > T(width_ * 10u) || offsetPixelY > T(height_ * 10u))
1408 {
1409 return distorted;
1410 }
1411 }
1412
1413 return VectorT2<T>(nIntermediateUndistorted.x() * focalLengthX() * zoom + principalPointX(), nIntermediateUndistorted.y() * focalLengthY() * zoom + principalPointY());
1414}
1415
1416template <typename T>
1417template <bool tUseBorderDistortionIfOutside>
1418VectorT2<T> PinholeCameraT<T>::distort(const VectorT2<T>& undistorted) const
1419{
1420 if (hasDistortionParameters())
1421 {
1422 if constexpr (tUseBorderDistortionIfOutside)
1423 {
1424 const VectorT2<T> nUndistorted((undistorted.x() - principalPointX()) * inverseFocalLengthX(),
1425 (undistorted.y() - principalPointY()) * inverseFocalLengthY());
1426
1427 const VectorT2<T> clampedNormalizedImagePoint(minmax(-principalPointX() * inverseFocalLengthX(), nUndistorted.x(), (T(width_) - principalPointX()) * inverseFocalLengthX()),
1428 minmax(-principalPointY() * inverseFocalLengthY(), nUndistorted.y(), (T(height_) - principalPointY()) * inverseFocalLengthY()));
1429
1430 const T sqr = clampedNormalizedImagePoint.sqr();
1431
1432 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1433
1434 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
1435 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
1436
1437 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
1438 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
1439
1440 return VectorT2<T>((nUndistorted.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() + principalPointX(),
1441 (nUndistorted.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() + principalPointY());
1442 }
1443 else
1444 {
1445 const VectorT2<T> nUndistorted((undistorted.x() - principalPointX()) * inverseFocalLengthX(),
1446 (undistorted.y() - principalPointY()) * inverseFocalLengthY());
1447
1448 const T sqr = nUndistorted.sqr();
1449
1450 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1451
1452 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * nUndistorted.x() * nUndistorted.y()
1453 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(nUndistorted.x()));
1454
1455 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(nUndistorted.y()))
1456 + tangentialDistortion_.second * 2 * nUndistorted.x() * nUndistorted.y();
1457
1458 return VectorT2<T>((nUndistorted.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() + principalPointX(),
1459 (nUndistorted.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() + principalPointY());
1460 }
1461 }
1462 else
1463 {
1464 return undistorted;
1465 }
1466}
1467
1468template <typename T>
1469inline bool PinholeCameraT<T>::isInside(const VectorT2<T>& imagePoint, const T signedBorder) const
1470{
1471 ocean_assert(isValid());
1472 ocean_assert(signedBorder < T(std::min(width_ / 2u, height_ / 2u)));
1473
1474 return imagePoint.x() >= signedBorder && imagePoint.y() >= signedBorder
1475 && imagePoint.x() < T(width_) - signedBorder && imagePoint.y() < T(height_) - signedBorder;
1476}
1477
1478template <typename T>
1479inline HomogenousMatrixT4<T> PinholeCameraT<T>::transformationMatrixIF(const HomogenousMatrixT4<T>& iFlippedExtrinsic, const T zoom) const
1480{
1481 ocean_assert(zoom > NumericT<T>::eps());
1482
1483#ifdef OCEAN_DEBUG
1484 SquareMatrixT4<T> debugCameraMatrix(intrinsic());
1485 debugCameraMatrix(0, 0) *= zoom;
1486 debugCameraMatrix(1, 1) *= zoom;
1487 debugCameraMatrix[15] = 1;
1488
1489 const SquareMatrixT4<T> debugEntireMatrix(debugCameraMatrix * (SquareMatrixT4<T>&)iFlippedExtrinsic);
1490 const HomogenousMatrixT4<T>& debugTransformationMatrix = (const HomogenousMatrixT4<T>&)debugEntireMatrix;
1491
1492 const HomogenousMatrixT4<T> transformationMatrix(HomogenousMatrixT4<T>(VectorT3<T>(intrinsics_[0] * zoom, intrinsics_[1], intrinsics_[2]), VectorT3<T>(intrinsics_[3], intrinsics_[4] * zoom, intrinsics_[5]), VectorT3<T>(intrinsics_[6], intrinsics_[7], intrinsics_[8])) * iFlippedExtrinsic);
1493 ocean_assert(transformationMatrix.isValid());
1494
1495 ocean_assert(transformationMatrix == debugTransformationMatrix);
1496#endif
1497
1498 return HomogenousMatrixT4<T>(HomogenousMatrixT4<T>(VectorT3<T>(intrinsics_[0] * zoom, intrinsics_[1], intrinsics_[2]), VectorT3<T>(intrinsics_[3], intrinsics_[4] * zoom, intrinsics_[5]), VectorT3<T>(intrinsics_[6], intrinsics_[7], intrinsics_[8])) * iFlippedExtrinsic);
1499}
1500
1501template <typename T>
1502template <typename U, bool tUseDistortionParameters>
1503inline void PinholeCameraT<T>::pointJacobian2x3IF(const VectorT3<U>& flippedCameraObjectPoint, U* jx, U* jy) const
1504{
1505 ocean_assert(isValid());
1506 ocean_assert(jx != nullptr && jy != nullptr);
1507
1508 if (tUseDistortionParameters && hasDistortionParameters())
1509 {
1510 const U x = U(flippedCameraObjectPoint.x());
1511 const U y = U(flippedCameraObjectPoint.y());
1512 const U z = U(flippedCameraObjectPoint.z());
1513
1514 const U fx = U(focalLengthX());
1515 const U fy = U(focalLengthY());
1516
1517 const U k1 = U(radialDistortion().first);
1518 const U k2 = U(radialDistortion().second);
1519
1520 const U p1 = U(tangentialDistortion().first);
1521 const U p2 = U(tangentialDistortion().second);
1522
1523 ocean_assert(NumericT<U>::isNotEqualEps(z));
1524 const U invZ = U(1) / z;
1525
1526 const U u = x * invZ;
1527 const U v = y * invZ;
1528
1529 const U dist1_u = U(1) + U(6) * p2 * u + U(2) * p1 * v + k1 * (U(3) * u * u + v * v) + k2 * (u * u + v * v) * (U(5) * u * u + v * v);
1530 const U dist2_u_1_v = U(2) * (p1 * u + v * (p2 + u * (k1 + U(2) * k2 * (u * u + v * v))));
1531 const U dist2_v = U(1) + U(2) * p2 * u + U(6) * p1 * v + k1 * (u * u + U(3) * v * v) + k2 * (u * u + v * v) * (u * u + U(5) * v * v);
1532
1533 const U Fx_w_dist1_u = fx * invZ * dist1_u;
1534 const U Fy_w_dist2_u = fy * invZ * dist2_u_1_v;
1535
1536 const U Fx_w_dist1_v = fx * invZ * dist2_u_1_v;
1537 const U Fy_w_dist2_v = fy * invZ * dist2_v;
1538
1539 const U Fx_w2__ = -fx * invZ * invZ * (x * dist1_u + y * dist2_u_1_v);
1540 const U Fy_w2__ = -fy * invZ * invZ * (x * dist2_u_1_v + y * dist2_v);
1541
1542 jx[0] = Fx_w_dist1_u;
1543 jx[1] = Fx_w_dist1_v;
1544 jx[2] = Fx_w2__;
1545
1546 jy[0] = Fy_w_dist2_u;
1547 jy[1] = Fy_w_dist2_v;
1548 jy[2] = Fy_w2__;
1549 }
1550 else
1551 {
1552 ocean_assert(NumericT<U>::isNotEqualEps(U(flippedCameraObjectPoint.z())));
1553 const U invZ = U(1) / U(flippedCameraObjectPoint.z());
1554
1555 const U fx_z = U(focalLengthX()) * invZ;
1556 const U fy_z = U(focalLengthY()) * invZ;
1557
1558 const U fx_x_z2 = -fx_z * U(flippedCameraObjectPoint.x()) * invZ;
1559 const U fy_y_z2 = -fy_z * U(flippedCameraObjectPoint.y()) * invZ;
1560
1561 jx[0] = fx_z;
1562 jx[1] = 0;
1563 jx[2] = fx_x_z2;
1564
1565 jy[0] = 0;
1566 jy[1] = fy_z;
1567 jy[2] = fy_y_z2;
1568 }
1569}
1570
1571template <typename T>
1572inline bool PinholeCameraT<T>::isValid() const
1573{
1574 return width_ != 0u && height_ != 0u;
1575}
1576
1577template <typename T>
1578template <bool tUseBorderDistortionIfOutside>
1579VectorT2<T> PinholeCameraT<T>::imagePoint2normalizedImagePoint(const VectorT2<T>& imagePoint, const bool undistortImagePoint) const
1580{
1581 if (undistortImagePoint)
1582 {
1583 return invertedIntrinsics_ * undistort<tUseBorderDistortionIfOutside>(imagePoint);
1584 }
1585
1586 return invertedIntrinsics_ * imagePoint;
1587}
1588
1589template <typename T>
1590inline VectorT2<T> PinholeCameraT<T>::imagePoint2normalizedImagePointDamped(const VectorT2<T>& imagePoint, const bool undistortImagePoint) const
1591{
1592 if (undistortImagePoint)
1593 {
1594 return invertedIntrinsics_ * undistortDamped(imagePoint);
1595 }
1596
1597 return invertedIntrinsics_ * imagePoint;
1598}
1599
1600template <typename T>
1601template <bool tUseBorderDistortionIfOutside>
1602VectorT2<T> PinholeCameraT<T>::normalizedImagePoint2imagePoint(const VectorT2<T>& normalizedImagePoint, const bool distortImagePoints) const
1603{
1604 ocean_assert(isValid());
1605
1606 if (distortImagePoints)
1607 {
1608 if constexpr (tUseBorderDistortionIfOutside)
1609 {
1610 const VectorT2<T> clampedNormalizedImagePoint(minmax(-principalPointX() * inverseFocalLengthX(), normalizedImagePoint.x(), (T(width_) - principalPointX()) * inverseFocalLengthX()),
1611 minmax(-principalPointY() * inverseFocalLengthY(), normalizedImagePoint.y(), (T(height_) - principalPointY()) * inverseFocalLengthY()));
1612
1613 // if the camera does not provide a tangential distortion
1614 if (tangentialDistortion_.first == 0 && tangentialDistortion_.second == 0)
1615 {
1616 const T sqr = clampedNormalizedImagePoint.sqr();
1617 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1618
1619 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor) * focalLengthX() + principalPointX(),
1620 (normalizedImagePoint.y() * radialDistortionFactor) * focalLengthY() + principalPointY());
1621 }
1622 else
1623 {
1624 ocean_assert(tangentialDistortion_.first != 0 || tangentialDistortion_.second != 0);
1625
1626 const T sqr = clampedNormalizedImagePoint.sqr();
1627 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1628
1629 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
1630 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
1631
1632 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
1633 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
1634
1635 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() + principalPointX(),
1636 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() + principalPointY());
1637 }
1638 }
1639 else
1640 {
1641 // if the camera does not provide a tangential distortion
1642 if (tangentialDistortion_.first == 0 && tangentialDistortion_.second == 0)
1643 {
1644 const T sqr = normalizedImagePoint.sqr();
1645 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1646
1647 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor) * focalLengthX() + principalPointX(),
1648 (normalizedImagePoint.y() * radialDistortionFactor) * focalLengthY() + principalPointY());
1649 }
1650 else
1651 {
1652 ocean_assert(tangentialDistortion_.first != 0 || tangentialDistortion_.second != 0);
1653
1654 const T sqr = normalizedImagePoint.sqr();
1655 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1656
1657 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * normalizedImagePoint.x() * normalizedImagePoint.y()
1658 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.x()));
1659
1660 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.y()))
1661 + tangentialDistortion_.second * 2 * normalizedImagePoint.x() * normalizedImagePoint.y();
1662
1663 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() + principalPointX(),
1664 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() + principalPointY());
1665 }
1666 }
1667 }
1668 else
1669 {
1670 return VectorT2<T>(normalizedImagePoint.x() * focalLengthX() + principalPointX(), normalizedImagePoint.y() * focalLengthY() + principalPointY());
1671 }
1672}
1673
1674template <typename T>
1675template <bool tUseBorderDistortionIfOutside>
1676void PinholeCameraT<T>::normalizedImagePoints2imagePoints(const VectorT2<T>* normalizedImagePoints, const size_t numberNormalizedImagePoints, const bool distortImagePoints, VectorT2<T>* imagePoints) const
1677{
1678 ocean_assert(numberNormalizedImagePoints == 0u || (normalizedImagePoints && imagePoints));
1679
1680 const T leftClamping = -principalPointX() * inverseFocalLengthX();
1681 const T rightClamping = (T(width_) - principalPointX()) * inverseFocalLengthX();
1682 const T topClamping = -principalPointY() * inverseFocalLengthY();
1683 const T bottomClamping = (T(height_) - principalPointY()) * inverseFocalLengthY();
1684
1685 if (distortImagePoints && hasDistortionParameters())
1686 {
1687 // if the camera does not provide a tangential distortion
1688 if (tangentialDistortion_.first == 0 && tangentialDistortion_.second == 0)
1689 {
1690 T sqr, radialDistortionFactor;
1691
1692 for (unsigned int n = 0u; n < numberNormalizedImagePoints; ++n)
1693 {
1694 const VectorT2<T>& normalizedImagePoint(normalizedImagePoints[n]);
1695
1696 if constexpr (tUseBorderDistortionIfOutside)
1697 {
1698 const VectorT2<T> clampedNormalizedImagePoint(minmax(leftClamping, normalizedImagePoint.x(), rightClamping), minmax(topClamping, normalizedImagePoint.y(), bottomClamping));
1699
1700 sqr = clampedNormalizedImagePoint.sqr();
1701 radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1702
1703 imagePoints[n].x() = (normalizedImagePoint.x() * radialDistortionFactor) * focalLengthX() + principalPointX();
1704 imagePoints[n].y() = (normalizedImagePoint.y() * radialDistortionFactor) * focalLengthY() + principalPointY();
1705 }
1706 else
1707 {
1708 sqr = normalizedImagePoint.sqr();
1709 radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1710
1711 imagePoints[n].x() = (normalizedImagePoint.x() * radialDistortionFactor) * focalLengthX() + principalPointX();
1712 imagePoints[n].y() = (normalizedImagePoint.y() * radialDistortionFactor) * focalLengthY() + principalPointY();
1713 }
1714 }
1715 }
1716 else
1717 {
1718 ocean_assert(tangentialDistortion_.first != 0 || tangentialDistortion_.second != 0);
1719 T sqr, radialDistortionFactor, tangentialDistortionCorrectionX, tangentialDistortionCorrectionY;
1720
1721 for (unsigned int n = 0u; n < numberNormalizedImagePoints; ++n)
1722 {
1723 const VectorT2<T>& normalizedImagePoint(normalizedImagePoints[n]);
1724
1725 if constexpr (tUseBorderDistortionIfOutside)
1726 {
1727 const VectorT2<T> clampedNormalizedImagePoint(minmax(leftClamping, normalizedImagePoint.x(), rightClamping), minmax(topClamping, normalizedImagePoint.y(), bottomClamping));
1728
1729 sqr = clampedNormalizedImagePoint.sqr();
1730 radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1731
1732 tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
1733 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
1734
1735 tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
1736 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
1737
1738 imagePoints[n].x() = (normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() + principalPointX();
1739 imagePoints[n].y() = (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() + principalPointY();
1740 }
1741 else
1742 {
1743 sqr = normalizedImagePoint.sqr();
1744 radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1745
1746 tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * normalizedImagePoint.x() * normalizedImagePoint.y()
1747 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.x()));
1748
1749 tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.y()))
1750 + tangentialDistortion_.second * 2 * normalizedImagePoint.x() * normalizedImagePoint.y();
1751
1752 imagePoints[n].x() = (normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() + principalPointX();
1753 imagePoints[n].y() = (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() + principalPointY();
1754 }
1755 }
1756 }
1757 }
1758 else
1759 {
1760 for (unsigned int n = 0u; n < numberNormalizedImagePoints; ++n)
1761 {
1762 const VectorT2<T>& normalizedImagePoint(normalizedImagePoints[n]);
1763
1764 imagePoints[n].x() = normalizedImagePoint.x() * focalLengthX() + principalPointX();
1765 imagePoints[n].y() = normalizedImagePoint.y() * focalLengthY() + principalPointY();
1766 }
1767 }
1768}
1769
1770template <typename T>
1771template <bool tUseBorderDistortionIfOutside>
1772inline VectorT2<T> PinholeCameraT<T>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>& worldObjectPoint, const bool distortImagePoint, const T zoom) const
1773{
1774 ocean_assert(world_T_camera.isValid() && zoom > NumericT<T>::eps());
1775 return projectToImageIF<tUseBorderDistortionIfOutside>(CameraT<T>::standard2InvertedFlipped(world_T_camera), worldObjectPoint, distortImagePoint, zoom);
1776}
1777
1778template <typename T>
1779template <bool tUseBorderDistortionIfOutside>
1780inline BoxT2<T> PinholeCameraT<T>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const BoxT3<T>& worldObjectBox, const bool distortImagePoint, const T zoom) const
1781{
1782 ocean_assert(world_T_camera.isValid() && worldObjectBox.isValid() && zoom > NumericT<T>::eps());
1783 return projectToImageIF<tUseBorderDistortionIfOutside>(CameraT<T>::standard2InvertedFlipped(world_T_camera), worldObjectBox, distortImagePoint, zoom);
1784}
1785
1786template <typename T>
1787template <bool tUseBorderDistortionIfOutside>
1788inline TriangleT2<T> PinholeCameraT<T>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const TriangleT3<T>& worldObjectTriangle, const bool distortImagePoint, const T zoom) const
1789{
1790 ocean_assert(world_T_camera.isValid() && worldObjectTriangle.isValid() && zoom > NumericT<T>::eps());
1791 return projectToImageIF<tUseBorderDistortionIfOutside>(CameraT<T>::standard2InvertedFlipped(world_T_camera), worldObjectTriangle, distortImagePoint, zoom);
1792}
1793
1794template <typename T>
1795template <bool tUseBorderDistortionIfOutside>
1796inline void PinholeCameraT<T>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const VectorT3<T>* worldObjectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T zoom) const
1797{
1798 ocean_assert(world_T_camera.isValid() && zoom > NumericT<T>::eps());
1799 ocean_assert(numberObjectPoints == 0u || (worldObjectPoints != nullptr && imagePoints != nullptr));
1800
1801 return projectToImageIF<tUseBorderDistortionIfOutside>(CameraT<T>::standard2InvertedFlipped(world_T_camera), worldObjectPoints, numberObjectPoints, distortImagePoints, imagePoints, zoom);
1802}
1803
1804template <typename T>
1805template <bool tUseBorderDistortionIfOutside>
1806inline LineT2<T> PinholeCameraT<T>::projectToImage(const HomogenousMatrixT4<T>& world_T_camera, const LineT3<T>& worldLine, const bool distortProjectedLine, const T zoom) const
1807{
1808 ocean_assert(world_T_camera.isValid() && zoom > NumericT<T>::eps());
1809 ocean_assert(worldLine.isValid());
1810
1811 return projectToImageIF<tUseBorderDistortionIfOutside>(CameraT<T>::standard2InvertedFlipped(world_T_camera), worldLine, distortProjectedLine, zoom);
1812}
1813
1814template <typename T>
1815template <bool tUseBorderDistortionIfOutside>
1816VectorT2<T> PinholeCameraT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, const bool distortImagePoint, const T zoom) const
1817{
1818 ocean_assert(flippedCamera_T_world.isValid() && zoom > NumericT<T>::eps());
1819
1820 const VectorT3<T> transformedObjectPoint(flippedCamera_T_world * worldObjectPoint);
1821
1822 ocean_assert(NumericT<T>::isNotEqualEps(transformedObjectPoint.z()));
1823 const T factor = T(1) / transformedObjectPoint.z();
1824
1825 const VectorT2<T> normalizedImagePoint(transformedObjectPoint.x() * factor, transformedObjectPoint.y() * factor);
1826
1827 if (!distortImagePoint)
1828 {
1829 return VectorT2<T>(normalizedImagePoint.x() * focalLengthX() * zoom + principalPointX(), normalizedImagePoint.y() * focalLengthY() * zoom + principalPointY());
1830 }
1831
1832 if constexpr (tUseBorderDistortionIfOutside)
1833 {
1834 const T invZoom = T(1) / zoom;
1835
1836 const VectorT2<T> clampedNormalizedImagePoint(minmax(-principalPointX() * inverseFocalLengthX() * invZoom, normalizedImagePoint.x(), (T(width_) - principalPointX()) * inverseFocalLengthX() * invZoom),
1837 minmax(-principalPointY() * inverseFocalLengthY() * invZoom, normalizedImagePoint.y(), (T(height_) - principalPointY()) * inverseFocalLengthY() * invZoom));
1838
1839 const T sqr = clampedNormalizedImagePoint.sqr();
1840
1841 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1842
1843 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
1844 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
1845
1846 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
1847 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
1848
1849 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
1850 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
1851 }
1852 else
1853 {
1854 const T sqr = normalizedImagePoint.sqr();
1855
1856 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1857
1858 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * normalizedImagePoint.x() * normalizedImagePoint.y()
1859 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.x()));
1860
1861 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.y()))
1862 + tangentialDistortion_.second * 2 * normalizedImagePoint.x() * normalizedImagePoint.y();
1863
1864 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
1865 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
1866 }
1867}
1868
1869template <typename T>
1870template <bool tUseBorderDistortionIfOutside>
1871BoxT2<T> PinholeCameraT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const BoxT3<T>& worldObjectBox, const bool distortImagePoint, const T zoom) const
1872{
1873 ocean_assert(flippedCamera_T_world.isValid() && zoom > NumericT<T>::eps());
1874 ocean_assert(worldObjectBox.isValid());
1875
1876 VectorT3<T> boxObjectCorners[8];
1877 const unsigned int numberBoxImagePoints = worldObjectBox.corners(boxObjectCorners);
1878
1879 BoxT2<T> result;
1880 for (unsigned int n = 0; n < numberBoxImagePoints; ++n)
1881 {
1882 result += projectToImageIF<tUseBorderDistortionIfOutside>(flippedCamera_T_world, boxObjectCorners[n], distortImagePoint, zoom);
1883 }
1884
1885 return result;
1886}
1887
1888template <typename T>
1889template <bool tUseBorderDistortionIfOutside>
1890TriangleT2<T> PinholeCameraT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const TriangleT3<T>& worldObjectTriangle, const bool distortImagePoint, const T zoom) const
1891{
1892 ocean_assert(flippedCamera_T_world.isValid() && zoom > NumericT<T>::eps());
1893 ocean_assert(worldObjectTriangle.isValid());
1894
1895 return TriangleT2<T>(projectToImageIF<tUseBorderDistortionIfOutside>(flippedCamera_T_world, worldObjectTriangle.point0(), distortImagePoint, zoom),
1896 projectToImageIF<tUseBorderDistortionIfOutside>(flippedCamera_T_world, worldObjectTriangle.point1(), distortImagePoint, zoom),
1897 projectToImageIF<tUseBorderDistortionIfOutside>(flippedCamera_T_world, worldObjectTriangle.point2(), distortImagePoint, zoom));
1898}
1899
1900template <typename T>
1901template <bool tUseBorderDistortionIfOutside>
1902VectorT2<T> PinholeCameraT<T>::projectToImageIF(const VectorT2<T>& normalizedObjectPoint, const bool distortImagePoint, const T zoom) const
1903{
1904 ocean_assert(zoom > NumericT<T>::eps());
1905
1906 if (distortImagePoint)
1907 {
1908 if constexpr (tUseBorderDistortionIfOutside)
1909 {
1910 const T invZoom = T(1) / zoom;
1911
1912 const VectorT2<T> clampedNormalizedImagePoint(minmax(-principalPointX() * inverseFocalLengthX() * invZoom, normalizedObjectPoint.x(), (T(width_) - principalPointX()) * inverseFocalLengthX() * invZoom),
1913 minmax(-principalPointY() * inverseFocalLengthY() * invZoom, normalizedObjectPoint.y(), (T(height_) - principalPointY()) * inverseFocalLengthY() * invZoom));
1914
1915 const T sqr = clampedNormalizedImagePoint.sqr();
1916
1917 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1918
1919 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
1920 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
1921
1922 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
1923 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
1924
1925 return VectorT2<T>((normalizedObjectPoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
1926 (normalizedObjectPoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
1927 }
1928 else
1929 {
1930 const T sqr = normalizedObjectPoint.sqr();
1931
1932 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1933
1934 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * normalizedObjectPoint.x() * normalizedObjectPoint.y()
1935 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(normalizedObjectPoint.x()));
1936
1937 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(normalizedObjectPoint.y()))
1938 + tangentialDistortion_.second * 2 * normalizedObjectPoint.x() * normalizedObjectPoint.y();
1939
1940 return VectorT2<T>((normalizedObjectPoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
1941 (normalizedObjectPoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
1942 }
1943 }
1944 else
1945 {
1946 return VectorT2<T>(normalizedObjectPoint.x() * focalLengthX() * zoom + principalPointX(), normalizedObjectPoint.y() * focalLengthY() * zoom + principalPointY());
1947 }
1948}
1949
1950template <typename T>
1951template <bool tUseBorderDistortionIfOutside>
1952void PinholeCameraT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>* worldObjectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T zoom) const
1953{
1954 ocean_assert(flippedCamera_T_world.isValid() && zoom > NumericT<T>::eps());
1955 ocean_assert((worldObjectPoints != nullptr && imagePoints != nullptr) || numberObjectPoints == 0u);
1956
1957 if (distortImagePoints && hasDistortionParameters())
1958 {
1959 const T invZoom = T(1) / zoom;
1960
1961 const T leftClamping = -principalPointX() * inverseFocalLengthX() * invZoom;
1962 const T rightClamping = (T(width_) - principalPointX()) * inverseFocalLengthX() * invZoom;
1963 const T topClamping = -principalPointY() * inverseFocalLengthY() * invZoom;
1964 const T bottomClamping = (T(height_) - principalPointY()) * inverseFocalLengthY() * invZoom;
1965
1966 // if the camera does not provide tangential distortion
1967 if (tangentialDistortion_.first == 0 && tangentialDistortion_.second == 0)
1968 {
1969 for (size_t n = 0; n < numberObjectPoints; ++n)
1970 {
1971 const VectorT3<T> objectPoint(flippedCamera_T_world * worldObjectPoints[n]);
1972
1973 ocean_assert(NumericT<T>::isNotEqualEps(objectPoint.z()));
1974 const T factor = 1 / objectPoint.z();
1975
1976 const VectorT2<T> normalizedImagePoint(objectPoint.x() * factor, objectPoint.y() * factor);
1977
1978 if constexpr (tUseBorderDistortionIfOutside)
1979 {
1980 const VectorT2<T> clampedNormalizedImagePoint(minmax(leftClamping, normalizedImagePoint.x(), rightClamping), minmax(topClamping, normalizedImagePoint.y(), bottomClamping));
1981
1982 const T sqr = clampedNormalizedImagePoint.sqr();
1983 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1984
1985 *imagePoints = VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor) * focalLengthX() * zoom + principalPointX(),
1986 (normalizedImagePoint.y() * radialDistortionFactor) * focalLengthY() * zoom + principalPointY());
1987 }
1988 else
1989 {
1990 const T sqr = normalizedImagePoint.sqr();
1991 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
1992
1993 *imagePoints = VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor) * focalLengthX() * zoom + principalPointX(),
1994 (normalizedImagePoint.y() * radialDistortionFactor) * focalLengthY() * zoom + principalPointY());
1995 }
1996
1997 ++imagePoints;
1998 }
1999 }
2000 else
2001 {
2002 ocean_assert(tangentialDistortion_.first != 0 || tangentialDistortion_.second != 0);
2003
2004 for (size_t n = 0; n < numberObjectPoints; ++n)
2005 {
2006 const VectorT3<T> objectPoint(flippedCamera_T_world * worldObjectPoints[n]);
2007
2008 ocean_assert(NumericT<T>::isNotEqualEps(objectPoint.z()));
2009 const T factor = 1 / objectPoint.z();
2010
2011 const VectorT2<T> normalizedImagePoint(objectPoint.x() * factor, objectPoint.y() * factor);
2012
2013 if constexpr (tUseBorderDistortionIfOutside)
2014 {
2015 const VectorT2<T> clampedNormalizedImagePoint(minmax(leftClamping, normalizedImagePoint.x(), rightClamping), minmax(topClamping, normalizedImagePoint.y(), bottomClamping));
2016
2017 const T sqr = clampedNormalizedImagePoint.sqr();
2018 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
2019
2020 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
2021 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
2022
2023 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
2024 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
2025
2026 *imagePoints = VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
2027 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
2028 }
2029 else
2030 {
2031 const T sqr = normalizedImagePoint.sqr();
2032 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
2033
2034 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * normalizedImagePoint.x() * normalizedImagePoint.y()
2035 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.x()));
2036
2037 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.y()))
2038 + tangentialDistortion_.second * 2 * normalizedImagePoint.x() * normalizedImagePoint.y();
2039
2040 *imagePoints = VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
2041 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
2042 }
2043
2044 ++imagePoints;
2045 }
2046 }
2047 }
2048 else
2049 {
2050 // create one transformation matrix covering the entire pipeline (transformation and then projection)
2051 const HomogenousMatrixT4<T> transformationIF(transformationMatrixIF(flippedCamera_T_world, zoom));
2052 ocean_assert(transformationIF.isValid());
2053
2054 for (size_t n = 0; n < numberObjectPoints; ++n)
2055 {
2056 const VectorT3<T> transformedObjectPoint(transformationIF * worldObjectPoints[n]);
2057
2058 ocean_assert(NumericT<T>::isNotEqualEps(transformedObjectPoint.z()));
2059 const T factor = 1 / transformedObjectPoint.z();
2060
2061 *imagePoints++ = VectorT2<T>(transformedObjectPoint.x() * factor, transformedObjectPoint.y() * factor);
2062 }
2063 }
2064}
2065
2066template <typename T>
2067template <bool tUseBorderDistortionIfOutside>
2068LineT2<T> PinholeCameraT<T>::projectToImageIF(const HomogenousMatrixT4<T>& flippedCamera_T_world, const LineT3<T>& worldLine, const bool distortProjectedLine, const T zoom) const
2069{
2070 ocean_assert(flippedCamera_T_world.isValid() && zoom > NumericT<T>::eps());
2071 ocean_assert(worldLine.isValid());
2072
2073 const VectorT2<T> firstImagePoint(projectToImageIF<tUseBorderDistortionIfOutside>(flippedCamera_T_world, worldLine.point(), distortProjectedLine, zoom));
2074 const VectorT2<T> secondImagePoint(projectToImageIF<tUseBorderDistortionIfOutside>(flippedCamera_T_world, worldLine.point(10), distortProjectedLine, zoom));
2075
2076 if (firstImagePoint == secondImagePoint)
2077 {
2078 return LineT2<T>();
2079 }
2080
2081 return LineT2<T>(firstImagePoint, (secondImagePoint - firstImagePoint).normalized());
2082}
2083
2084template <typename T>
2085inline VectorT2<T> PinholeCameraT<T>::projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const VectorT3<T>& objectPoint, const bool distortImagePoint, const T dampingFactor, const T zoom) const
2086{
2087 ocean_assert(extrinsic.isValid() && dampingFactor >= 0 && zoom > NumericT<T>::eps());
2088 return projectToImageDampedIF(CameraT<T>::standard2InvertedFlipped(extrinsic), objectPoint, distortImagePoint, dampingFactor, zoom);
2089}
2090
2091template <typename T>
2092inline BoxT2<T> PinholeCameraT<T>::projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const BoxT3<T>& objectBox, const bool distortImagePoint, const T dampingFactor, const T zoom) const
2093{
2094 ocean_assert(extrinsic.isValid() && dampingFactor >= 0 && zoom > NumericT<T>::eps());
2095 return projectToImageDampedIF(CameraT<T>::standard2InvertedFlipped(extrinsic), objectBox, distortImagePoint, dampingFactor, zoom);
2096}
2097
2098template <typename T>
2099inline TriangleT2<T> PinholeCameraT<T>::projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const TriangleT3<T>& objectTriangle, const bool distortImagePoint, const T dampingFactor, const T zoom) const
2100{
2101 ocean_assert(extrinsic.isValid() && dampingFactor >= 0 && zoom > NumericT<T>::eps());
2102 return projectToImageDampedIF(CameraT<T>::standard2InvertedFlipped(extrinsic), objectTriangle, distortImagePoint, dampingFactor, zoom);
2103}
2104
2105template <typename T>
2106inline void PinholeCameraT<T>::projectToImageDamped(const HomogenousMatrixT4<T>& extrinsic, const VectorT3<T>* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2<T>* imagePoints, const T dampingFactor, const T zoom) const
2107{
2108 ocean_assert(extrinsic.isValid() && dampingFactor >= 0 && zoom > NumericT<T>::eps());
2109 ocean_assert(numberObjectPoints == 0u || (objectPoints && imagePoints));
2110
2111 return projectToImageDampedIF(CameraT<T>::standard2InvertedFlipped(extrinsic), objectPoints, numberObjectPoints, distortImagePoints, imagePoints, dampingFactor, zoom);
2112}
2113
2114template <typename T>
2115template <bool tDistortImagePoint, bool tUseBorderDistortionIfOutside>
2116inline VectorT2<T> PinholeCameraT<T>::projectToImageIF(const HomogenousMatrixT4<T>& iFlippedExtrinsic, const VectorT3<T>& objectPoint, const T zoom) const
2117{
2118 ocean_assert(iFlippedExtrinsic.isValid() && zoom > NumericT<T>::eps());
2119
2120 const VectorT3<T> transformedObjectPoint(iFlippedExtrinsic * objectPoint);
2121
2122 ocean_assert(NumericT<T>::isNotEqualEps(transformedObjectPoint.z()));
2123 const T factor = T(1) / transformedObjectPoint.z();
2124 ocean_assert(NumericT<T>::isNotEqualEps(factor));
2125
2126 const VectorT2<T> normalizedImagePoint(transformedObjectPoint.x() * factor, transformedObjectPoint.y() * factor);
2127
2128 if (!tDistortImagePoint)
2129 {
2130 return VectorT2<T>(normalizedImagePoint.x() * focalLengthX() * zoom + principalPointX(), normalizedImagePoint.y() * focalLengthY() * zoom + principalPointY());
2131 }
2132
2133 if constexpr (tUseBorderDistortionIfOutside)
2134 {
2135 const T invZoom = T(1) / zoom;
2136
2137 const VectorT2<T> clampedNormalizedImagePoint(minmax(-principalPointX() * inverseFocalLengthX() * invZoom, normalizedImagePoint.x(), (T(width_) - principalPointX()) * inverseFocalLengthX() * invZoom),
2138 minmax(-principalPointY() * inverseFocalLengthY() * invZoom, normalizedImagePoint.y(), (T(height_) - principalPointY()) * inverseFocalLengthY() * invZoom));
2139
2140 const T sqr = clampedNormalizedImagePoint.sqr();
2141
2142 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
2143
2144 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
2145 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
2146
2147 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
2148 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
2149
2150 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
2151 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
2152 }
2153 else
2154 {
2155 const T sqr = normalizedImagePoint.sqr();
2156
2157 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
2158
2159 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * normalizedImagePoint.x() * normalizedImagePoint.y()
2160 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.x()));
2161
2162 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(normalizedImagePoint.y()))
2163 + tangentialDistortion_.second * 2 * normalizedImagePoint.x() * normalizedImagePoint.y();
2164
2165 return VectorT2<T>((normalizedImagePoint.x() * radialDistortionFactor + tangentialDistortionCorrectionX) * focalLengthX() * zoom + principalPointX(),
2166 (normalizedImagePoint.y() * radialDistortionFactor + tangentialDistortionCorrectionY) * focalLengthY() * zoom + principalPointY());
2167 }
2168}
2169
2170template <typename T>
2171inline VectorT3<T> PinholeCameraT<T>::vector(const VectorT2<T>& position, const bool makeUnitVector) const
2172{
2173 /**
2174 * Ray determination: with object point (X, Y, Z) and image point (x, y, z) -> (x', y')
2175 *
2176 * x = Fx * X + mx * Z
2177 * y = Fy * Y + my * Z
2178 * z = Z
2179 *
2180 * x' = x / z = (Fx * X) / Z + mx
2181 * y' = y / z = (Fy * Y) / Z + my
2182 *
2183 * Inverse calculation:
2184 * (x' - mx) / Fx = X / Z
2185 * (y' - my) / Fy = Y / Z
2186 *
2187 * Using a distance of Z:= 1 results in:
2188 * X = (x' - mx) / Fx
2189 * Y = (y' - my) / Fy
2190 */
2191
2192 /**
2193 * The calculation is identical to the multiplication between the inverted camera matrix and the position vector.
2194 *
2195 * Inverse camera matrix:
2196 * | 1/Fx 0 -mx/Fx |<br>
2197 * | 0 1/Fy -my/Fy |<br>
2198 * | 0 0 1 |<br>
2199 */
2200
2201#ifdef OCEAN_DEBUG
2202
2203 const VectorT3<T> testVector(VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX(), (position(1) - principalPointY()) * -inverseFocalLengthY(), -1).normalized());
2204 ocean_assert(position.isEqual(projectToImage<false>(HomogenousMatrixT4<T>(true), testVector, false), T(0.01)));
2205 ocean_assert(NumericT<T>::isWeakEqual(testVector.length(), 1));
2206
2207#endif
2208
2209 if (makeUnitVector)
2210 {
2211 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX(), (position(1) - principalPointY()) * -inverseFocalLengthY(), -1).normalized();
2212 }
2213 else
2214 {
2215 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX(), (position(1) - principalPointY()) * -inverseFocalLengthY(), -1);
2216 }
2217}
2218
2219template <typename T>
2220inline VectorT3<T> PinholeCameraT<T>::vector(const VectorT2<T>& position, const T zoom, const bool makeUnitVector) const
2221{
2222 ocean_assert(zoom > NumericT<T>::eps());
2223
2224 /**
2225 * Ray determination: with object point (X, Y, Z), zoom s, and image point (x, y, z) -> (x', y')
2226 *
2227 * x = s * Fx * X + mx * Z
2228 * y = s * Fy * Y + my * Z
2229 * z = Z
2230 *
2231 * x' = x / z = (s * Fx * X) / Z + mx
2232 * y' = y / z = (s * Fy * Y) / Z + my
2233 *
2234 * Inverse calculation:
2235 * (x' - mx) / (s * Fx) = X / Z
2236 * (y' - my) / (s * Fy) = Y / Z
2237 *
2238 * Using a distance of Z:= 1 results in:
2239 * X = (x' - mx) / (s * Fx)
2240 * Y = (y' - my) / (s * Fy)
2241 */
2242
2243 /**
2244 * The calculation is identical to the multiplication between the inverted camera matrix and the position vector.
2245 *
2246 * Inverse camera matrix:
2247 * | 1/(s Fx) 0 -mx/(s Fx) |<br>
2248 * | 0 1/(s Fy) -my/(s Fy) |<br>
2249 * | 0 0 1 |<br>
2250 */
2251
2252 const T invZoom = T(1) / zoom;
2253
2254#ifdef OCEAN_DEBUG
2255
2256 const VectorT3<T> testVector(VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, (position(1) - principalPointY()) * -inverseFocalLengthY() * invZoom, -1).normalized());
2257 ocean_assert(position.isEqual(projectToImage<false>(HomogenousMatrixT4<T>(true), testVector, false, zoom), T(0.01)));
2258 ocean_assert(NumericT<T>::isWeakEqual(testVector.length(), 1));
2259
2260#endif
2261
2262 if (makeUnitVector)
2263 {
2264 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, (position(1) - principalPointY()) * -inverseFocalLengthY() * invZoom, -1).normalized();
2265 }
2266 else
2267 {
2268 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, (position(1) - principalPointY()) * -inverseFocalLengthY() * invZoom, -1);
2269 }
2270}
2271
2272template <typename T>
2273inline VectorT3<T> PinholeCameraT<T>::vectorToPlane(const VectorT2<T>& position, const T distance) const
2274{
2275 /**
2276 * Ray determination: with object point (X, Y, Z) and image point (x, y, z) -> (x / z, y / z, 1) -> (x', y')
2277 *
2278 * x = Fx * X + mx * Z
2279 * y = Fy * Y + my * Z
2280 * z = Z
2281 *
2282 * x' = x / z = (Fx * X) / Z + mx
2283 * y' = y / z = (Fy * Y) / Z + my
2284 *
2285 * Inverse calculation:
2286 * (x' - mx) / Fx = X / Z
2287 * (y' - my) / Fy = Y / Z
2288 */
2289
2290#ifdef OCEAN_DEBUG
2291
2292 const VectorT3<T> testVector(distance * (position(0) - principalPointX()) * inverseFocalLengthX(), distance * (position(1) - principalPointY()) * -inverseFocalLengthY(), -distance);
2293 ocean_assert(position.isEqual(projectToImage<false>(HomogenousMatrixT4<T>(true), testVector, false), T(0.01)));
2294 ocean_assert(NumericT<T>::isWeakEqual(testVector * VectorT3<T>(0, 0, -1), distance));
2295
2296#endif
2297
2298 return VectorT3<T>(distance * (position(0) - principalPointX()) * inverseFocalLengthX(), distance * (position(1) - principalPointY()) * -inverseFocalLengthY(), -distance);
2299}
2300
2301template <typename T>
2302inline VectorT3<T> PinholeCameraT<T>::vectorToPlane(const VectorT2<T>& position, const T distance, const T zoom) const
2303{
2304 ocean_assert(zoom > NumericT<T>::eps());
2305
2306 /**
2307 * Ray determination: with object point (X, Y, Z) and image point (x, y, z) -> (x / z, y / z, 1) -> (x', y')
2308 *
2309 * x = Fx * X + mx * Z
2310 * y = Fy * Y + my * Z
2311 * z = Z
2312 *
2313 * x' = x / z = (Fx * X) / Z + mx
2314 * y' = y / z = (Fy * Y) / Z + my
2315 *
2316 * Inverse calculation:
2317 * (x' - mx) / Fx = X / Z
2318 * (y' - my) / Fy = Y / Z
2319 */
2320
2321 const T invZoom = T(1) / zoom;
2322
2323#ifdef OCEAN_DEBUG
2324
2325 const VectorT3<T> testVector(distance * (position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, distance * (position(1) - principalPointY()) * -inverseFocalLengthY() * invZoom, -distance);
2326 ocean_assert(position.isEqual(projectToImage<false>(HomogenousMatrixT4<T>(true), testVector, false, zoom), T(0.01)));
2327 ocean_assert(NumericT<T>::isWeakEqual(testVector * VectorT3<T>(0, 0, -1), distance));
2328
2329#endif
2330
2331 return VectorT3<T>(distance * (position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, distance * (position(1) - principalPointY()) * -inverseFocalLengthY() * invZoom, -distance);
2332}
2333
2334template <typename T>
2335inline VectorT3<T> PinholeCameraT<T>::vectorIF(const VectorT2<T>& position, const bool makeUnitVector) const
2336{
2337#ifdef OCEAN_DEBUG
2338
2339 const VectorT3<T> testVector(VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX(), (position(1) - principalPointY()) * inverseFocalLengthY(), 1).normalized());
2340 ocean_assert((std::is_same<T, float>::value) || position.isEqual(projectToImageIF<false>(HomogenousMatrixT4<T>(true), testVector, false), T(0.01)));
2341#endif
2342
2343 if (makeUnitVector)
2344 {
2345 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX(), (position(1) - principalPointY()) * inverseFocalLengthY(), 1).normalized();
2346 }
2347 else
2348 {
2349 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX(), (position(1) - principalPointY()) * inverseFocalLengthY(), 1);
2350 }
2351}
2352
2353template <typename T>
2354inline VectorT3<T> PinholeCameraT<T>::vectorIF(const VectorT2<T>& position, const T zoom, const bool makeUnitVector) const
2355{
2356 ocean_assert(zoom > NumericT<T>::eps());
2357
2358 const T invZoom = T(1) / zoom;
2359
2360#ifdef OCEAN_DEBUG
2361
2362 const VectorT3<T> testVector(VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, (position(1) - principalPointY()) * inverseFocalLengthY() * invZoom, 1).normalized());
2363 ocean_assert(position.isEqual(projectToImageIF<false>(HomogenousMatrixT4<T>(true), testVector, false, zoom), T(0.01)));
2364
2365#endif
2366
2367 if (makeUnitVector)
2368 {
2369 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, (position(1) - principalPointY()) * inverseFocalLengthY() * invZoom, 1).normalized();
2370 }
2371 else
2372 {
2373 return VectorT3<T>((position(0) - principalPointX()) * inverseFocalLengthX() * invZoom, (position(1) - principalPointY()) * inverseFocalLengthY() * invZoom, 1);
2374 }
2375}
2376
2377template <typename T>
2378inline LineT3<T> PinholeCameraT<T>::ray(const VectorT2<T>& position, const HomogenousMatrixT4<T>& world_T_camera, const T zoom) const
2379{
2380 ocean_assert(world_T_camera.isValid() && zoom > NumericT<T>::eps());
2381 ocean_assert((std::is_same<T, float>::value) || NumericT<T>::rad2deg((world_T_camera.rotation() * vector(position, zoom)).angle(world_T_camera.rotationMatrix(vector(position, zoom)))) <= T(0.01));
2382 ocean_assert(NumericT<T>::isEqual((world_T_camera.rotation() * vector(position, zoom)).length(), 1));
2383
2384 return LineT3<T>(world_T_camera.translation(), world_T_camera.rotationMatrix(vector(position, zoom)));
2385}
2386
2387template <typename T>
2388inline LineT3<T> PinholeCameraT<T>::ray(const VectorT2<T>& position, const VectorT3<T>& world_t_camera, const QuaternionT<T>& world_Q_camera, const T zoom) const
2389{
2390 ocean_assert(world_Q_camera.isValid() && zoom >= NumericT<T>::eps());
2391
2392 return LineT3<T>(world_t_camera, world_Q_camera * vector(position, zoom));
2393}
2394
2395template <typename T>
2396inline void PinholeCameraT<T>::calculateInverseIntrinsic()
2397{
2398 /**
2399 * The calculation is identical to the multiplication between the inverted camera matrix and the position vector.
2400 *
2401 * Inverse camera matrix:
2402 * | 1/Fx 0 -mx/Fx |<br>
2403 * | 0 1/Fy -my/Fy |<br>
2404 * | 0 0 1 |<br>
2405 */
2406
2407 ocean_assert(NumericT<T>::isNotEqualEps(focalLengthX()));
2408 ocean_assert(NumericT<T>::isNotEqualEps(focalLengthY()));
2409
2410 const T inverseFocalLengthX = T(1) / focalLengthX();
2411 const T inverseFocalLengthY = T(1) / focalLengthY();
2412
2413 invertedIntrinsics_(0, 0) = inverseFocalLengthX;
2414 invertedIntrinsics_(1, 1) = inverseFocalLengthY;
2415 invertedIntrinsics_(0, 2) = - principalPointX() * inverseFocalLengthX;
2416 invertedIntrinsics_(1, 2) = - principalPointY() * inverseFocalLengthY;
2417 invertedIntrinsics_(2, 2) = 1;
2418
2419 ocean_assert(invertedIntrinsics_(1, 0) == 0);
2420 ocean_assert(invertedIntrinsics_(2, 0) == 0);
2421 ocean_assert(invertedIntrinsics_(0, 1) == 0);
2422 ocean_assert(invertedIntrinsics_(2, 1) == 0);
2423}
2424
2425template <typename T>
2426inline bool PinholeCameraT<T>::operator!=(const PinholeCameraT<T>& camera) const
2427{
2428 return !(*this == camera);
2429}
2430
2431template <typename T>
2432inline PinholeCameraT<T>::operator bool() const
2433{
2434 return isValid();
2435}
2436
2437template <typename T>
2438template <bool tUseBorderDistortionIfOutside>
2439VectorT2<T> PinholeCameraT<T>::distortNormalized(const VectorT2<T>& undistortedNormalized, const T invZoom) const
2440{
2441 ocean_assert(invZoom > NumericT<T>::eps());
2442
2443 if (hasDistortionParameters())
2444 {
2445 if constexpr (tUseBorderDistortionIfOutside)
2446 {
2447 const VectorT2<T> clampedNormalizedImagePoint(minmax(-principalPointX() * inverseFocalLengthX() * invZoom, undistortedNormalized.x(), (T(width_) - principalPointX()) * inverseFocalLengthX() * invZoom),
2448 minmax(-principalPointY() * inverseFocalLengthY() * invZoom, undistortedNormalized.y(), (T(height_) - principalPointY()) * inverseFocalLengthY() * invZoom));
2449
2450 const T sqr = clampedNormalizedImagePoint.sqr();
2451
2452 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
2453
2454 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y()
2455 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.x()));
2456
2457 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(clampedNormalizedImagePoint.y()))
2458 + tangentialDistortion_.second * 2 * clampedNormalizedImagePoint.x() * clampedNormalizedImagePoint.y();
2459
2460 return VectorT2<T>(undistortedNormalized.x() * radialDistortionFactor + tangentialDistortionCorrectionX,
2461 undistortedNormalized.y() * radialDistortionFactor + tangentialDistortionCorrectionY);
2462 }
2463 else
2464 {
2465 const T sqr = undistortedNormalized.sqr();
2466
2467 const T radialDistortionFactor = T(1) + radialDistortion_.first * sqr + radialDistortion_.second * NumericT<T>::sqr(sqr);
2468
2469 const T tangentialDistortionCorrectionX = tangentialDistortion_.first * 2 * undistortedNormalized.x() * undistortedNormalized.y()
2470 + tangentialDistortion_.second * (sqr + 2 * NumericT<T>::sqr(undistortedNormalized.x()));
2471
2472 const T tangentialDistortionCorrectionY = tangentialDistortion_.first * (sqr + 2 * NumericT<T>::sqr(undistortedNormalized.y()))
2473 + tangentialDistortion_.second * 2 * undistortedNormalized.x() * undistortedNormalized.y();
2474
2475 return VectorT2<T>(undistortedNormalized.x() * radialDistortionFactor + tangentialDistortionCorrectionX,
2476 undistortedNormalized.y() * radialDistortionFactor + tangentialDistortionCorrectionY);
2477 }
2478 }
2479 else
2480 {
2481 return undistortedNormalized;
2482 }
2483}
2484
2485}
2486
2487#endif // META_OCEAN_MATH_PINHOLE_CAMERA_H
Ocean::BoxT2
This class implements an axis aligned 2D box object.
Definition Box2.h:68
Ocean::BoxT3
This class implements an axis aligned 3D bounding box.
Definition Box3.h:67
Ocean::BoxT3::isValid
bool isValid() const
Returns whether the bounding box is valid.
Ocean::BoxT3::corners
unsigned int corners(VectorT3< T > *corners) const
Returns the corner positions of this box.
Ocean::CameraT
This class implements the base class for all cameras.
Definition Camera.h:54
Ocean::HomogenousMatrixT4
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
Ocean::HomogenousMatrixT4::rotationMatrix
SquareMatrixT3< T > rotationMatrix() const
Returns the rotation matrix of the transformation.
Definition HomogenousMatrix4.h:1493
Ocean::HomogenousMatrixT4::translation
VectorT3< T > translation() const
Returns the translation of the transformation.
Definition HomogenousMatrix4.h:1381
Ocean::HomogenousMatrixT4::isValid
bool isValid() const
Returns whether this matrix is a valid homogeneous transformation.
Definition HomogenousMatrix4.h:1806
Ocean::HomogenousMatrixT4::rotation
QuaternionT< T > rotation() const
Returns the rotation of the transformation as quaternion.
Definition HomogenousMatrix4.h:1388
Ocean::LineT2
This class implements an infinite line in 2D space.
Definition Line2.h:83
Ocean::LineT3
This class implements an infinite line in 3D space.
Definition Line3.h:68
Ocean::LineT3::isValid
bool isValid() const
Returns whether this line has valid parameters.
Definition Line3.h:301
Ocean::LineT3::point
const VectorT3< T > & point() const
Returns a point on the line.
Definition Line3.h:269
Ocean::LookupCenter2
This class implements a 2D lookup object with values at the bins' center positions defining the indiv...
Definition Lookup2.h:198
Ocean::NumericT
This class provides basic numeric functionalities.
Definition Numeric.h:57
Ocean::NumericT::abs
static T abs(const T value)
Returns the absolute value of a given value.
Definition Numeric.h:1220
Ocean::NumericT::sqr
static constexpr T sqr(const T value)
Returns the square of a given value.
Definition Numeric.h:1495
Ocean::PinholeCameraT::DistortionLookup
This class encapsulates a lookup table for camera distortion offsets allowing for faster un-distortio...
Definition PinholeCamera.h:154
Ocean::PinholeCameraT::DistortionLookup::DistortionLookup
DistortionLookup(const PinholeCameraT< T > &camera, const unsigned int binSize)
Creates an lookup object for a given camera.
Ocean::PinholeCameraT::DistortionLookup::undistortionOffset
VectorT2< T > undistortionOffset(const VectorT2< T > &distortedImagePoint) const
Returns the offset that needs to be added to an distorted image point so that it would be undistorted...
Definition PinholeCamera.h:1236
Ocean::PinholeCameraT::DistortionLookup::undistortedImagePointBicubic
VectorT2< T > undistortedImagePointBicubic(const VectorT2< T > &distortedImagePoint) const
Returns the undistorted image point for a given (distorted) image point (by application of a bicubic ...
Definition PinholeCamera.h:1243
Ocean::PinholeCameraT::DistortionLookup::undistortionOffsetBicubic
VectorT2< T > undistortionOffsetBicubic(const VectorT2< T > &distortedImagePoint) const
Returns the offset that needs to be added to an distorted image point so that it would be undistorted...
Definition PinholeCamera.h:1250
Ocean::PinholeCameraT::DistortionLookup::undistortedImagePoint
VectorT2< T > undistortedImagePoint(const VectorT2< T > &distortedImagePoint) const
Returns the undistorted image point for a given (distorted) image point (by application of a bilinear...
Definition PinholeCamera.h:1229
Ocean::PinholeCameraT::DistortionLookup::DistortionLookup
DistortionLookup()
Creates an invalid lookup object.
Definition PinholeCamera.h:1223
Ocean::PinholeCameraT::DistortionLookup::distortionLookupTable
LookupTable distortionLookupTable
The distortion lookup table.
Definition PinholeCamera.h:207
Ocean::PinholeCameraT::DistortionLookup::LookupTable
LookupCenter2< VectorT2< T >, T > LookupTable
Definition of a lookup table for 2D vectors.
Definition PinholeCamera.h:160
Ocean::PinholeCameraT
Definition of a pinhole camera model.
Definition PinholeCamera.h:114
Ocean::PinholeCameraT::projectToImageIF
VectorT2< T > projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &worldObjectPoint, const T zoom=T(1)) const
Projects a 3D object point to the 2D image plane of the camera by a given inverse camera pose.
Definition PinholeCamera.h:2116
Ocean::PinholeCameraT::setTangentialDistortion
void setTangentialDistortion(const DistortionPair &tangential)
Sets the tangential distortion parameters.
Definition PinholeCamera.h:1366
Ocean::PinholeCameraT::imagePoint2normalizedImagePoint
VectorT2< T > imagePoint2normalizedImagePoint(const VectorT2< T > &imagePoint, const bool undistortImagePoint) const
Calculates the normalized image point corresponding to a given (distorted) image point.
Definition PinholeCamera.h:1579
Ocean::PinholeCameraT::width
unsigned int width() const
Returns the width of the camera image.
Definition PinholeCamera.h:1300
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const unsigned int width, const unsigned int height, const T focalX, const T focalY, const T principalX, const T principalY, const DistortionPair &radial, const DistortionPair &tangential)
Creates a new PinholeCameraT<T> object by it's given intrinsic parameters.
Ocean::PinholeCameraT::projectToImageDamped
VectorT2< T > projectToImageDamped(const HomogenousMatrixT4< T > &extrinsic, const VectorT3< T > &objectPoint, const bool distortImagePoint, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a 3D object point to the 2D image plane of the camera by a given extrinsic camera matrix.
Definition PinholeCamera.h:2085
Ocean::PinholeCameraT::fovYBottom
T fovYBottom() const
Returns the bottom field of view in y direction.
Ocean::PinholeCameraT::isEqual
bool isEqual(const PinholeCameraT< T > &camera, const T eps=NumericT< T >::eps()) const
Returns whether two camera profiles are identical up to a given epsilon.
Ocean::PinholeCameraT::isInside
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 PinholeCamera.h:1469
Ocean::PinholeCameraT::fovYTop
T fovYTop() const
Returns the top field of view in y direction.
Ocean::PinholeCameraT::dampedNormalized
VectorT2< T > dampedNormalized(const VectorT2< T > &normalized, const T dampingFactor, const T invZoom) const
Determines the damped normalized coordinate for a given normalized coordinate.
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const unsigned int width, const unsigned int height, const T *parameters, const bool radialDistortion=true, const bool tangentialDistortion=true)
Creates a new PinholeCameraT<T> object by it's given intrinsic parameters.
Ocean::PinholeCameraT::principalPointX
T principalPointX() const
Returns the x-value of the principal point of the camera image in the pixel domain.
Definition PinholeCamera.h:1318
Ocean::PinholeCameraT::tangentialDistortion_
DistortionPair tangentialDistortion_
Pair of tangential distortion parameters.
Definition PinholeCamera.h:1219
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT()=default
Standard constructor.
Ocean::PinholeCameraT::invertedIntrinsic
const SquareMatrixT3< T > & invertedIntrinsic() const
Returns the inverted intrinsic camera matrix.
Definition PinholeCamera.h:1263
Ocean::PinholeCameraT::operator==
bool operator==(const PinholeCameraT< T > &camera) const
Returns whether two camera objects are identical up to a small epsilon.
Ocean::PinholeCameraT::principalPoint
VectorT2< T > principalPoint() const
Returns the coordinate of the principal point of the camera image in the pixel domain.
Definition PinholeCamera.h:1312
Ocean::PinholeCameraT::projectToImage
BoxT2< T > projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const BoxT3< T > &worldObjectBox, const bool distortImagePoint, const T zoom=T(1)) const
Projects a 3D box to the 2D image plane of the camera by a given camera pose.
Definition PinholeCamera.h:1780
Ocean::PinholeCameraT::DistortionPair
std::pair< T, T > DistortionPair
Definition of a pair of distortion values.
Definition PinholeCamera.h:123
Ocean::PinholeCameraT::projectToImageIF
TriangleT2< T > projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const TriangleT3< T > &worldObjectTriangle, const bool distortImagePoint, const T zoom=T(1)) const
Projects a 3D triangle to the 2D image plane of the camera by a given extrinsic camera pose.
Definition PinholeCamera.h:1890
Ocean::PinholeCameraT::projectToImageIF
VectorT2< T > projectToImageIF(const VectorT2< T > &normalizedObjectPoint, const bool distortImagePoint, const T zoom=T(1)) const
Transforms a normalized object point (a 3D object point transformed by the inverted and flipped extri...
Definition PinholeCamera.h:1902
Ocean::PinholeCameraT::isDistortionPlausible
bool isDistortionPlausible(const T symmetricFocalLengthRatio=T(1.05), const T modelAccuracy=T(0.001), const T symmetricDistortionRatio=T(1.08)) const
Checks whether the distortion of this camera is plausible.
Ocean::PinholeCameraT::dampedNormalized
VectorT2< T > dampedNormalized(const VectorT2< T > &normalized, const T dampingFactor, const T leftNormalizedBorder, const T rightNormalizedBorder, const T topNormalizedBorder, const T bottomNormalizedBorder) const
Determines the damped normalized coordinate for a given normalized coordinate.
Ocean::PinholeCameraT::ray
LineT3< T > ray(const VectorT2< T > &position, const HomogenousMatrixT4< T > &world_T_camera, const T zoom=T(1)) const
Returns a ray starting at the camera's center and intersection a given 2D point on the image plane.
Definition PinholeCamera.h:2378
Ocean::PinholeCameraT::fovXRight
T fovXRight() const
Returns the right field of view in x direction.
Ocean::PinholeCameraT::projectToImageDamped
BoxT2< T > projectToImageDamped(const HomogenousMatrixT4< T > &extrinsic, const BoxT3< T > &objectBox, const bool distortImagePoint, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a 3D box to the 2D image plane of the camera by a given inverse extrinsic camera matrix.
Definition PinholeCamera.h:2092
Ocean::PinholeCameraT::distortDamped
VectorT2< T > distortDamped(const VectorT2< T > &undistorted, const T dampingFactor=T(1), const T zoom=T(1)) const
Returns the distorted position of a given undistorted position defined in pixel coordinates.
Ocean::PinholeCameraT::distort
VectorT2< T > distort(const VectorT2< T > &undistorted) const
Returns the distorted position of a given undistorted position defined in pixel coordinates.
Definition PinholeCamera.h:1418
Ocean::PinholeCameraT::ray
LineT3< T > ray(const VectorT2< T > &position, const VectorT3< T > &world_t_camera, const QuaternionT< T > &world_Q_camera, const T zoom=T(1)) const
Returns a ray starting at the camera's center and intersection a given 2D point on the image plane.
Definition PinholeCamera.h:2388
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const unsigned int width, const unsigned int height, const T fovX, const T principalX, const T principalY)
Creates a new PinholeCameraT<T> object by the given frame dimensions, the ideal field of view,...
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const SquareMatrixT3< T > &intrinsic)
Creates a new PinholeCameraT<T> object by a given projection matrix with the intrinsic camera paramet...
Ocean::PinholeCameraT::distortNormalized
VectorT2< T > distortNormalized(const VectorT2< T > &undistortedNormalized, const T invZoom) const
Returns the distorted position of a given undistorted normalized position.
Definition PinholeCamera.h:2439
Ocean::PinholeCameraT::projectToImageDampedIF
BoxT2< T > projectToImageDampedIF(const HomogenousMatrixT4< T > &iFlippedExtrinsic, const BoxT3< T > &objectBox, const bool distortImagePoint, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a 3D box to the 2D image plane of the camera by a given inverse extrinsic camera matrix.
Ocean::PinholeCameraT::undistort
VectorT2< T > undistort(const VectorT2< T > &distorted, const unsigned int iterations=10u, const T zoom=T(1)) const
Returns the undistorted position of a given distorted position defined in pixel coordinates.
Definition PinholeCamera.h:1373
Ocean::PinholeCameraT::isValid
bool isValid() const
Returns whether this camera is valid.
Definition PinholeCamera.h:1572
Ocean::PinholeCameraT::vectorToPlane
VectorT3< T > vectorToPlane(const VectorT2< T > &position, const T distance, const T zoom) const
Returns a vector starting at the camera's center and intersecting a given 2D point on the image plane...
Definition PinholeCamera.h:2302
Ocean::PinholeCameraT::setRadialDistortion
void setRadialDistortion(const DistortionPair &radial)
Sets the radial distortion parameters.
Definition PinholeCamera.h:1360
Ocean::PinholeCameraT::projectToImageIF
BoxT2< T > projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const BoxT3< T > &worldObjectBox, const bool distortImagePoint, const T zoom=T(1)) const
Projects a 3D box to the 2D image plane of the camera by a given inverse camera pose.
Definition PinholeCamera.h:1871
Ocean::PinholeCameraT::focalLengthY
T focalLengthY() const
Returns the vertical focal length parameter.
Definition PinholeCamera.h:1336
Ocean::PinholeCameraT::invertedIntrinsics_
SquareMatrixT3< T > invertedIntrinsics_
Inverted intrinsic camera matrix.
Definition PinholeCamera.h:1207
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const PinholeCameraT< U > &pinholeCamera, const bool copyDistortionParameters=true)
Copy constructor for a pinhole camera with difference element data type than T.
Definition PinholeCamera.h:1282
Ocean::PinholeCameraT::hasDistortionParameters
bool hasDistortionParameters() const
Returns whether this camera object has specified distortion parameters.
Definition PinholeCamera.h:1293
Ocean::PinholeCameraT::projectToImageDampedIF
VectorT2< T > projectToImageDampedIF(const HomogenousMatrixT4< T > &iFlippedExtrinsic, const VectorT3< T > &objectPoint, const bool distortImagePoint, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a 3D object point to the 2D image plane of the camera by a given inverse extrinsic camera ma...
Ocean::PinholeCameraT::intrinsic
const SquareMatrixT3< T > & intrinsic() const
Returns the intrinsic camera matrix.
Definition PinholeCamera.h:1257
Ocean::PinholeCameraT::calculateCosBetween
T calculateCosBetween(const VectorT2< T > &first, const VectorT2< T > &second) const
Returns the cosine of the viewing angle between two undistorted points on the camera's image plane.
Ocean::PinholeCameraT::projectToImageDamped
TriangleT2< T > projectToImageDamped(const HomogenousMatrixT4< T > &extrinsic, const TriangleT3< T > &objectTriangle, const bool distortImagePoint, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a 3D triangle to the 2D image plane of the camera by a given extrinsic camera matrix.
Definition PinholeCamera.h:2099
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const unsigned int width, const unsigned int height, const T focalX, const T focalY, const T principalX, const T principalY)
Creates a new PinholeCameraT<T> object by it's given intrinsic parameters.
Ocean::PinholeCameraT::calculateInverseIntrinsic
void calculateInverseIntrinsic()
Determines the inverse of the intrinsic camera matrix.
Definition PinholeCamera.h:2396
Ocean::PinholeCameraT::projectToImageDamped
void projectToImageDamped(const HomogenousMatrixT4< T > &extrinsic, const VectorT3< T > *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2< T > *imagePoints, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a set of 3D object points onto an image plane of the camera by a given extrinsic camera matr...
Definition PinholeCamera.h:2106
Ocean::PinholeCameraT::vectorToPlane
VectorT3< T > vectorToPlane(const VectorT2< T > &position, const T distance) const
Returns a vector starting at the camera's center and intersecting a given 2D point on the image plane...
Definition PinholeCamera.h:2273
Ocean::PinholeCameraT::rotation
bool rotation(const VectorT2< T > &undistortedPosition, T &angleX, T &angleY) const
Gets two rotation parameters of the viewing ray for a given undistorted 2D position in the camera ima...
Ocean::PinholeCameraT::fovY
T fovY() const
Returns the field of view in x direction of the camera.
Ocean::PinholeCameraT::intrinsics_
SquareMatrixT3< T > intrinsics_
Intrinsic camera matrix.
Definition PinholeCamera.h:1204
Ocean::PinholeCameraT::fovDiagonal
T fovDiagonal() const
Returns the diagonal field of view of the camera.
Ocean::PinholeCameraT::frustumMatrix
SquareMatrixT4< T > frustumMatrix(const T nearDistance, const T farDistance) const
Returns the 4x4 frustum projection matrix corresponding to this camera.
Ocean::PinholeCameraT::transformationMatrixIF
HomogenousMatrixT4< T > transformationMatrixIF(const HomogenousMatrixT4< T > &iFlippedExtrinsic, const T zoom=T(1)) const
Returns a 4x4 homogenous transformation matrix (corresponding to a 3x4 matrix) that covers an extrins...
Definition PinholeCamera.h:1479
Ocean::PinholeCameraT::normalizedImagePoints2imagePoints
void normalizedImagePoints2imagePoints(const VectorT2< T > *normalizedImagePoints, const size_t numberNormalizedImagePoints, const bool distortImagePoints, VectorT2< T > *imagePoints) const
Calculates the image points corresponding to a set of given normalized image points.
Definition PinholeCamera.h:1676
Ocean::PinholeCameraT::vectorIF
VectorT3< T > vectorIF(const VectorT2< T > &position, const bool makeUnitVector=true) const
Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given ...
Definition PinholeCamera.h:2335
Ocean::PinholeCameraT::OptimizationStrategy
OptimizationStrategy
Definition of individual optimization strategies for camera parameters.
Definition PinholeCamera.h:129
Ocean::PinholeCameraT::height
unsigned int height() const
Returns the height of the camera image.
Definition PinholeCamera.h:1306
Ocean::PinholeCameraT::radialDistortion
const DistortionPair & radialDistortion() const
Returns the pair of radial distortion parameters.
Definition PinholeCamera.h:1269
Ocean::PinholeCameraT::radialDistortion_
DistortionPair radialDistortion_
Pair of radial distortion parameters for r^2 and r^4.
Definition PinholeCamera.h:1216
Ocean::PinholeCameraT::projectToImage
TriangleT2< T > projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const TriangleT3< T > &worldObjectTriangle, const bool distortImagePoint, const T zoom=T(1)) const
Projects a 3D triangle to the 2D image plane of the camera by a given camera pose.
Definition PinholeCamera.h:1788
Ocean::PinholeCameraT::vectorIF
VectorT3< T > vectorIF(const VectorT2< T > &position, const T zoom, const bool makeUnitVector) const
Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given ...
Definition PinholeCamera.h:2354
Ocean::PinholeCameraT::applyZoomFactor
void applyZoomFactor(const T relativeZoom)
Applies a given (relative) zoom factor which mainly multiplies the focal length parameters by the giv...
Ocean::PinholeCameraT::TScalar
T TScalar
The scalar data type of this object.
Definition PinholeCamera.h:120
Ocean::PinholeCameraT::inverseFocalLengthX
T inverseFocalLengthX() const
Returns the inverse horizontal focal length parameter.
Definition PinholeCamera.h:1342
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const SquareMatrixT3< T > &intrinsic, const unsigned int width, const unsigned int height)
Creates a new PinholeCameraT<T> object by the given intrinsic camera matrix and width and height of t...
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const unsigned int width, const unsigned int height, const T fovX)
Creates a new PinholeCameraT<T> object by the given width, height and field of view of a camera.
Ocean::PinholeCameraT::tangentialDistortion
const DistortionPair & tangentialDistortion() const
Returns the pair of tangential distortion parameters.
Definition PinholeCamera.h:1275
Ocean::PinholeCameraT::normalizedImagePoint2imagePoint
VectorT2< T > normalizedImagePoint2imagePoint(const VectorT2< T > &normalizedImagePoint, const bool distortImagePoint) const
Calculates the image point corresponding to a given normalized image point.
Definition PinholeCamera.h:1602
Ocean::PinholeCameraT::distortNormalizedDamped
VectorT2< T > distortNormalizedDamped(const VectorT2< T > &undistortedNormalized, const T dampingFactor, const T invZoom) const
Returns the distorted position of a given undistorted normalized position.
Ocean::PinholeCameraT::copyElements
void copyElements(T *arrayValues, const bool copyRadialDistortion=true, const bool copyTangentialDistortion=true) const
Copies the elements of this camera to an array with 4 to 8 floating point values.
Ocean::PinholeCameraT::projectToImageDampedIF
void projectToImageDampedIF(const HomogenousMatrixT4< T > &invertedFlippedExtrinsic, const VectorT3< T > *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2< T > *imagePoints, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a set of 3D object points onto an image plane of the camera by a given inverse extrinsic cam...
Ocean::PinholeCameraT::projectToImageIF
VectorT2< T > projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint, const bool distortImagePoint, const T zoom=T(1)) const
Projects a 3D object point to the 2D image plane of the camera by a given inverse camera pose.
Definition PinholeCamera.h:1816
Ocean::PinholeCameraT::principalPointY
T principalPointY() const
Returns the y-value of the principal point of the camera image in the pixel domain.
Definition PinholeCamera.h:1324
Ocean::PinholeCameraT::projectToImage
LineT2< T > projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const LineT3< T > &worldLine, const bool distortProjectedLine, const T zoom=T(1)) const
Projects a 3D line onto an image plane of the camera by a given camera pose.
Definition PinholeCamera.h:1806
Ocean::PinholeCameraT::fovX
T fovX() const
Returns the field of view in x direction of the camera.
Ocean::PinholeCameraT::calculateAngleBetween
T calculateAngleBetween(const VectorT2< T > &first, const VectorT2< T > &second) const
Returns the viewing angle between two undistorted points on the camera's image plane.
Ocean::PinholeCameraT::undistortDamped
VectorT2< T > undistortDamped(const VectorT2< T > &distorted, const T dampingFactor=T(1), const unsigned int iterations=10u, const T zoom=T(1)) const
Returns the undistorted position of a given distorted position defined in pixel coordinates.
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const unsigned int width, const unsigned int height, const PinholeCameraT< T > &camera)
Creates a new camera object with specified frame dimension and intrinsic camera parameters best match...
Ocean::PinholeCameraT::operator!=
bool operator!=(const PinholeCameraT< T > &camera) const
Returns whether two camera objects are not identical up to a small epsilon.
Definition PinholeCamera.h:2426
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const SquareMatrixT3< T > &intrinsic, const unsigned int width, const unsigned int height, const DistortionPair &radial, const DistortionPair &tangential)
Creates a new PinholeCameraT<T> object by the given intrinsic camera matrix, the width and height and...
Ocean::PinholeCameraT::fovXLeft
T fovXLeft() const
Returns the left field of view in x direction.
Ocean::PinholeCameraT::inverseFocalLengthY
T inverseFocalLengthY() const
Returns the inverse vertical focal length parameter.
Definition PinholeCamera.h:1351
Ocean::PinholeCameraT::vector
VectorT3< T > vector(const VectorT2< T > &position, const T zoom, const bool makeUnitVector=true) const
Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given ...
Definition PinholeCamera.h:2220
Ocean::PinholeCameraT::setIntrinsic
bool setIntrinsic(const SquareMatrixT3< T > &intrinsic)
Sets the intrinsic camera matrix.
Ocean::PinholeCameraT::imagePoint2normalizedImagePointDamped
VectorT2< T > imagePoint2normalizedImagePointDamped(const VectorT2< T > &imagePoint, const bool undistortImagePoint) const
Calculates the normalized image point corresponding to a given (distorted) image point.
Definition PinholeCamera.h:1590
Ocean::PinholeCameraT::PinholeCameraT
PinholeCameraT(const T subFrameLeft, const T subFrameTop, const unsigned int subFrameWidth, const unsigned int subFrameHeight, const PinholeCameraT< T > &camera)
Creates a new sub-frame camera profile based on a camera profile of the entire camera frame.
Ocean::PinholeCameraT::projectToImageIF
LineT2< T > projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const LineT3< T > &worldLine, const bool distortProjectedLine, const T zoom=T(1)) const
Projects a 3D line onto an image plane of the camera by a given inverse camera pose.
Definition PinholeCamera.h:2068
Ocean::PinholeCameraT::pointJacobian2x3IF
void pointJacobian2x3IF(const VectorT3< U > &flippedCameraObjectPoint, U *jx, U *jy) const
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Definition PinholeCamera.h:1503
Ocean::PinholeCameraT::projectToImageIF
void projectToImageIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > *worldObjectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2< T > *imagePoints, const T zoom=T(1)) const
Projects a set of 3D object points onto an image plane of the camera by a given inverse camera pose.
Definition PinholeCamera.h:1952
Ocean::PinholeCameraT::PinholeCameraT
friend class PinholeCameraT
Definition PinholeCamera.h:115
Ocean::PinholeCameraT::projectToImageDampedIF
TriangleT2< T > projectToImageDampedIF(const HomogenousMatrixT4< T > &iFlippedExtrinsic, const TriangleT3< T > &objectTriangle, const bool distortImagePoint, const T dampingFactor=T(1), const T zoom=T(1)) const
Projects a 3D triangle to the 2D image plane of the camera by a given inverse extrinsic camera matrix...
Ocean::PinholeCameraT::projectToImage
void projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const VectorT3< T > *worldObjectPoints, const size_t numberObjectPoints, const bool distortImagePoints, VectorT2< T > *imagePoints, const T zoom=T(1)) const
Projects a set of 3D object points onto an image plane of the camera by a given camera pose.
Definition PinholeCamera.h:1796
Ocean::PinholeCameraT::projectToImage
VectorT2< T > projectToImage(const HomogenousMatrixT4< T > &world_T_camera, const VectorT3< T > &worldObjectPoint, const bool distortImagePoint, const T zoom=T(1)) const
Projects a 3D object point to the 2D image plane of the camera by a given camera pose.
Definition PinholeCamera.h:1772
Ocean::PinholeCameraT::focalLengthX
T focalLengthX() const
Returns the horizontal focal length parameter.
Definition PinholeCamera.h:1330
Ocean::PinholeCameraT::vector
VectorT3< T > vector(const VectorT2< T > &position, const bool makeUnitVector=true) const
Returns a normalized vector (with length 1) starting at the camera's center and intersecting a given ...
Definition PinholeCamera.h:2171
Ocean::QuaternionT
This class implements a unit quaternion rotation.
Definition Quaternion.h:100
Ocean::QuaternionT::isValid
bool isValid() const
Returns whether this quaternion is a valid unit quaternion.
Definition Quaternion.h:899
Ocean::SquareMatrixT3
This class implements a 3x3 square matrix.
Definition SquareMatrix3.h:88
Ocean::SquareMatrixT3::inverted
SquareMatrixT3< T > inverted() const
Returns the inverted matrix of this matrix.
Definition SquareMatrix3.h:1176
Ocean::SquareMatrixT4
This class implements a 4x4 square matrix.
Definition SquareMatrix4.h:85
Ocean::TriangleT2
This class implements a 2D triangle with Cartesian coordinates.
Definition Triangle2.h:81
Ocean::TriangleT3
This class implements a 3D triangle.
Definition Triangle3.h:80
Ocean::TriangleT3::point0
const VectorT3< T > & point0() const
Returns the first point of this triangle.
Definition Triangle3.h:278
Ocean::TriangleT3::isValid
bool isValid() const
Returns whether this triangle is valid.
Definition Triangle3.h:580
Ocean::TriangleT3::point2
const VectorT3< T > & point2() const
Returns the third point of this triangle.
Definition Triangle3.h:290
Ocean::TriangleT3::point1
const VectorT3< T > & point1() const
Returns the second point of this triangle.
Definition Triangle3.h:284
Ocean::VectorT2
This class implements a vector with two elements.
Definition Vector2.h:96
Ocean::VectorT2::x
const T & x() const noexcept
Returns the x value.
Definition Vector2.h:710
Ocean::VectorT2::y
const T & y() const noexcept
Returns the y value.
Definition Vector2.h:722
Ocean::VectorT2::isEqual
bool isEqual(const VectorT2< T > &vector, const T eps) const
Returns whether two vectors are equal up to a specified epsilon.
Definition Vector2.h:758
Ocean::VectorT2::sqr
T sqr() const
Returns the square of the vector length.
Definition Vector2.h:633
Ocean::VectorT3
This class implements a vector with three elements.
Definition Vector3.h:97
Ocean::VectorT3::y
const T & y() const noexcept
Returns the y value.
Definition Vector3.h:824
Ocean::VectorT3::x
const T & x() const noexcept
Returns the x value.
Definition Vector3.h:812
Ocean::VectorT3::normalized
VectorT3< T > normalized() const
Returns the normalized vector.
Definition Vector3.h:617
Ocean::VectorT3::z
const T & z() const noexcept
Returns the z value.
Definition Vector3.h:836
Ocean::VectorT3::length
T length() const
Returns the length of the vector.
Definition Vector3.h:676
Ocean::minmax
T minmax(const T &lowerBoundary, const T &value, const T &upperBoundary)
This function fits a given parameter into a specified value range.
Definition base/Utilities.h:903
Ocean::sqr
unsigned int sqr(const char value)
Returns the square value of a given value.
Definition base/Utilities.h:1029
Ocean::PinholeCameraD
PinholeCameraT< double > PinholeCameraD
Definition of an pinhole camera object with double precision.
Definition PinholeCamera.h:46
Ocean::PinholeCameraF
PinholeCameraT< float > PinholeCameraF
Definition of an pinhole camera object with float precision.
Definition PinholeCamera.h:53
Ocean::PinholeCamerasD
PinholeCamerasT< double > PinholeCamerasD
Definition of a vector holding PinholeCameraD objects.
Definition PinholeCamera.h:75
Ocean::PinholeCameras
PinholeCamerasT< Scalar > PinholeCameras
Definition of a vector holding pinhole camera objects.
Definition PinholeCamera.h:68
Ocean::PinholeCamerasT
std::vector< PinholeCameraT< T > > PinholeCamerasT
Definition of a typename alias for vectors with PinholeCameraT objects.
Definition PinholeCamera.h:61
Ocean::PinholeCamera
PinholeCameraT< Scalar > PinholeCamera
Definition of an pinhole camera object with Scalar precision.
Definition PinholeCamera.h:39
Ocean::PinholeCamerasF
PinholeCamerasT< float > PinholeCamerasF
Definition of a vector holding PinholeCameraF objects.
Definition PinholeCamera.h:82
Ocean
The namespace covering the entire Ocean framework.
Definition Accessor.h:15