Ocean
Loading...
Searching...
No Matches
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
26namespace Ocean
27{
28
29namespace Geometry
30{
31
32/**
33 * This class implements function to calculate the jacobian matrices for geometry functions.
34 * @ingroup geometry
35 */
36class 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
861inline 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
869inline 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
877inline 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
907template <typename T, typename TRotation>
908OCEAN_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
957template <typename T>
958OCEAN_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
980template <typename T>
981OCEAN_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
1002template <typename T>
1003void 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::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::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::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::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:27
Ocean::Scalar
float Scalar
Definition of a scalar type.
Definition Math.h:129
Ocean::Vector3
VectorT3< Scalar > Vector3
Definition of a 3D vector.
Definition Vector3.h:29
Ocean
The namespace covering the entire Ocean framework.
Definition Accessor.h:15