Ocean
Jacobian.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_GEOMETRY_JACOBIAN_H
9 #define META_OCEAN_GEOMETRY_JACOBIAN_H
10 
11 #include "ocean/geometry/Geometry.h"
12 
13 #include "ocean/base/Accessor.h"
14 
15 #include "ocean/math/AnyCamera.h"
16 #include "ocean/math/ExponentialMap.h"
17 #include "ocean/math/FisheyeCamera.h"
18 #include "ocean/math/Matrix.h"
19 #include "ocean/math/PinholeCamera.h"
20 #include "ocean/math/Pose.h"
21 #include "ocean/math/Quaternion.h"
22 #include "ocean/math/SquareMatrix3.h"
23 #include "ocean/math/StaticMatrix.h"
24 #include "ocean/math/Vector3.h"
25 
26 namespace Ocean
27 {
28 
29 namespace Geometry
30 {
31 
32 /**
33  * This class implements function to calculate the jacobian matrices for geometry functions.
34  * @ingroup geometry
35  */
36 class OCEAN_GEOMETRY_EXPORT Jacobian
37 {
38  public:
39 
40  /**
41  * This function determines the 3x3 Jacobian of a rotation function rotating a 3D object point by application of an exponential map.
42  * The given exponential map stores the rotation as the rotation axis with a vector length equal to the rotation angle.<br>
43  * The Jacobian is determined by application of the Rodrigues formula for the specified exponential map.<br>
44  * The resulting 3x3 Jacobian depends on the three rotation parameters and on the 3D object point.<br>
45  * However, we can separate the dependency allowing to calculate the major parts of the Jacobian for the rotation parameters first.<br>
46  * Therefore, we calculate three 3x3 matrices depending only on the rotation parameters.<br>
47  * Each of the matrix can be used to calculate one column of the final 3x3 Jacobian by multiplication with the 3D object point.<br>
48  * Thus, we can reuse the three 3x3 matrices if we have several 3D object points which can improve the performance significantly.<br>
49  * The final 3x3 Jacobian for the provided exponential map and an object point O is defined by the following three vectors:<br>
50  * [dwx * O | dwy * O | dwz * O]
51  * @param rotation The rotation for which the three derivative rotations will be determined
52  * @param dwx The resulting rotation matrix derived to wx
53  * @param dwy The resulting rotation matrix derived to wy
54  * @param dwz The resulting rotation matrix derived to wz
55  * @tparam T The data type of the scalar to be used, either 'float' or 'double'
56  */
57  template <typename T>
58  static void calculateRotationRodriguesDerivative(const ExponentialMapT<T>& rotation, SquareMatrixT3<T>& dwx, SquareMatrixT3<T>& dwy, SquareMatrixT3<T>& dwz);
59 
60  /**
61  * Calculates the three jacobian rows for a given exponential rotation map representing the location of a 3D object point.
62  * The rotation map defines the rotation of the vector [0, 0, -objectPointDistance].<br>
63  * The corresponding function f, which transforms the 3D object point defined in coordinates into a 3D object point defined in the Cartesian coordinate system, is given as follows:<br>
64  * f(w, r) = f(wx, wy, wz, r) = R(wx, wy, wz) * [0, 0, -r] = [ox, oy, oz]<br>
65  * The resulting 3x3 jacobian has the following form:
66  * <pre>
67  * | dfx / dwx, dfx / dwy, dfx / dwz |
68  * | dfy / dwx, dfy / dwy, dfy / dwz |
69  * | dfz / dwx, dfz / dwy, dfz / dwz |
70  * </pre>
71  * @param jx First row of the resulting jacobian, with 3 column entries
72  * @param jy Second row of the resulting jacobian, with 3 column entries
73  * @param jz Third row of the resulting jacobian, with 3 column entries
74  * @param sphericalObjectPoint The rotation defining the 3D object point for which the derivatives will be determined
75  * @param objectPointDistance The distance of the 3D object point, which is the distance to the origin, with range (0, infinity)
76  * @tparam T The data type of the scalar to be used, either 'float' or 'double'
77  */
78  template <typename T>
79  static void calculateSphericalObjectPointJacobian3x3(T* jx, T* jy, T* jz, const ExponentialMapT<T>& sphericalObjectPoint, const T objectPointDistance);
80 
81  /**
82  * Calculates the two jacobian rows for a given exponential rotation map representing the location of a 3D object point projecting into the camera frame with orientational camera pose.
83  * The rotation map defines the rotation of the vector [0, 0, -objectPointDistance].<br>
84  * The corresponding function f is given as follows:<br>
85  * f(w, r) = fproj(fori(fobj(wx, wy, wz, r))) = [x, y]<br>
86  * Where fobj translates the location of the 3D object point defined in spherical coordinates into a 3D object point defined in the Cartesian coordinate system.
87  * fproj defines the function which projects (and optional distorts) a 3D object point into the camera,<br>
88  * and fori defines the function which applies the orientation (rotational pose) of the camera.<br>
89  * The resulting 2x3 jacobian has the following form:
90  * <pre>
91  * | dfx / dwx, dfx / dwy, dfx / dwz |
92  * | dfy / dwx, dfy / dwy, dfy / dwz |
93  * </pre>
94  * @param jx First row of the resulting jacobian, with 3 column entries
95  * @param jy Second row of the resulting jacobian, with 3 column entries
96  * @param camera The camera profile defining the projection, must be valid
97  * @param flippedCamera_R_world The inverted and flipped orientation of the camera, transforming the world to the flipped camera, with default flipped camera pointing towards the positive z-space with y-axis downwards, must be valid
98  * @param sphericalObjectPoint The rotation defining the 3D object point for which the derivatives will be determined
99  * @param objectPointDistance The distance of the 3D object point, which is the distance to the origin, with range (0, infinity)
100  * @tparam T The data type of the scalar to be used, either 'float' or 'double'
101  */
102  template <typename T>
103  static void calculateSphericalObjectPointOrientationJacobian2x3IF(T* jx, T* jy, const AnyCameraT<T>& camera, const SquareMatrixT3<T>& flippedCamera_R_world, const ExponentialMapT<T>& sphericalObjectPoint, const T objectPointDistance);
104 
105  /**
106  * Calculates the two Jacobian rows for the 3-DOF rotational part of a 6-DOF camera pose and a given 3D object point.
107  * The 6-DOF camera pose is separated into a (fixed) translational part and a (flexible) rotational part.<br>
108  * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
109  * The 3 derivatives are calculated for the 3-DOF orientation.<br>
110  * The resulting jacobian rows have the following form:
111  * <pre>
112  * | dfx / dwx, dfx / dwy, dfx / dwz |
113  * | dfy / dwx, dfy / dwy, dfy / dwz |
114  * </pre>
115  * With transformation function f = (fx, fy) and exponential map w = (wx, wy, wz).<br>
116  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.
117  * In the following, how to separate a common (inverted flipped) 6-DOF camera pose into a translational and rotational part.
118  * <pre>
119  * world_T_camera: translational part rotational part
120  * | R | t | | I | t | | R | 0 |
121  * | 0 | 1 | = | 0 | 1 | * | 0 | 1 |
122  *
123  * flippedCamera_T_world: rotational part translational part
124  * | R | t | | R | 0 | | I | R^-1 t |
125  * | 0 | 1 | = | 0 | 1 | * | 0 | 1 |
126  * </pre>
127  * @param anyCamera The camera profile to determine the jacobian values for, must be valid
128  * @param flippedCamera_R_translation The rotation between the translational part of the camera and the flipped camera pose, with default flipped camera pointing towards the positive z-space with y-axis downwards, must be valid
129  * @param translation_T_world The translation between the world and the translational part of the camera pose
130  * @param worldObjectPoint The 3D object point to determine the jacobian for, defined in world
131  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
132  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
133  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
134  * @param jx The resulting first row of the jacobian, with 3 column entries, must be valid
135  * @param jy The resulting second row of the jacobian, with 3 column entries, must be valid
136  * @see calculateRotationRodriguesDerivative().
137  * @tparam T The scalar data type, either 'float' or 'double'
138  * @tparam TRotation The data type of the provided rotation, either 'QuaternionT<T>' or 'SquareMatrixT3<T>'
139  */
140  template <typename T, typename TRotation>
141  static OCEAN_FORCE_INLINE void calculateOrientationalJacobianRodrigues2x3IF(const AnyCameraT<T>& anyCamera, const TRotation& flippedCamera_R_translation, const VectorT3<T>& translation_T_world, const VectorT3<T>& worldObjectPoint, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz, T* jx, T* jy);
142 
143  /**
144  * Calculates all 3-DOF orientational jacobian rows for a given (flexible) camera pose and a set of static 3D object points.
145  * The 3 derivatives are calculated for the orientation part of the 6-DOF pose.<br>
146  * The resulting jacobian rows have the following form:
147  * <pre>
148  * | dfx / dwx, dfx / dwy, dfx / dwz |
149  * | dfy / dwx, dfy / dwy, dfy / dwz |
150  * </pre>
151  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz).<br>
152  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
153  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * objectPoints.size() rows and 6 columns
154  * @param camera The camera profile defining the projection, must be valid
155  * @param flippedCamera_R_world Inverted and flipped camera pose to determine the jacobian for
156  * @param objectPoints The accessor providing the 3D object points to determine the jacobian for
157  * @tparam T The scalar data type, either 'float' or 'double'
158  * @see calculateOrientationJacobianRodrigues2x3().
159  */
160  template <typename T>
161  static void calculateOrientationJacobianRodrigues2nx3IF(T* jacobian, const AnyCameraT<T>& camera, const ExponentialMapT<T>& flippedCamera_R_world, const ConstIndexedAccessor<VectorT3<T>>& objectPoints);
162 
163  /**
164  * Deprecated.
165  *
166  * Calculates all 3-DOF orientational jacobian rows for a given (flexible) pose and a set of static 3D object points.
167  * The 3 derivatives are calculated for the orientation part of the 6DOF pose.<br>
168  * The resulting jacobian rows have the following form:
169  * <pre>
170  * | dfx / dwx, dfx / dwy, dfx / dwz |
171  * | dfy / dwx, dfy / dwy, dfy / dwz |
172  * </pre>
173  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz).<br>
174  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
175  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * objectPoints.size() rows and 6 columns
176  * @param pinholeCamera The pinhole camera to determine the jacobian values for
177  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for, while the rotational part is use only
178  * @param objectPoints The accessor providing the 3D object points to determine the jacobian for
179  * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
180  * @see calculateOrientationJacobianRodrigues2x3().
181  */
182  static void calculateOrientationJacobianRodrigues2nx3(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const ConstIndexedAccessor<Vector3>& objectPoints, const bool distortImagePoints);
183 
184  /**
185  * Calculates the two jacobian rows for a given (flexible) pose and static camera and one static 3D object point.
186  * The 6 derivatives are calculated for the 6DOF pose.<br>
187  * The resulting jacobian rows have the following form:
188  * <pre>
189  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
190  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
191  * </pre>
192  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
193  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
194  * @param jx First row of the jacobian, with 6 column entries
195  * @param jy Second row of the jacobian, with 6 column entries
196  * @param pinholeCamera The pinhole camera to determine the jacobian values for
197  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
198  * @param objectPoint 3D object point to determine the jacobian for
199  * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
200  * @see calculatePoseJacobianRodrigues2nx6().
201  */
202  static inline void calculatePoseJacobianRodrigues2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Vector3& objectPoint, const bool distortImagePoint);
203 
204  /**
205  * Deprecated.
206  *
207  * Calculates the two jacobian rows for a given (flexible) pose and one static 3D object point.
208  * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
209  * The 6 derivatives are calculated for the 6DOF pose.<br>
210  * The resulting jacobian rows have the following form:
211  * <pre>
212  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
213  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
214  * </pre>
215  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
216  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
217  * @param jx First row of the jacobian, with 6 column entries
218  * @param jy Second row of the jacobian, with 6 column entries
219  * @param pinholeCamera The pinhole camera to determine the jacobian values for
220  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
221  * @param objectPoint 3D object point to determine the jacobian for
222  * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
223  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
224  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
225  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
226  * @see calculateRotationRodriguesDerivative().
227  */
228  static void calculatePoseJacobianRodrigues2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_P_world, const Vector3& objectPoint, const bool distortImagePoint, const SquareMatrix3& dwx, const SquareMatrix3& dwy, const SquareMatrix3& dwz);
229 
230  /**
231  * Deprecated.
232  *
233  * Calculates the two jacobian rows for a given (flexible) pose and one static 3D object point.
234  * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
235  * The 6 derivatives are calculated for the 6DOF pose.<br>
236  * The resulting jacobian rows have the following form:
237  * <pre>
238  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
239  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
240  * </pre>
241  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
242  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
243  * @param jx First row of the jacobian, with 6 column entries
244  * @param jy Second row of the jacobian, with 6 column entries
245  * @param fisheyeCamera Fisheye camera to determine the jacobian values for, must be valid
246  * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
247  * @param worldObjectPoint 3D object point to determine the jacobian for, in world coordinate system
248  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
249  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
250  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
251  * @see calculateRotationRodriguesDerivative().
252  */
253  static void calculatePoseJacobianRodrigues2x6(Scalar* jx, Scalar* jy, const FisheyeCamera& fisheyeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Vector3& worldObjectPoint, const SquareMatrix3& dwx, const SquareMatrix3& dwy, const SquareMatrix3& dwz);
254 
255  /**
256  * Calculates the two jacobian rows for a given (flexible) 6-DOF camera pose and one static 3D object point.
257  * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
258  * The 6 derivatives are calculated for the 6DOF pose.<br>
259  * The resulting jacobian rows have the following form:
260  * <pre>
261  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
262  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
263  * </pre>
264  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
265  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
266  * @param anyCamera The camera profile to determine the jacobian values for, must be valid
267  * @param flippedCamera_T_world The inverted and flipped camera pose to determine the jacobian for, with default flipped camera pointing towards the positive z-space with y-axis downwards, must be valid
268  * @param worldObjectPoint 3D object point to determine the jacobian for, in world coordinate system
269  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
270  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
271  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
272  * @param jx The resulting first row of the jacobian, with 6 column entries, must be valid
273  * @param jy The resulting second row of the jacobian, with 6 column entries, must be valid
274  * @see calculateRotationRodriguesDerivative().
275  * @tparam T The scalar data type
276  */
277  template <typename T>
278  static OCEAN_FORCE_INLINE void calculatePoseJacobianRodrigues2x6IF(const AnyCameraT<T>& anyCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz, T* jx, T* jy);
279 
280  /**
281  * Calculates all jacobian rows for a given (flexible) 6-DOF camera pose with a static camera profile and several static 3D object points.
282  * The 6 derivatives are calculated for the entire 6DOF pose.<br>
283  * The resulting jacobian rows have the following form:
284  * <pre>
285  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
286  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
287  * </pre>
288  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
289  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
290  * @param jacobian First element in the first row of the (row major aligned) jacobian matrix, with 2 * numberObjectPoints rows and 6 columns
291  * @param camera The camera profile defining the projection, must be valid
292  * @param flippedCamera_P_world The inverted and flipped pose to determine the jacobian for, with default flipped camera pointing towards the positive z-space with y-axis downwards, must be valid
293  * @param objectPoints The 3D object points to determine the jacobian for, must be valid
294  * @param numberObjectPoints The number of given object points, with range [1, infinity)
295  * @see calculatePoseJacobianRodrigues2x6IF().
296  */
297  template <typename T>
298  static void calculatePoseJacobianRodrigues2nx6IF(T* jacobian, const AnyCameraT<T>& camera, const PoseT<T>& flippedCamera_P_world, const VectorT3<T>* objectPoints, const size_t numberObjectPoints);
299 
300  /**
301  * Deprecated.
302  *
303  * Calculates all pose jacobian rows for a given (flexible) pose with a static camera profile supporting distortion and a set of static 3D object points.
304  * The 6 derivatives are calculated for the entire 6DOF pose.<br>
305  * The resulting jacobian rows have the following form:
306  * <pre>
307  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
308  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
309  * </pre>
310  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
311  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
312  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 6 columns
313  * @param pinholeCamera The pinhole camera to determine the jacobian values for
314  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
315  * @param objectPoints 3D object points to determine the jacobian for
316  * @param numberObjectPoints Number of given object points
317  * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
318  * @see calculatePoseJacobianRodrigues2x6().
319  */
320  static void calculatePoseJacobianRodrigues2nx6(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Vector3* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
321 
322  /**
323  * Calculates all pose jacobian rows for a given (flexible) pose with a static camera profile supporting distortion and a set of static 3D object points.
324  * The distortion is damped for 3D object points not projecting into the camera frame but outside the camera frame.<br>
325  * The 6 derivatives are calculated for the entire 6DOF pose.<br>
326  * The resulting jacobian rows have the following form:
327  * <pre>
328  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
329  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
330  * </pre>
331  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
332  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
333  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 6 columns
334  * @param pinholeCamera The pinhole camera to determine the jacobian values for
335  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
336  * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
337  * @param objectPoints 3D object points to determine the jacobian for
338  * @param numberObjectPoints Number of given object points
339  * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
340  * @see calculatePoseJacobianRodrigues2x6().
341  */
342  static void calculatePoseJacobianRodriguesDampedDistortion2nx6(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Scalar dampingFactor, const Vector3* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
343 
344  /**
345  * Calculates the two jacobian rows for a given (flexible) pose with (flexible) zoom factor and one static 3D object point.
346  * The 7 derivatives are calculated for the entire 6DOF pose including the zoom factor.<br>
347  * The resulting jacobian rows have the following form:
348  * <pre>
349  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz, dfx / dts |
350  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz, dfy / dts |
351  * </pre>
352  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz), translation t = (tx, ty, tz) and zoom factor s.<br>
353  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
354  * @param jx First row of the jacobian, with 7 column entries
355  * @param jy Second row of the jacobian, with 7 column entries
356  * @param pinholeCamera The pinhole camera to determine the jacobian values for
357  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
358  * @param zoom The zoom factor of the pose, with range (0, infinity)
359  * @param objectPoint 3D object point to determine the jacobian for
360  * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
361  */
362  static void calculatePoseZoomJacobianRodrigues2x7(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Scalar zoom, const Vector3& objectPoint, const bool distortImagePoints);
363 
364  /**
365  * Calculates all pose jacobian rows for a given (flexible) pose with (flexible) zoom factor and a set of static 3D object points.
366  * The 7 derivatives are calculated for the entire 6DOF pose including the zoom factor.<br>
367  * The resulting jacobian rows have the following form:
368  * <pre>
369  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz, dfx / dts |
370  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz, dfy / dts |
371  * </pre>
372  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz), translation t = (tx, ty, tz) and zoom factor s.<br>
373  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
374  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 7 columns
375  * @param pinholeCamera The pinhole camera to determine the jacobian values for
376  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
377  * @param zoom The zoom factor of the pose, with range (0, infinity)
378  * @param objectPoints 3D object points to determine the jacobian for
379  * @param numberObjectPoints Number of given object points
380  * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
381  */
382  static void calculatePoseZoomJacobianRodrigues2nx7(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Scalar zoom, const Vector3* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
383 
384  /**
385  * Calculates the two jacobian rows for a given (flexible) 6-DOF object transformation, and a static 6-DOF camera pose, and a static camera, and one static 3D object point.
386  * The 6 derivatives are calculated for the 6-DOF object transformation.<br>
387  * The resulting jacobian rows have the following form:
388  * <pre>
389  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
390  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
391  * </pre>
392  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
393  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
394  * @param jx First row of the jacobian, with 6 column entries
395  * @param jy Second row of the jacobian, with 6 column entries
396  * @param pinholeCamera The pinhole camera profile defining the projection, must be valid
397  * @param extrinsicIF Inverted and flipped camera pose, must be valid
398  * @param objectPose The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
399  * @param objectPoint 3D object point to determine the jacobian for
400  * @see calculateObjectTransformation2nx6().
401  */
402  static inline void calculateObjectTransformation2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3& objectPoint);
403 
404  /**
405  * Calculates the two jacobian rows for a given (flexible) 6-DOF object transformation, and a static 6-DOF camera pose, and a static camera, and one static 3D object point.
406  * This function uses the pre-calculated 3x3 Jacobian matrix of the object transformation's orientation provided by three separated 3x3 matrices.<br>
407  * The 6 derivatives are calculated for the 6-DOF object transformation.<br>
408  * The resulting jacobian rows have the following form:
409  * <pre>
410  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
411  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
412  * </pre>
413  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
414  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
415  * @param jx First row of the jacobian, with 6 column entries
416  * @param jy Second row of the jacobian, with 6 column entries
417  * @param pinholeCamera The pinhole camera profile defining the projection, must be valid
418  * @param extrinsicIF Inverted and flipped camera pose, must be valid
419  * @param objectPose The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
420  * @param objectPoint 3D object point to determine the jacobian for
421  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
422  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
423  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
424  * @see calculateObjectTransformation2nx6().
425  */
426  static void calculateObjectTransformation2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3& objectPoint, const SquareMatrix3& dwx, const SquareMatrix3& dwy, const SquareMatrix3& dwz);
427 
428  /**
429  * Calculates all pose jacobian rows for a given (flexible) 6-DOF object transformation, and a static 6-DOF camera pose, and a static camera, and several static 3D object points.
430  * The 6 derivatives are calculated for the entire 6DOF pose.<br>
431  * The resulting jacobian rows have the following form:
432  * <pre>
433  * | ... |
434  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
435  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
436  * | ... |
437  * </pre>
438  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
439  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
440  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with `2 * numberObjectPoints` rows and 6 columns, must be valid
441  * @param pinholeCamera The pinhole camera profile defining the projection, must be valid
442  * @param extrinsicIF Inverted and flipped camera pose, must be valid
443  * @param objectPose The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
444  * @param objectPoints 3D object points to determine the jacobian for, must be valid
445  * @param numberObjectPoints Number of given object points, with range [1, infinity)
446  * @see calculateObjectTransformation2x6().
447  */
448  static void calculateObjectTransformation2nx6(Scalar* jacobian, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3* objectPoints, const size_t numberObjectPoints);
449 
450  /**
451  * Calculates all pose jacobian rows for a given (flexible) 6-DOF object transformation, and a static 6-DOF camera pose, and a static fisheye camera, and several static 3D object points.
452  * The 6 derivatives are calculated for the entire 6DOF pose.<br>
453  * The resulting jacobian rows have the following form:
454  * <pre>
455  * | ... |
456  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
457  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
458  * | ... |
459  * </pre>
460  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
461  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
462  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with `2 * numberObjectPoints` rows and 6 columns, must be valid
463  * @param fisheyeCamera The fisheye camera profile defining the projection, must be valid
464  * @param flippedCamera_T_world Inverted and flipped camera pose, must be valid
465  * @param world_T_object The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
466  * @param objectPoints 3D object points to determine the jacobian for, must be valid
467  * @param numberObjectPoints Number of given object points, with range [1, infinity)
468  * @see calculateObjectTransformation2x6().
469  */
470  static void calculateObjectTransformation2nx6(Scalar* jacobian, const FisheyeCamera& fisheyeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Pose& world_T_object, const Vector3* objectPoints, const size_t numberObjectPoints);
471 
472  /**
473  * Calculates the two jacobian rows for a given pose with translation scale ambiguities and static object point.
474  * The 5 derivatives are calculated for the 5DOF pose.<br>
475  * The resulting jacobian rows have the following form:
476  * <pre>
477  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtu, dfx / dtv |
478  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtu, dfy / dtv |
479  * </pre>
480  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tu, tv, sqrt(1 - tu*tu - tv*tv)).<br>
481  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
482  * @param jx First row of the jacobian, with 5 column entries
483  * @param jy Second row of the jacobian, with 5 column entries
484  * @param pinholeCamera The pinhole camera to determine the jacobian values for
485  * @param rotation Exponential map defining the rotation to determine the jacobian for
486  * @param translation Translation with scale ambiguities to determine the jacobian for, with range tx*tx + ty*ty < 1
487  * @param objectPoint 3D object point to determine the jacobian for
488  */
489  static void calculatePoseJacobianRodrigues2x5(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const ExponentialMap& rotation, const Vector2& translation, const Vector3& objectPoint);
490 
491  /**
492  * Calculates all jacobian rows for a given pose with translation scale ambiguities and a set of static object points.
493  * The 5 derivatives are calculated for the 5DOF pose.<br>
494  * The resulting jacobian rows have the following form:
495  * <pre>
496  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtu, dfx / dtv |
497  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtu, dfy / dtv |
498  * </pre>
499  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tu, tv, sqrt(1 - tu*tu - tv*tv)).<br>
500  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
501  * @param jacobian First row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 5 columns
502  * @param pinholeCamera The pinhole camera to determine the jacobian values for
503  * @param rotation Exponential map defining the rotation to determine the jacobian for
504  * @param translation Translation with scale ambiguities to determine the jacobian for, with range tx*tx + ty*ty < 1
505  * @param objectPoints 3D object points to determine the jacobian for
506  * @param numberObjectPoints Number of given object points
507  */
508  static void calculatePoseJacobianRodrigues2nx5(Scalar* jacobian, const PinholeCamera& pinholeCamera, const ExponentialMap& rotation, const Vector2& translation, const Vector3* objectPoints, const size_t numberObjectPoints);
509 
510  /**
511  * Calculates the two jacobian rows for a given pose and dynamic object point.
512  * The derivatives are calculated for the 3D object point only.<br>
513  * The resulting jacobian rows have the following form:
514  * <pre>
515  * | dfx / dX, dfx / dY, dfx / dZ |
516  * | dfy / dX, dfy / dY, dfy / dZ |
517  * </pre>
518  * @param jx First row of the jacobian, with 3 column entries receiving the point derivatives
519  * @param jy Second row of the jacobian, with 3 column entries receiving the point derivatives
520  * @param pinholeCamera The pinhole camera to determine the jacobian values for
521  * @param flippedCamera_P_world Pose to determine the jacobian for (inverted and flipped)
522  * @param objectPoint 3D object point to determine the jacobian for
523  * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
524  */
525  static void calculatePointJacobian2x3(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_P_world, const Vector3& objectPoint, const bool distortImagePoint);
526 
527  /**
528  * Calculates the two jacobian rows for a given pose and dynamic object point.
529  * The derivatives are calculated for the 3D object point only.<br>
530  * The resulting jacobian rows have the following form:
531  * <pre>
532  * | dfx / dX, dfx / dY, dfx / dZ |
533  * | dfy / dX, dfy / dY, dfy / dZ |
534  * </pre>
535  * @param jx First row of the jacobian, with 3 column entries receiving the point derivatives
536  * @param jy Second row of the jacobian, with 3 column entries receiving the point derivatives
537  * @param fisheyeCamera Fisheye camera profile defining the projection, must be valid
538  * @param flippedCamera_T_world Flipped and inverted pose of the camera, must be valid
539  * @param worldObjectPoint The 3D object point, defined in world
540  */
541  static void calculatePointJacobian2x3(Scalar* jx, Scalar* jy, const FisheyeCamera& fisheyeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Vector3& worldObjectPoint);
542 
543  /**
544  * Calculates the two jacobian rows for a given pose and dynamic object point.
545  * The derivatives are calculated for the 3D object point only.<br>
546  * The resulting jacobian rows have the following form:
547  * <pre>
548  * | dfx / dX, dfx / dY, dfx / dZ |
549  * | dfy / dX, dfy / dY, dfy / dZ |
550  * </pre>
551  * @param anyCamera The camera profile defining the projection, must be valid
552  * @param flippedCamera_T_world Flipped and inverted pose of the camera, must be valid
553  * @param worldObjectPoint The 3D object point, defined in world
554  * @param jx The resulting first row of the jacobian, with 3 column entries receiving the point derivatives, must be valid
555  * @param jy The resulting second row of the jacobian, with 3 column entries receiving the point derivatives, must be valid
556  * @tparam T The scalar data type
557  */
558  template <typename T>
559  static OCEAN_FORCE_INLINE void calculatePointJacobian2x3IF(const AnyCameraT<T>& anyCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, T* jx, T* jy);
560 
561  /**
562  * Calculates the two jacobian rows for a given pose and several dynamic object points.
563  * The derivatives are calculated for the 3D object point only.<br>
564  * The resulting jacobian rows have the following form:
565  * <pre>
566  * | dfx / dX0, dfx / dY0, dfx / dZ0 |
567  * | dfy / dX0, dfy / dY0, dfy / dZ0 |
568  * | dfx / dX1, dfx / dY1, dfx / dZ1 |
569  * | dfx / dX1, dfx / dY1, dfx / dZ1 |
570  * | ............................... |
571  * | ............................... |
572  * </pre>
573  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 3 columns
574  * @param pinholeCamera The pinhole camera to determine the jacobian values for
575  * @param flippedCamera_P_world Pose to determine the jacobian for (inverted and flipped)
576  * @param objectPoints 3D objects point to determine the jacobian for
577  * @param numberObjectPoints Number of 3D object points
578  * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
579  */
580  static void calculatePointJacobian2nx3(Scalar* jacobian, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_P_world, const Vector3* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
581 
582  /**
583  * Calculates the two jacobian rows for a given camera and image point.
584  * The jacobian is determined for the radial and tangential distortion parameters.<br>
585  * The resulting jacobian has the following form:
586  * <pre>
587  * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2 |
588  * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2 |
589  * </pre>
590  * @param jx First row of the jacobian, with 4 column entries
591  * @param jy Second row of the jacobian, with 4 column entries
592  * @param pinholeCamera The pinhole camera to determine the jacobian values for
593  * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
594  */
595  static void calculateCameraDistortionJacobian2x4(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
596 
597  /**
598  * Calculates the two jacobian rows for a given camera and image point.
599  * The jacobian is determined for the focal length, the principal point and the radial distortion parameters.<br>
600  * The resulting jacobian has the following form:
601  * <pre>
602  * | dfx / dk1, dfx / dk2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
603  * | dfy / dk1, dfy / dk2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
604  * </pre>
605  * @param jx First row of the jacobian, with 6 column entries
606  * @param jy Second row of the jacobian, with 6 column entries
607  * @param pinholeCamera The pinhole camera to determine the jacobian values for
608  * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
609  */
610  static void calculateCameraJacobian2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
611 
612  /**
613  * Calculates the two jacobian rows for a given camera and image point.
614  * The jacobian is determined for the focal length (same for horizontal and vertical axis), the principal point and the radial and tangential distortion parameters.<br>
615  * The resulting jacobian has the following form:
616  * <pre>
617  * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dF, dfx / dmx, dfx / dmy |
618  * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dF, dfy / dmx, dfy / dmy |
619  * </pre>
620  * @param jx First row of the jacobian, with 7 column entries
621  * @param jy Second row of the jacobian, with 7 column entries
622  * @param pinholeCamera The pinhole camera to determine the jacobian values for
623  * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
624  */
625  static void calculateCameraJacobian2x7(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
626 
627  /**
628  * Calculates the two jacobian rows for a given camera and image point.
629  * The jacobian is determined for the focal length, the principal point and the radial and tangential distortion parameters.<br>
630  * The resulting jacobian has the following form:
631  * <pre>
632  * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
633  * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
634  * </pre>
635  * @param jx First row of the jacobian, with 8 column entries
636  * @param jy Second row of the jacobian, with 8 column entries
637  * @param pinholeCamera The pinhole camera to determine the jacobian values for
638  * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
639  */
640  static void calculateCameraJacobian2x8(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
641 
642  /**
643  * Calculates the two jacobian rows for a given (orientational pose) and a camera and a static object point.
644  * The jacobian is determined for the three rotational angle of the pose, the radial and tangential distortion of the camera and the intrinsic parameters of the camera.<br>
645  * The resulting jacobian has the following form:
646  * <pre>
647  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
648  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
649  * </pre>
650  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and radial distortion k1, k2, tangential distortion p1, p2, focal parameters Fx, Fy and principal point (mx, my).<br>
651  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
652  * @param jx First row of the jacobian, with 11 column entries
653  * @param jy Second row of the jacobian, with 11 column entries
654  * @param pinholeCamera The pinhole camera to determine the jacobian values for
655  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
656  * @param objectPoint 3D object point to determine the jacobian for
657  */
658  static void calculateOrientationCameraJacobianRodrigues2x11(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Vector3& objectPoint);
659 
660  /**
661  * Calculates the two jacobian rows for a given (orientational pose) and a camera and a set of static object points.
662  * The jacobian is determined for the three rotational angle of the pose, the radial and tangential distortion of the camera and the intrinsic parameters of the camera.<br>
663  * The resulting jacobian has the following form:<br>
664  * <pre>
665  * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
666  * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
667  * </pre>
668  * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and radial distortion k1, k2, tangential distortion p1, p2, focal parameters Fx, Fy and principal point (mx, my).<br>
669  * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
670  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 11 columns
671  * @param pinholeCamera The pinhole camera to determine the jacobian values for
672  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
673  * @param objectPoints The accessor providing the 3D object points to determine the jacobian for
674  */
675  static void calculateOrientationCameraJacobianRodrigues2nx11(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const ConstIndexedAccessor<Vector3>& objectPoints);
676 
677  /**
678  * Calculates the entire jacobian matrix for an object point to image point transformation covering a flexible 6-DOF camera pose, the four intrinsic camera parameters and two parameters for radial distortion.
679  * The resulting jacobian has the following form:
680  * <pre>
681  * | dfx / dk1, dfx / dk2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
682  * | dfy / dk1, dfy / dk2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
683  * </pre>
684  * @param jx First row of the jacobian, with 12 column entries
685  * @param jy Second row of the jacobian, with 12 column entries
686  * @param pinholeCamera The pinhole camera to determine the jacobian values for
687  * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
688  * @param objectPoint 3D object point to determine the jacobian for
689  */
690  static void calculateJacobianCameraPoseRodrigues2x12(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Vector3& objectPoint);
691 
692  /**
693  * Calculates the entire jacobian matrix for an object point to image point transformation covering a flexible 6-DOF camera pose, the four intrinsic camera parameters and two parameters for radial distortion.
694  * The resulting jacobian has the following form:
695  * <pre>
696  * | dfx / dk1, dfx / dk2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
697  * | dfy / dk1, dfy / dk2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
698  * </pre>
699  * @param jx First row of the jacobian, with 12 column entries
700  * @param jy Second row of the jacobian, with 12 column entries
701  * @param pinholeCamera The pinhole camera to determine the jacobian values for
702  * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
703  * @param flippedCamera_P_world Inverted and flipped pose identical to 'flippedCamera_T_world'
704  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
705  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
706  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
707  * @param objectPoint 3D object point to determine the jacobian for
708  */
709  static void calculateJacobianCameraPoseRodrigues2x12(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Pose& flippedCamera_P_world, const Vector3& objectPoint, const SquareMatrix3& dwx, const SquareMatrix3& dwy, const SquareMatrix3& dwz);
710 
711  /**
712  * Calculates the entire jacobian matrix for an object point to image point transformation covering a flexible 6-DOF camera pose, the four intrinsic camera parameters and four parameters for radial and tangential distortion.
713  * The resulting jacobian has the following form:
714  * <pre>
715  * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
716  * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
717  * </pre>
718  * @param jx First row of the jacobian, with 14 column entries
719  * @param jy Second row of the jacobian, with 14 column entries
720  * @param pinholeCamera The pinhole camera to determine the jacobian values for
721  * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
722  * @param objectPoint 3D object point to determine the jacobian for
723  */
724  static void calculateJacobianCameraPoseRodrigues2x14(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Vector3& objectPoint);
725 
726  /**
727  * Calculates the entire jacobian matrix for an object point to image point transformation covering a flexible 6-DOF camera pose, the four intrinsic camera parameters and four parameters for radial and tangential distortion.
728  * The resulting jacobian has the following form:
729  * <pre>
730  * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
731  * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
732  * </pre>
733  * @param jx First row of the jacobian, with 14 column entries
734  * @param jy Second row of the jacobian, with 14 column entries
735  * @param pinholeCamera The pinhole camera to determine the jacobian values for
736  * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
737  * @param flippedCamera_P_world Inverted and flipped pose identical to 'flippedCamera_T_world'
738  * @param objectPoint 3D object point to determine the jacobian for
739  * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
740  * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
741  * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
742  * @see calculateRotationRodriguesDerivative().
743  */
744  static void calculateJacobianCameraPoseRodrigues2x14(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Pose& flippedCamera_P_world, const Vector3& objectPoint, const SquareMatrix3& dwx, const SquareMatrix3& dwy, const SquareMatrix3& dwz);
745 
746  /**
747  * Calculates the two jacobian rows for a given (6-DOF pose) and a camera and a set of static object points.
748  * The resulting jacobian has the following form:
749  * <pre>
750  * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
751  * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
752  * </pre>
753  * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 14 columns
754  * @param pinholeCamera The pinhole camera to determine the jacobian values for
755  * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
756  * @param objectPoints The accessor providing the 3D object points to determine the jacobian for
757  */
758  static void calculateJacobianCameraPoseRodrigues2nx14(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const ConstIndexedAccessor<Vector3>& objectPoints);
759 
760  /**
761  * Determines the 2x8 Jacobian of a homography function that transforms a 2D coordinate (interpreted as a 3D vector with homogeneous extension) to a 2D coordinate (the de-homogenization is included).
762  * This Jacobian can be used e.g., for additive image alignment.<br>
763  * The resulting jacobian has the following form:
764  * <pre>
765  * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7 |
766  * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7 |
767  * </pre>
768  * @param jx First row of the jacobian, with 8 column entries
769  * @param jy Second row of the jacobian, with 8 column entries
770  * @param x The horizontal coordinate to be used
771  * @param y The vertical coordinate to be used
772  * @param homography The homography for which the Jacobian will be determined, must have 1 in the lower right corner
773  * @see calculateHomographyJacobian2x9(), calculateIdentityHomographyJacobian2x8().
774  */
775  static void calculateHomographyJacobian2x8(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& homography);
776 
777  /**
778  * Determines the 2x9 Jacobian of a homography function that transforms a 2D coordinate (interpreted as a 3D vector with homogeneous extension) to a 2D coordinate (the de-homogenization is included).
779  * This Jacobian can be used e.g., for additive image alignment.<br>
780  * The resulting jacobian has the following form:
781  * <pre>
782  * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7, dfx / dh8 |
783  * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7, dfx / dh8 |
784  * </pre>
785  * @param jx First row of the jacobian, with 9 column entries
786  * @param jy Second row of the jacobian, with 9 column entries
787  * @param x The horizontal coordinate to be used
788  * @param y The vertical coordinate to be used
789  * @param homography The homography for which the Jacobian will be determined, must have 1 in the lower right corner
790  * @see calculateHomographyJacobian2x8(), calculateIdentityHomographyJacobian2x9().
791  */
792  static void calculateHomographyJacobian2x9(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& homography);
793 
794  /**
795  * Determines the 2x8 Jacobian of the identity homography function that transforms a 2D coordinate (interpreted as a 3D vector with homogeneous extension) to a 2D coordinate (the de-homogenization is included).
796  * This Jacobian can be used e.g., for inverse compositional image alignment..<br>
797  * The resulting jacobian has the following form:
798  * <pre>
799  * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7 |
800  * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7 |
801  * </pre>
802  * @param jx First row of the jacobian, with 8 column entries
803  * @param jy Second row of the jacobian, with 8 column entries
804  * @param x The horizontal coordinate to be used
805  * @param y The vertical coordinate to be used
806  * @see calculateIdentityHomographyJacobian2x9(), calculateHomographyJacobian2x8().
807  */
808  static void calculateIdentityHomographyJacobian2x8(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y);
809 
810  /**
811  * Determines the 2x9 Jacobian of the identity homography function that transforms a 2D coordinate (interpreted as a 3D vector with homogeneous extension) to a 2D coordinate (the de-homogenization is included).
812  * This Jacobian can be used e.g., for inverse compositional image alignment.<br>
813  * The resulting jacobian has the following form:
814  * <pre>
815  * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7, dfx / dh8 |
816  * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7, dfx / dh8 |
817  * </pre>
818  * @param jx First row of the jacobian, with 9 column entries
819  * @param jy Second row of the jacobian, with 9 column entries
820  * @param x The horizontal coordinate to be used
821  * @param y The vertical coordinate to be used
822  * @see calculateIdentityHomographyJacobian2x8(), calculateHomographyJacobian2x9().
823  */
824  static void calculateIdentityHomographyJacobian2x9(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y);
825 
826  /**
827  * Determines the 2x4 Jacobian of a similarity transformation that transforms a 2D coordinate (interpreted as a 3D vector with homogeneous extension) to a 2D coordinate (the de-homogenization is included).
828  * This Jacobian can be used e.g., for additive image alignment.<br>
829  * The resulting jacobian has the following form:
830  * <pre>
831  * | dfx / ds0, dfx / ds1, dfx / ds2, dfx / ds3 |
832  * | dfy / ds0, dfy / ds1, dfy / ds2, dfy / ds3 |
833  * </pre>
834  * @param jx First row of the jacobian, with 4 column entries
835  * @param jy Second row of the jacobian, with 4 column entries
836  * @param x The horizontal coordinate to be used
837  * @param y The vertical coordinate to be used
838  * @param similarity The similarity for which the Jacobian will be determined, must have 1 in the lower right corner
839  */
840  static inline void calculateSimilarityJacobian2x4(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& similarity);
841 
842  /**
843  * Determines the 2x2 Jacobian of distorting a normalized image point in a fisheye camera with radial and tangential distortion.
844  * The resulting jacobian has the following form:
845  * <pre>
846  * | dfx / dx, dfx / dy |
847  * | dfy / dx, dfy / dy |
848  * </pre>
849  * @param jx First row of the jacobian, with 2 column entries, must be valid
850  * @param jy Second row of the jacobian, with 2 column entries, must be valid
851  * @param x The horizontal coordinate of the normalized image point to be distorted
852  * @param y The vertical coordinate of the normalized image point to be distorted
853  * @param radialDistortion The six radial distortion parameters, must be valid
854  * @param tangentialDistortion The two radial distortion parameters, must be valid
855  * @tparam T The data type of a scalar, 'float' or 'double'
856  */
857  template <typename T>
858  static void calculateFisheyeDistortNormalized2x2(T* jx, T* jy, const T x, const T y, const T* radialDistortion, const T* tangentialDistortion);
859 };
860 
861 inline void Jacobian::calculatePoseJacobianRodrigues2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Vector3& objectPoint, const bool distortImagePoint)
862 {
863  SquareMatrix3 dwx, dwy, dwz;
864  calculateRotationRodriguesDerivative(ExponentialMap(Vector3(flippedCamera_P_world.rx(), flippedCamera_P_world.ry(), flippedCamera_P_world.rz())), dwx, dwy, dwz);
865 
866  calculatePoseJacobianRodrigues2x6(jx, jy, pinholeCamera, flippedCamera_P_world.transformation(), objectPoint, distortImagePoint, dwx, dwy, dwz);
867 }
868 
869 inline void Jacobian::calculateObjectTransformation2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3& objectPoint)
870 {
871  SquareMatrix3 Rwx, Rwy, Rwz;
872  calculateRotationRodriguesDerivative(ExponentialMap(Vector3(objectPose.rx(), objectPose.ry(), objectPose.rz())), Rwx, Rwy, Rwz);
873 
874  calculateObjectTransformation2x6(jx, jy, pinholeCamera, extrinsicIF, objectPose, objectPoint, Rwx, Rwy, Rwz);
875 }
876 
877 inline void Geometry::Jacobian::calculateSimilarityJacobian2x4(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& similarity)
878 {
879  ocean_assert(jx != nullptr && jy != nullptr);
880 
881  ocean_assert_and_suppress_unused(Numeric::isEqual(similarity(2, 0), 0), similarity);
882  ocean_assert(Numeric::isEqual(similarity(2, 1), 0));
883  ocean_assert(Numeric::isEqual(similarity(2, 2), 1));
884 
885  // Similarity:
886  // | s0 -s1 s2 |
887  // | s1 s0 s3 |
888  // | 0 0 1 |
889 
890  // sx(x, y) = s0*x - s1*y + s2
891  // sy(x, y) = s1*x + s0*y + s3
892 
893  // Jacobian: x -y 1 0
894  // y x 0 1
895 
896  jx[0] = x;
897  jx[1] = -y;
898  jx[2] = 1;
899  jx[3] = 0;
900 
901  jy[0] = y;
902  jy[1] = x;
903  jy[2] = 0;
904  jy[3] = 1;
905 }
906 
907 template <typename T, typename TRotation>
908 OCEAN_FORCE_INLINE void Jacobian::calculateOrientationalJacobianRodrigues2x3IF(const AnyCameraT<T>& anyCamera, const TRotation& flippedCamera_R_translation, const VectorT3<T>& translation_T_world, const VectorT3<T>& worldObjectPoint, const SquareMatrixT3<T>& Rwx, const SquareMatrixT3<T>& Rwy, const SquareMatrixT3<T>& Rwz, T* jx, T* jy)
909 {
910  static_assert(std::is_same<TRotation, QuaternionT<T>>::value || std::is_same<TRotation, SquareMatrixT3<T>>::value, "Invalid rotation type!");
911 
912  ocean_assert(anyCamera.isValid());
913  ocean_assert(jx != nullptr && jy != nullptr);
914 
915 #ifdef OCEAN_DEBUG
916  if constexpr (std::is_same<TRotation, QuaternionT<T>>::value)
917  {
918  ocean_assert(flippedCamera_R_translation.isValid());
919  }
920  else
921  {
922  ocean_assert(flippedCamera_R_translation.isOrthonormal());
923  }
924 #endif
925 
926  /**
927  * f = fC(fR(fT(X))
928  *
929  * with fC camera function, fR object point rotation function, fT object point translation function
930  *
931  * fR(fT(X)) = R(X + t)
932  */
933 
934  const VectorT3<T> translatedWorldObjectPoint = translation_T_world + worldObjectPoint;
935  const VectorT3<T> flippedCameraObjectPoint = flippedCamera_R_translation * translatedWorldObjectPoint;
936 
937  T jxPoint[3];
938  T jyPoint[3];
939 
940  // let's determine the left 2x3 sub-matrix first
941  anyCamera.pointJacobian2x3IF(flippedCameraObjectPoint, jxPoint, jyPoint);
942 
943  const VectorT3<T> dwx(Rwx * translatedWorldObjectPoint);
944  const VectorT3<T> dwy(Rwy * translatedWorldObjectPoint);
945  const VectorT3<T> dwz(Rwz * translatedWorldObjectPoint);
946 
947  // now, we apply the chain rule to determine the 2x3 Jacobian
948  jx[0] = jxPoint[0] * dwx[0] + jxPoint[1] * dwx[1] + jxPoint[2] * dwx[2];
949  jx[1] = jxPoint[0] * dwy[0] + jxPoint[1] * dwy[1] + jxPoint[2] * dwy[2];
950  jx[2] = jxPoint[0] * dwz[0] + jxPoint[1] * dwz[1] + jxPoint[2] * dwz[2];
951 
952  jy[0] = jyPoint[0] * dwx[0] + jyPoint[1] * dwx[1] + jyPoint[2] * dwx[2];
953  jy[1] = jyPoint[0] * dwy[0] + jyPoint[1] * dwy[1] + jyPoint[2] * dwy[2];
954  jy[2] = jyPoint[0] * dwz[0] + jyPoint[1] * dwz[1] + jyPoint[2] * dwz[2];
955 }
956 
957 template <typename T>
958 OCEAN_FORCE_INLINE void Jacobian::calculatePoseJacobianRodrigues2x6IF(const AnyCameraT<T>& anyCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, const SquareMatrixT3<T>& Rwx, const SquareMatrixT3<T>& Rwy, const SquareMatrixT3<T>& Rwz, T* jx, T* jy)
959 {
960  ocean_assert(anyCamera.isValid());
961  ocean_assert(flippedCamera_T_world.isValid());
962  ocean_assert(jx != nullptr && jy != nullptr);
963 
964  anyCamera.pointJacobian2x3IF(flippedCamera_T_world * worldObjectPoint, jx + 3, jy + 3);
965 
966  const VectorT3<T> dwx(Rwx * worldObjectPoint);
967  const VectorT3<T> dwy(Rwy * worldObjectPoint);
968  const VectorT3<T> dwz(Rwz * worldObjectPoint);
969 
970  // now, we apply the chain rule to determine the left 2x3 sub-matrix
971  jx[0] = jx[3] * dwx[0] + jx[4] * dwx[1] + jx[5] * dwx[2];
972  jx[1] = jx[3] * dwy[0] + jx[4] * dwy[1] + jx[5] * dwy[2];
973  jx[2] = jx[3] * dwz[0] + jx[4] * dwz[1] + jx[5] * dwz[2];
974 
975  jy[0] = jy[3] * dwx[0] + jy[4] * dwx[1] + jy[5] * dwx[2];
976  jy[1] = jy[3] * dwy[0] + jy[4] * dwy[1] + jy[5] * dwy[2];
977  jy[2] = jy[3] * dwz[0] + jy[4] * dwz[1] + jy[5] * dwz[2];
978 }
979 
980 template <typename T>
981 OCEAN_FORCE_INLINE void Jacobian::calculatePointJacobian2x3IF(const AnyCameraT<T>& anyCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, T* jx, T* jy)
982 {
983  ocean_assert(anyCamera.isValid());
984  ocean_assert(flippedCamera_T_world.isValid());
985  ocean_assert(jx != nullptr && jy != nullptr);
986 
987  // | Fx 0 | | df_distx_u df_distx_v | | 1/W 0 -U/W^2 |
988  // | 0 Fy | * | df_disty_u df_disty_v | * | 0 1/W -V/W^2 | * R
989 
990  T pointJacobian2x3[6];
991  anyCamera.pointJacobian2x3IF(flippedCamera_T_world * worldObjectPoint, pointJacobian2x3 + 0, pointJacobian2x3 + 3);
992 
993  jx[0] = pointJacobian2x3[0] * flippedCamera_T_world[0] + pointJacobian2x3[1] * flippedCamera_T_world[1] + pointJacobian2x3[2] * flippedCamera_T_world[2];
994  jx[1] = pointJacobian2x3[0] * flippedCamera_T_world[4] + pointJacobian2x3[1] * flippedCamera_T_world[5] + pointJacobian2x3[2] * flippedCamera_T_world[6];
995  jx[2] = pointJacobian2x3[0] * flippedCamera_T_world[8] + pointJacobian2x3[1] * flippedCamera_T_world[9] + pointJacobian2x3[2] * flippedCamera_T_world[10];
996 
997  jy[0] = pointJacobian2x3[3] * flippedCamera_T_world[0] + pointJacobian2x3[4] * flippedCamera_T_world[1] + pointJacobian2x3[5] * flippedCamera_T_world[2];
998  jy[1] = pointJacobian2x3[3] * flippedCamera_T_world[4] + pointJacobian2x3[4] * flippedCamera_T_world[5] + pointJacobian2x3[5] * flippedCamera_T_world[6];
999  jy[2] = pointJacobian2x3[3] * flippedCamera_T_world[8] + pointJacobian2x3[4] * flippedCamera_T_world[9] + pointJacobian2x3[5] * flippedCamera_T_world[10];
1000 }
1001 
1002 template <typename T>
1003 void Jacobian::calculateFisheyeDistortNormalized2x2(T* jx, T* jy, const T x, const T y, const T* radialDistortion, const T* tangentialDistortion)
1004 {
1005  ocean_assert(jx != nullptr && jy != nullptr);
1006  ocean_assert(radialDistortion != nullptr && tangentialDistortion != nullptr);
1007 
1008  const T& k3 = radialDistortion[0];
1009  const T& k5 = radialDistortion[1];
1010  const T& k7 = radialDistortion[2];
1011  const T& k9 = radialDistortion[3];
1012  const T& k11 = radialDistortion[4];
1013  const T& k13 = radialDistortion[5];
1014 
1015  const T& p1 = tangentialDistortion[0];
1016  const T& p2 = tangentialDistortion[1];
1017 
1018  const T x2 = x * x;
1019  const T y2 = y * y;
1020 
1021  const T xy2 = x2 + y2;
1022 
1023  const T r = NumericT<T>::sqrt(xy2);
1024  const T r3 = r * r * r;
1025 
1026  const T t = NumericT<T>::atan(r);
1027  const T t2 = t * t;
1028  const T t3 = t2 * t;
1029  const T t4 = t3 * t;
1030  const T t5 = t4 * t;
1031  const T t6 = t5 * t;
1032  const T t7 = t6 * t;
1033  const T t8 = t7 * t;
1034  const T t9 = t8 * t;
1035  const T t10 = t9 * t;
1036  const T t11 = t10 * t;
1037  const T t12 = t11 * t;
1038  const T t13 = t12 * t;
1039 
1040  const T term0 = k13 * t13 + k11 * t11 + k9 * t9 + k7 * t7 + k5 * t5 + k3 * t3 + t;
1041  const T term1 = 13 * k13 * t12 + 11 * k11 * t10 + 9 * k9 * t8 + 7 * k7 * t6 + 5 * k5 * t4 + 3 * k3 * t2 + 1;
1042 
1043  const T term2 = (xy2 + 1) * term0;
1044  const T term3 = r3 * (xy2 + 1);
1045  const T invTerm3 = T(1) / term3;
1046 
1047  const T xDistortion_dx = (xy2 * term2 - x2 * term2 + x2 * r * term1) * invTerm3;
1048  const T xDistortion_dy = (x * term1 * y) / (xy2 * (xy2 + 1)) - (x * y * term0) / r3;
1049 
1050  //const T yDistortion_dx = (y * term1 * x) / (xy2 * (xy2 + 1)) - (y * x * term0) / r3; == xDistortion_dy
1051  const T& yDistortion_dx = xDistortion_dy;
1052  const T yDistortion_dy = (xy2 * term2 - y2 * term2 + y2 * r * term1) * invTerm3;
1053 
1054  const T radialDistortionFactor = term0 / r;
1055 
1056  const T rx = x * radialDistortionFactor;
1057  const T ry = y * radialDistortionFactor;
1058 
1059  const T xTangential_dx = 6 * p1 * rx + 2 * p2 * ry + 1;
1060  const T xTangential_dy = 2 * p1 * ry + 2 * p2 * rx;
1061 
1062  // const T yTangential_dx = 2 * p2 * rx + 2 * p1 * ry; // == yTangential_dx
1063  const T& yTangential_dx = xTangential_dy;
1064  const T yTangential_dy = 6 * p2 * ry + 2 * p1 * rx + 1;
1065 
1066  // chain rule
1067  // | xTangential_dx xTangential_dy | | xDistortion_dx xDistortion_dy |
1068  // | yTangential_dx yTangential_dy | * | yDistortion_dx yDistortion_dy |
1069 
1070  jx[0] = xTangential_dx * xDistortion_dx + xTangential_dy * yDistortion_dx;
1071  jx[1] = xTangential_dx * xDistortion_dy + xTangential_dy * yDistortion_dy;
1072 
1073  jy[0] = yTangential_dx * xDistortion_dx + yTangential_dy * yDistortion_dx;
1074  jy[1] = yTangential_dx * xDistortion_dy + yTangential_dy * yDistortion_dy;
1075 }
1076 
1077 }
1078 
1079 }
1080 
1081 #endif // META_OCEAN_GEOMETRY_JACOBIAN_H
Ocean::AnyCameraT
This class implements the abstract base class for all AnyCamera objects.
Definition: AnyCamera.h:130
Ocean::AnyCameraT::pointJacobian2x3IF
virtual void pointJacobian2x3IF(const VectorT3< T > &flippedCameraObjectPoint, T *jx, T *jy) const =0
Calculates the 2x3 jacobian matrix for the 3D object point projection into the camera frame.
Ocean::AnyCameraT::isValid
virtual bool isValid() const =0
Returns whether this camera is valid.
Ocean::ConstIndexedAccessor
This class implements a base class for all indexed-based accessors allowing a constant reference acce...
Definition: Accessor.h:241
Ocean::ExponentialMapT
This class implements an exponential map defining a rotation by three parameters.
Definition: ExponentialMap.h:67
Ocean::Geometry::Jacobian
This class implements function to calculate the jacobian matrices for geometry functions.
Definition: Jacobian.h:37
Ocean::Geometry::Jacobian::calculateObjectTransformation2x6
static void calculateObjectTransformation2x6(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &extrinsicIF, const Pose &objectPose, const Vector3 &objectPoint)
Calculates the two jacobian rows for a given (flexible) 6-DOF object transformation,...
Definition: Jacobian.h:869
Ocean::Geometry::Jacobian::calculateOrientationalJacobianRodrigues2x3IF
static OCEAN_FORCE_INLINE void calculateOrientationalJacobianRodrigues2x3IF(const AnyCameraT< T > &anyCamera, const TRotation &flippedCamera_R_translation, const VectorT3< T > &translation_T_world, const VectorT3< T > &worldObjectPoint, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz, T *jx, T *jy)
Calculates the two Jacobian rows for the 3-DOF rotational part of a 6-DOF camera pose and a given 3D ...
Definition: Jacobian.h:908
Ocean::Geometry::Jacobian::calculateSphericalObjectPointOrientationJacobian2x3IF
static void calculateSphericalObjectPointOrientationJacobian2x3IF(T *jx, T *jy, const AnyCameraT< T > &camera, const SquareMatrixT3< T > &flippedCamera_R_world, const ExponentialMapT< T > &sphericalObjectPoint, const T objectPointDistance)
Calculates the two jacobian rows for a given exponential rotation map representing the location of a ...
Ocean::Geometry::Jacobian::calculatePointJacobian2x3IF
static OCEAN_FORCE_INLINE void calculatePointJacobian2x3IF(const AnyCameraT< T > &anyCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &worldObjectPoint, T *jx, T *jy)
Calculates the two jacobian rows for a given pose and dynamic object point.
Definition: Jacobian.h:981
Ocean::Geometry::Jacobian::calculateOrientationJacobianRodrigues2nx3
static void calculateOrientationJacobianRodrigues2nx3(Scalar *jacobian, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const ConstIndexedAccessor< Vector3 > &objectPoints, const bool distortImagePoints)
Deprecated.
Ocean::Geometry::Jacobian::calculateOrientationJacobianRodrigues2nx3IF
static void calculateOrientationJacobianRodrigues2nx3IF(T *jacobian, const AnyCameraT< T > &camera, const ExponentialMapT< T > &flippedCamera_R_world, const ConstIndexedAccessor< VectorT3< T >> &objectPoints)
Calculates all 3-DOF orientational jacobian rows for a given (flexible) camera pose and a set of stat...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2x6IF
static OCEAN_FORCE_INLINE void calculatePoseJacobianRodrigues2x6IF(const AnyCameraT< T > &anyCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &worldObjectPoint, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz, T *jx, T *jy)
Calculates the two jacobian rows for a given (flexible) 6-DOF camera pose and one static 3D object po...
Definition: Jacobian.h:958
Ocean::Geometry::Jacobian::calculateFisheyeDistortNormalized2x2
static void calculateFisheyeDistortNormalized2x2(T *jx, T *jy, const T x, const T y, const T *radialDistortion, const T *tangentialDistortion)
Determines the 2x2 Jacobian of distorting a normalized image point in a fisheye camera with radial an...
Definition: Jacobian.h:1003
Ocean::Geometry::Jacobian::calculateSphericalObjectPointJacobian3x3
static void calculateSphericalObjectPointJacobian3x3(T *jx, T *jy, T *jz, const ExponentialMapT< T > &sphericalObjectPoint, const T objectPointDistance)
Calculates the three jacobian rows for a given exponential rotation map representing the location of ...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2x6
static void calculatePoseJacobianRodrigues2x6(Scalar *jx, Scalar *jy, const FisheyeCamera &fisheyeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Vector3 &worldObjectPoint, const SquareMatrix3 &dwx, const SquareMatrix3 &dwy, const SquareMatrix3 &dwz)
Deprecated.
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2nx6
static void calculatePoseJacobianRodrigues2nx6(Scalar *jacobian, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const Vector3 *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints)
Deprecated.
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2nx14
static void calculateJacobianCameraPoseRodrigues2nx14(Scalar *jacobian, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const ConstIndexedAccessor< Vector3 > &objectPoints)
Calculates the two jacobian rows for a given (6-DOF pose) and a camera and a set of static object poi...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodriguesDampedDistortion2nx6
static void calculatePoseJacobianRodriguesDampedDistortion2nx6(Scalar *jacobian, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const Scalar dampingFactor, const Vector3 *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints)
Calculates all pose jacobian rows for a given (flexible) pose with a static camera profile supporting...
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2x14
static void calculateJacobianCameraPoseRodrigues2x14(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Vector3 &objectPoint)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2x5
static void calculatePoseJacobianRodrigues2x5(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const ExponentialMap &rotation, const Vector2 &translation, const Vector3 &objectPoint)
Calculates the two jacobian rows for a given pose with translation scale ambiguities and static objec...
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2x12
static void calculateJacobianCameraPoseRodrigues2x12(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Vector3 &objectPoint)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
Ocean::Geometry::Jacobian::calculatePointJacobian2nx3
static void calculatePointJacobian2nx3(Scalar *jacobian, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_P_world, const Vector3 *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints)
Calculates the two jacobian rows for a given pose and several dynamic object points.
Ocean::Geometry::Jacobian::calculateObjectTransformation2nx6
static void calculateObjectTransformation2nx6(Scalar *jacobian, const FisheyeCamera &fisheyeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Pose &world_T_object, const Vector3 *objectPoints, const size_t numberObjectPoints)
Calculates all pose jacobian rows for a given (flexible) 6-DOF object transformation,...
Ocean::Geometry::Jacobian::calculatePoseZoomJacobianRodrigues2nx7
static void calculatePoseZoomJacobianRodrigues2nx7(Scalar *jacobian, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const Scalar zoom, const Vector3 *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints)
Calculates all pose jacobian rows for a given (flexible) pose with (flexible) zoom factor and a set o...
Ocean::Geometry::Jacobian::calculateHomographyJacobian2x9
static void calculateHomographyJacobian2x9(Scalar *jx, Scalar *jy, const Scalar x, const Scalar y, const SquareMatrix3 &homography)
Determines the 2x9 Jacobian of a homography function that transforms a 2D coordinate (interpreted as ...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2x6
static void calculatePoseJacobianRodrigues2x6(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_P_world, const Vector3 &objectPoint, const bool distortImagePoint, const SquareMatrix3 &dwx, const SquareMatrix3 &dwy, const SquareMatrix3 &dwz)
Deprecated.
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2nx6IF
static void calculatePoseJacobianRodrigues2nx6IF(T *jacobian, const AnyCameraT< T > &camera, const PoseT< T > &flippedCamera_P_world, const VectorT3< T > *objectPoints, const size_t numberObjectPoints)
Calculates all jacobian rows for a given (flexible) 6-DOF camera pose with a static camera profile an...
Ocean::Geometry::Jacobian::calculateObjectTransformation2x6
static void calculateObjectTransformation2x6(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &extrinsicIF, const Pose &objectPose, const Vector3 &objectPoint, const SquareMatrix3 &dwx, const SquareMatrix3 &dwy, const SquareMatrix3 &dwz)
Calculates the two jacobian rows for a given (flexible) 6-DOF object transformation,...
Ocean::Geometry::Jacobian::calculateCameraDistortionJacobian2x4
static void calculateCameraDistortionJacobian2x4(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Vector2 &normalizedImagePoint)
Calculates the two jacobian rows for a given camera and image point.
Ocean::Geometry::Jacobian::calculateCameraJacobian2x6
static void calculateCameraJacobian2x6(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Vector2 &normalizedImagePoint)
Calculates the two jacobian rows for a given camera and image point.
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2x6
static void calculatePoseJacobianRodrigues2x6(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const Vector3 &objectPoint, const bool distortImagePoint)
Calculates the two jacobian rows for a given (flexible) pose and static camera and one static 3D obje...
Definition: Jacobian.h:861
Ocean::Geometry::Jacobian::calculateCameraJacobian2x7
static void calculateCameraJacobian2x7(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Vector2 &normalizedImagePoint)
Calculates the two jacobian rows for a given camera and image point.
Ocean::Geometry::Jacobian::calculateSimilarityJacobian2x4
static void calculateSimilarityJacobian2x4(Scalar *jx, Scalar *jy, const Scalar x, const Scalar y, const SquareMatrix3 &similarity)
Determines the 2x4 Jacobian of a similarity transformation that transforms a 2D coordinate (interpret...
Definition: Jacobian.h:877
Ocean::Geometry::Jacobian::calculatePointJacobian2x3
static void calculatePointJacobian2x3(Scalar *jx, Scalar *jy, const FisheyeCamera &fisheyeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Vector3 &worldObjectPoint)
Calculates the two jacobian rows for a given pose and dynamic object point.
Ocean::Geometry::Jacobian::calculateRotationRodriguesDerivative
static void calculateRotationRodriguesDerivative(const ExponentialMapT< T > &rotation, SquareMatrixT3< T > &dwx, SquareMatrixT3< T > &dwy, SquareMatrixT3< T > &dwz)
This function determines the 3x3 Jacobian of a rotation function rotating a 3D object point by applic...
Ocean::Geometry::Jacobian::calculateObjectTransformation2nx6
static void calculateObjectTransformation2nx6(Scalar *jacobian, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &extrinsicIF, const Pose &objectPose, const Vector3 *objectPoints, const size_t numberObjectPoints)
Calculates all pose jacobian rows for a given (flexible) 6-DOF object transformation,...
Ocean::Geometry::Jacobian::calculateIdentityHomographyJacobian2x8
static void calculateIdentityHomographyJacobian2x8(Scalar *jx, Scalar *jy, const Scalar x, const Scalar y)
Determines the 2x8 Jacobian of the identity homography function that transforms a 2D coordinate (inte...
Ocean::Geometry::Jacobian::calculateIdentityHomographyJacobian2x9
static void calculateIdentityHomographyJacobian2x9(Scalar *jx, Scalar *jy, const Scalar x, const Scalar y)
Determines the 2x9 Jacobian of the identity homography function that transforms a 2D coordinate (inte...
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2x12
static void calculateJacobianCameraPoseRodrigues2x12(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Pose &flippedCamera_P_world, const Vector3 &objectPoint, const SquareMatrix3 &dwx, const SquareMatrix3 &dwy, const SquareMatrix3 &dwz)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
Ocean::Geometry::Jacobian::calculateCameraJacobian2x8
static void calculateCameraJacobian2x8(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Vector2 &normalizedImagePoint)
Calculates the two jacobian rows for a given camera and image point.
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2x14
static void calculateJacobianCameraPoseRodrigues2x14(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_T_world, const Pose &flippedCamera_P_world, const Vector3 &objectPoint, const SquareMatrix3 &dwx, const SquareMatrix3 &dwy, const SquareMatrix3 &dwz)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
Ocean::Geometry::Jacobian::calculateOrientationCameraJacobianRodrigues2x11
static void calculateOrientationCameraJacobianRodrigues2x11(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const Vector3 &objectPoint)
Calculates the two jacobian rows for a given (orientational pose) and a camera and a static object po...
Ocean::Geometry::Jacobian::calculatePointJacobian2x3
static void calculatePointJacobian2x3(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const HomogenousMatrix4 &flippedCamera_P_world, const Vector3 &objectPoint, const bool distortImagePoint)
Calculates the two jacobian rows for a given pose and dynamic object point.
Ocean::Geometry::Jacobian::calculateHomographyJacobian2x8
static void calculateHomographyJacobian2x8(Scalar *jx, Scalar *jy, const Scalar x, const Scalar y, const SquareMatrix3 &homography)
Determines the 2x8 Jacobian of a homography function that transforms a 2D coordinate (interpreted as ...
Ocean::Geometry::Jacobian::calculateOrientationCameraJacobianRodrigues2nx11
static void calculateOrientationCameraJacobianRodrigues2nx11(Scalar *jacobian, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const ConstIndexedAccessor< Vector3 > &objectPoints)
Calculates the two jacobian rows for a given (orientational pose) and a camera and a set of static ob...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2nx5
static void calculatePoseJacobianRodrigues2nx5(Scalar *jacobian, const PinholeCamera &pinholeCamera, const ExponentialMap &rotation, const Vector2 &translation, const Vector3 *objectPoints, const size_t numberObjectPoints)
Calculates all jacobian rows for a given pose with translation scale ambiguities and a set of static ...
Ocean::Geometry::Jacobian::calculatePoseZoomJacobianRodrigues2x7
static void calculatePoseZoomJacobianRodrigues2x7(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Pose &flippedCamera_P_world, const Scalar zoom, const Vector3 &objectPoint, const bool distortImagePoints)
Calculates the two jacobian rows for a given (flexible) pose with (flexible) zoom factor and one stat...
Ocean::HomogenousMatrixT4::isValid
bool isValid() const
Returns whether this matrix is a valid homogeneous transformation.
Definition: HomogenousMatrix4.h:1806
Ocean::NumericT::atan
static T atan(const T value)
Returns the arctangent of a given value.
Definition: Numeric.h:1616
Ocean::NumericT::sqrt
static T sqrt(const T value)
Returns the square root of a given value.
Definition: Numeric.h:1533
Ocean::NumericT::isEqual
static bool isEqual(const T first, const T second)
Returns whether two values are equal up to a small epsilon.
Definition: Numeric.h:2386
Ocean::PoseT::rx
T rx() const
Returns the x parameter of the rotation.
Definition: Pose.h:464
Ocean::PoseT::transformation
HomogenousMatrixT4< T > transformation() const
Returns the 4x4 homogeneous transformation matrix of this pose.
Ocean::PoseT::rz
T rz() const
Returns the z parameter of the rotation.
Definition: Pose.h:488
Ocean::PoseT::ry
T ry() const
Returns the y parameter of the rotation.
Definition: Pose.h:476
Ocean::QuaternionT
This class implements a unit quaternion rotation.
Definition: Quaternion.h:100
Ocean::SquareMatrixT3
This class implements a 3x3 square matrix.
Definition: SquareMatrix3.h:88
Ocean::VectorT3
This class implements a vector with three elements.
Definition: Vector3.h:97
Ocean::ExponentialMap
ExponentialMapT< Scalar > ExponentialMap
Definition of the ExponentialMap object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION flag either...
Definition: ExponentialMap.h:20
Ocean::Scalar
float Scalar
Definition of a scalar type.
Definition: Math.h:128
Ocean::Vector3
VectorT3< Scalar > Vector3
Definition of a 3D vector.
Definition: Vector3.h:22
Ocean
The namespace covering the entire Ocean framework.
Definition: Accessor.h:15