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 * @tparam T The data type of a scalar, 'float' or 'double'
201 * @see calculatePoseJacobianRodrigues2nx6().
202 */
203 template <typename T>
204 static inline void calculatePoseJacobianRodrigues2x6(T* jx, T* jy, const PinholeCameraT<T>& pinholeCamera, const PoseT<T>& flippedCamera_P_world, const VectorT3<T>& objectPoint, const bool distortImagePoint);
205
206 /**
207 * Deprecated.
208 *
209 * Calculates the two jacobian rows for a given (flexible) pose and one static 3D object point.
210 * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
211 * The 6 derivatives are calculated for the 6DOF pose.<br>
212 * The resulting jacobian rows have the following form:
213 * <pre>
214 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
215 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
216 * </pre>
217 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
218 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
219 * @param jx First row of the jacobian, with 6 column entries
220 * @param jy Second row of the jacobian, with 6 column entries
221 * @param pinholeCamera The pinhole camera to determine the jacobian values for
222 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
223 * @param objectPoint 3D object point to determine the jacobian for
224 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
225 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
226 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
227 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
228 * @tparam T The data type of a scalar, 'float' or 'double'
229 * @see calculateRotationRodriguesDerivative().
230 */
231 template <typename T>
232 static void calculatePoseJacobianRodrigues2x6(T* jx, T* jy, const PinholeCameraT<T>& pinholeCamera, const HomogenousMatrixT4<T>& flippedCamera_P_world, const VectorT3<T>& objectPoint, const bool distortImagePoint, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz);
233
234 /**
235 * Deprecated.
236 *
237 * Calculates the two jacobian rows for a given (flexible) pose and one static 3D object point.
238 * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
239 * The 6 derivatives are calculated for the 6DOF pose.<br>
240 * The resulting jacobian rows have the following form:
241 * <pre>
242 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
243 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
244 * </pre>
245 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
246 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
247 * @param jx First row of the jacobian, with 6 column entries
248 * @param jy Second row of the jacobian, with 6 column entries
249 * @param fisheyeCamera Fisheye camera to determine the jacobian values for, must be valid
250 * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
251 * @param worldObjectPoint 3D object point to determine the jacobian for, in world coordinate system
252 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
253 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
254 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
255 * @see calculateRotationRodriguesDerivative().
256 */
257 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);
258
259 /**
260 * Calculates the two jacobian rows for a given (flexible) 6-DOF camera pose and one static 3D object point.
261 * This function uses the pre-calculated 3x3 Jacobian matrix of the camera's orientation provided by three separated 3x3 matrices.<br>
262 * The 6 derivatives are calculated for the 6DOF pose.<br>
263 * The resulting jacobian rows have the following form:
264 * <pre>
265 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
266 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
267 * </pre>
268 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
269 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
270 * @param anyCamera The camera profile to determine the jacobian values for, must be valid
271 * @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
272 * @param worldObjectPoint 3D object point to determine the jacobian for, in world coordinate system
273 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
274 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
275 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
276 * @param jx The resulting first row of the jacobian, with 6 column entries, must be valid
277 * @param jy The resulting second row of the jacobian, with 6 column entries, must be valid
278 * @see calculateRotationRodriguesDerivative().
279 * @tparam T The scalar data type
280 */
281 template <typename T>
282 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);
283
284 /**
285 * Calculates all jacobian rows for a given (flexible) 6-DOF camera pose with a static camera profile and several static 3D object points.
286 * The 6 derivatives are calculated for the entire 6DOF pose.<br>
287 * The resulting jacobian rows have the following form:
288 * <pre>
289 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
290 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
291 * </pre>
292 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
293 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
294 * @param jacobian First element in the first row of the (row major aligned) jacobian matrix, with 2 * numberObjectPoints rows and 6 columns
295 * @param camera The camera profile defining the projection, must be valid
296 * @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
297 * @param objectPoints The 3D object points to determine the jacobian for, must be valid
298 * @param numberObjectPoints The number of given object points, with range [1, infinity)
299 * @see calculatePoseJacobianRodrigues2x6IF().
300 */
301 template <typename T>
302 static void calculatePoseJacobianRodrigues2nx6IF(T* jacobian, const AnyCameraT<T>& camera, const PoseT<T>& flippedCamera_P_world, const VectorT3<T>* objectPoints, const size_t numberObjectPoints);
303
304 /**
305 * Deprecated.
306 *
307 * 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.
308 * The 6 derivatives are calculated for the entire 6DOF pose.<br>
309 * The resulting jacobian rows have the following form:
310 * <pre>
311 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
312 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
313 * </pre>
314 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
315 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
316 * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 6 columns
317 * @param pinholeCamera The pinhole camera to determine the jacobian values for
318 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
319 * @param objectPoints 3D object points to determine the jacobian for
320 * @param numberObjectPoints Number of given object points
321 * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
322 * @tparam T The data type of the scalar, e.g., 'float' or 'double'
323 * @see calculatePoseJacobianRodrigues2x6().
324 */
325 template <typename T>
326 static void calculatePoseJacobianRodrigues2nx6(T* jacobian, const PinholeCameraT<T>& pinholeCamera, const PoseT<T>& flippedCamera_P_world, const VectorT3<T>* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
327
328 /**
329 * 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.
330 * The distortion is damped for 3D object points not projecting into the camera frame but outside the camera frame.<br>
331 * The 6 derivatives are calculated for the entire 6DOF pose.<br>
332 * The resulting jacobian rows have the following form:
333 * <pre>
334 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
335 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
336 * </pre>
337 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
338 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
339 * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 6 columns
340 * @param pinholeCamera The pinhole camera to determine the jacobian values for
341 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
342 * @param dampingFactor The factor defining the boundary of the asymptotic damping behavior for normalized coordinates, with range [0, infinity)
343 * @param objectPoints 3D object points to determine the jacobian for
344 * @param numberObjectPoints Number of given object points
345 * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
346 * @tparam T The scalar data type, either 'float' or 'double'
347 * @see calculatePoseJacobianRodrigues2x6().
348 */
349 template <typename T>
350 static void calculatePoseJacobianRodriguesDampedDistortion2nx6(T* jacobian, const PinholeCameraT<T>& pinholeCamera, const PoseT<T>& flippedCamera_P_world, const T dampingFactor, const VectorT3<T>* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
351
352 /**
353 * Calculates the two jacobian rows for a given (flexible) pose with (flexible) zoom factor and one static 3D object point.
354 * The 7 derivatives are calculated for the entire 6DOF pose including the zoom factor.<br>
355 * The resulting jacobian rows have the following form:
356 * <pre>
357 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz, dfx / dts |
358 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz, dfy / dts |
359 * </pre>
360 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz), translation t = (tx, ty, tz) and zoom factor s.<br>
361 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
362 * @param jx First row of the jacobian, with 7 column entries
363 * @param jy Second row of the jacobian, with 7 column entries
364 * @param pinholeCamera The pinhole camera to determine the jacobian values for
365 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
366 * @param zoom The zoom factor of the pose, with range (0, infinity)
367 * @param objectPoint 3D object point to determine the jacobian for
368 * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
369 * @tparam T The scalar data type, either 'float' or 'double'
370 */
371 template <typename T>
372 static void calculatePoseZoomJacobianRodrigues2x7(T* jx, T* jy, const PinholeCameraT<T>& pinholeCamera, const PoseT<T>& flippedCamera_P_world, const T zoom, const VectorT3<T>& objectPoint, const bool distortImagePoints);
373
374 /**
375 * Calculates all pose jacobian rows for a given (flexible) pose with (flexible) zoom factor and a set of static 3D object points.
376 * The 7 derivatives are calculated for the entire 6DOF pose including the zoom factor.<br>
377 * The resulting jacobian rows have the following form:
378 * <pre>
379 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz, dfx / dts |
380 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz, dfy / dts |
381 * </pre>
382 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz), translation t = (tx, ty, tz) and zoom factor s.<br>
383 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
384 * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 7 columns
385 * @param pinholeCamera The pinhole camera to determine the jacobian values for
386 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
387 * @param zoom The zoom factor of the pose, with range (0, infinity)
388 * @param objectPoints 3D object points to determine the jacobian for
389 * @param numberObjectPoints Number of given object points
390 * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
391 * @tparam T The scalar data type, either 'float' or 'double'
392 */
393 template <typename T>
394 static void calculatePoseZoomJacobianRodrigues2nx7(T* jacobian, const PinholeCameraT<T>& pinholeCamera, const PoseT<T>& flippedCamera_P_world, const T zoom, const VectorT3<T>* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
395
396 /**
397 * 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.
398 * The 6 derivatives are calculated for the 6-DOF object transformation.<br>
399 * The resulting jacobian rows have the following form:
400 * <pre>
401 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
402 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
403 * </pre>
404 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
405 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
406 * @param jx First row of the jacobian, with 6 column entries
407 * @param jy Second row of the jacobian, with 6 column entries
408 * @param pinholeCamera The pinhole camera profile defining the projection, must be valid
409 * @param extrinsicIF Inverted and flipped camera pose, must be valid
410 * @param objectPose The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
411 * @param objectPoint 3D object point to determine the jacobian for
412 * @see calculateObjectTransformation2nx6().
413 */
414 static inline void calculateObjectTransformation2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3& objectPoint);
415
416 /**
417 * 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.
418 * This function uses the pre-calculated 3x3 Jacobian matrix of the object transformation's orientation provided by three separated 3x3 matrices.<br>
419 * The 6 derivatives are calculated for the 6-DOF object transformation.<br>
420 * The resulting jacobian rows have the following form:
421 * <pre>
422 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
423 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
424 * </pre>
425 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
426 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
427 * @param jx First row of the jacobian, with 6 column entries
428 * @param jy Second row of the jacobian, with 6 column entries
429 * @param pinholeCamera The pinhole camera profile defining the projection, must be valid
430 * @param extrinsicIF Inverted and flipped camera pose, must be valid
431 * @param objectPose The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
432 * @param objectPoint 3D object point to determine the jacobian for
433 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
434 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
435 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
436 * @see calculateObjectTransformation2nx6().
437 */
438 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);
439
440 /**
441 * 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.
442 * The 6 derivatives are calculated for the entire 6DOF pose.<br>
443 * The resulting jacobian rows have the following form:
444 * <pre>
445 * | ... |
446 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
447 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
448 * | ... |
449 * </pre>
450 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
451 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
452 * @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
453 * @param pinholeCamera The pinhole camera profile defining the projection, must be valid
454 * @param extrinsicIF Inverted and flipped camera pose, must be valid
455 * @param objectPose The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
456 * @param objectPoints 3D object points to determine the jacobian for, must be valid
457 * @param numberObjectPoints Number of given object points, with range [1, infinity)
458 * @see calculateObjectTransformation2x6().
459 */
460 static void calculateObjectTransformation2nx6(Scalar* jacobian, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3* objectPoints, const size_t numberObjectPoints);
461
462 /**
463 * 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.
464 * The 6 derivatives are calculated for the entire 6DOF pose.<br>
465 * The resulting jacobian rows have the following form:
466 * <pre>
467 * | ... |
468 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
469 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
470 * | ... |
471 * </pre>
472 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tx, ty, tz).<br>
473 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
474 * @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
475 * @param fisheyeCamera The fisheye camera profile defining the projection, must be valid
476 * @param flippedCamera_T_world Inverted and flipped camera pose, must be valid
477 * @param world_T_object The 6-DOF object point transformation (rotation and translation) to determine the jacobian for
478 * @param objectPoints 3D object points to determine the jacobian for, must be valid
479 * @param numberObjectPoints Number of given object points, with range [1, infinity)
480 * @see calculateObjectTransformation2x6().
481 */
482 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);
483
484 /**
485 * Calculates the two jacobian rows for a given pose with translation scale ambiguities and static object point.
486 * The 5 derivatives are calculated for the 5DOF pose.<br>
487 * The resulting jacobian rows have the following form:
488 * <pre>
489 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtu, dfx / dtv |
490 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtu, dfy / dtv |
491 * </pre>
492 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tu, tv, sqrt(1 - tu*tu - tv*tv)).<br>
493 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
494 * @param jx First row of the jacobian, with 5 column entries
495 * @param jy Second row of the jacobian, with 5 column entries
496 * @param pinholeCamera The pinhole camera to determine the jacobian values for
497 * @param rotation Exponential map defining the rotation to determine the jacobian for
498 * @param translation Translation with scale ambiguities to determine the jacobian for, with range tx*tx + ty*ty < 1
499 * @param objectPoint 3D object point to determine the jacobian for
500 */
501 static void calculatePoseJacobianRodrigues2x5(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const ExponentialMap& rotation, const Vector2& translation, const Vector3& objectPoint);
502
503 /**
504 * Calculates all jacobian rows for a given pose with translation scale ambiguities and a set of static object points.
505 * The 5 derivatives are calculated for the 5DOF pose.<br>
506 * The resulting jacobian rows have the following form:
507 * <pre>
508 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtu, dfx / dtv |
509 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtu, dfy / dtv |
510 * </pre>
511 * With transformation function f = (fx, fy), exponential map w = (wx, wy, wz) and translation t = (tu, tv, sqrt(1 - tu*tu - tv*tv)).<br>
512 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
513 * @param jacobian First row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 5 columns
514 * @param pinholeCamera The pinhole camera to determine the jacobian values for
515 * @param rotation Exponential map defining the rotation to determine the jacobian for
516 * @param translation Translation with scale ambiguities to determine the jacobian for, with range tx*tx + ty*ty < 1
517 * @param objectPoints 3D object points to determine the jacobian for
518 * @param numberObjectPoints Number of given object points
519 */
520 static void calculatePoseJacobianRodrigues2nx5(Scalar* jacobian, const PinholeCamera& pinholeCamera, const ExponentialMap& rotation, const Vector2& translation, const Vector3* objectPoints, const size_t numberObjectPoints);
521
522 /**
523 * Calculates the two jacobian rows for a given pose and dynamic object point.
524 * The derivatives are calculated for the 3D object point only.<br>
525 * The resulting jacobian rows have the following form:
526 * <pre>
527 * | dfx / dX, dfx / dY, dfx / dZ |
528 * | dfy / dX, dfy / dY, dfy / dZ |
529 * </pre>
530 * @param jx First row of the jacobian, with 3 column entries receiving the point derivatives
531 * @param jy Second row of the jacobian, with 3 column entries receiving the point derivatives
532 * @param pinholeCamera The pinhole camera to determine the jacobian values for
533 * @param flippedCamera_P_world Pose to determine the jacobian for (inverted and flipped)
534 * @param objectPoint 3D object point to determine the jacobian for
535 * @param distortImagePoint True, to force the distortion of the image point using the distortion parameters of this camera object
536 */
537 static void calculatePointJacobian2x3(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_P_world, const Vector3& objectPoint, const bool distortImagePoint);
538
539 /**
540 * Calculates the two jacobian rows for a given pose and dynamic object point.
541 * The derivatives are calculated for the 3D object point only.<br>
542 * The resulting jacobian rows have the following form:
543 * <pre>
544 * | dfx / dX, dfx / dY, dfx / dZ |
545 * | dfy / dX, dfy / dY, dfy / dZ |
546 * </pre>
547 * @param jx First row of the jacobian, with 3 column entries receiving the point derivatives
548 * @param jy Second row of the jacobian, with 3 column entries receiving the point derivatives
549 * @param fisheyeCamera Fisheye camera profile defining the projection, must be valid
550 * @param flippedCamera_T_world Flipped and inverted pose of the camera, must be valid
551 * @param worldObjectPoint The 3D object point, defined in world
552 */
553 static void calculatePointJacobian2x3(Scalar* jx, Scalar* jy, const FisheyeCamera& fisheyeCamera, const HomogenousMatrix4& flippedCamera_T_world, const Vector3& worldObjectPoint);
554
555 /**
556 * Calculates the two jacobian rows for a given pose and dynamic object point.
557 * The derivatives are calculated for the 3D object point only.<br>
558 * The resulting jacobian rows have the following form:
559 * <pre>
560 * | dfx / dX, dfx / dY, dfx / dZ |
561 * | dfy / dX, dfy / dY, dfy / dZ |
562 * </pre>
563 * @param anyCamera The camera profile defining the projection, must be valid
564 * @param flippedCamera_T_world Flipped and inverted pose of the camera, must be valid
565 * @param worldObjectPoint The 3D object point, defined in world
566 * @param jx The resulting first row of the jacobian, with 3 column entries receiving the point derivatives, must be valid
567 * @param jy The resulting second row of the jacobian, with 3 column entries receiving the point derivatives, must be valid
568 * @tparam T The scalar data type
569 */
570 template <typename T>
571 static OCEAN_FORCE_INLINE void calculatePointJacobian2x3IF(const AnyCameraT<T>& anyCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, T* jx, T* jy);
572
573 /**
574 * Calculates the two jacobian rows for a given pose and several dynamic object points.
575 * The derivatives are calculated for the 3D object point only.<br>
576 * The resulting jacobian rows have the following form:
577 * <pre>
578 * | dfx / dX0, dfx / dY0, dfx / dZ0 |
579 * | dfy / dX0, dfy / dY0, dfy / dZ0 |
580 * | dfx / dX1, dfx / dY1, dfx / dZ1 |
581 * | dfx / dX1, dfx / dY1, dfx / dZ1 |
582 * | ............................... |
583 * | ............................... |
584 * </pre>
585 * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 3 columns
586 * @param pinholeCamera The pinhole camera to determine the jacobian values for
587 * @param flippedCamera_P_world Pose to determine the jacobian for (inverted and flipped)
588 * @param objectPoints 3D objects point to determine the jacobian for
589 * @param numberObjectPoints Number of 3D object points
590 * @param distortImagePoints True, to force the distortion of the image points using the distortion parameters of this camera object
591 */
592 static void calculatePointJacobian2nx3(Scalar* jacobian, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& flippedCamera_P_world, const Vector3* objectPoints, const size_t numberObjectPoints, const bool distortImagePoints);
593
594 /**
595 * Calculates the two jacobian rows for a given camera and image point.
596 * The jacobian is determined for the radial and tangential distortion parameters.<br>
597 * The resulting jacobian has the following form:
598 * <pre>
599 * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2 |
600 * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2 |
601 * </pre>
602 * @param jx First row of the jacobian, with 4 column entries
603 * @param jy Second row of the jacobian, with 4 column entries
604 * @param pinholeCamera The pinhole camera to determine the jacobian values for
605 * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
606 */
607 static void calculateCameraDistortionJacobian2x4(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
608
609 /**
610 * Calculates the two jacobian rows for the intrinsics of a given camera and image point.
611 * The jacobian is determined for the focal length, the principal point and the radial distortion parameters.<br>
612 * The resulting jacobian has the following form:
613 * <pre>
614 * | dfx / dk1, dfx / dk2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
615 * | dfy / dk1, dfy / dk2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
616 * </pre>
617 * @param jx First row of the jacobian, with 6 column entries
618 * @param jy Second row of the jacobian, with 6 column entries
619 * @param pinholeCamera The pinhole camera to determine the jacobian values for
620 * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
621 */
622 static void calculateCameraJacobian2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
623
624 /**
625 * Calculates the two jacobian rows for the intrinsics of a given camera and image point.
626 * 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>
627 * The resulting jacobian has the following form:
628 * <pre>
629 * | dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dF, dfx / dmx, dfx / dmy |
630 * | dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dF, dfy / dmx, dfy / dmy |
631 * </pre>
632 * @param jx First row of the jacobian, with 7 column entries
633 * @param jy Second row of the jacobian, with 7 column entries
634 * @param pinholeCamera The pinhole camera to determine the jacobian values for
635 * @param normalizedImagePoint Normalized 2D image point to determine the jacobian for
636 */
637 static void calculateCameraJacobian2x7(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Vector2& normalizedImagePoint);
638
639 /**
640 * Calculates the two jacobian rows for the intrinsics of a given camera and image point.
641 * The jacobian is determined for the focal length, the principal point and the radial and tangential distortion parameters.<br>
642 * The resulting jacobian has the following form:
643 * <pre>
644 * Jacobian Camera (parameter order as in PC_8_PARAMETERS):
645 * | dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2 |
646 * | dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2 |
647 * </pre>
648 * @param pinholeCamera The pinhole camera to determine the jacobian values for
649 * @param normalizedUndistortedImagePoint Normalized 2D image point to determine the jacobian for
650 * @param jx The resulting first row of the jacobian, with 8 column entries
651 * @param jy The resulting second row of the jacobian, with 8 column entries
652 * @tparam T The scalar data type, either 'float' or 'double'
653 */
654 template <typename T>
655 static void calculateCameraJacobian2x8(const PinholeCameraT<T>& pinholeCamera, const VectorT2<T>& normalizedUndistortedImagePoint, T* jx, T* jy);
656
657 /**
658 * Calculates the two jacobian rows for the intrinsics of a given fisheye camera and image point.
659 * The jacobian is determined for the focal length, the principal point, the radial distortion parameters, and the tangential distortion parameters.<br>
660 * The resulting jacobian has the following form:
661 * <pre>
662 * Jacobian Camera (parameter order as in PC_12_PARAMETERS):
663 * | dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dk3, dfx / dk5, dfx / dk7, dfx / dk9, dfx / dk11, dfx / dk13, dfx / dp1, dfx / dp2 |
664 * | dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dk3, dfy / dk5, dfy / dk7, dfy / dk9, dfy / dk11, dfy / dk13, dfy / dp1, dfy / dp2 |
665 * </pre>
666 * @param fisheyeCamera The fisheye camera to determine the jacobian values for
667 * @param normalizedUndistortedImagePoint The normalized 2D image point for which the Jacobian will be determined
668 * @param jx The resulting first row of the jacobian, with 12 column entries
669 * @param jy The resulting second row of the jacobian, with 12 column entries
670 * @tparam T The scalar data type, either 'float' or 'double'
671 */
672 template <typename T>
673 static void calculateCameraJacobian2x12(const FisheyeCameraT<T>& fisheyeCamera, const VectorT2<T>& normalizedUndistortedImagePoint, T* jx, T* jy);
674
675 /**
676 * Calculates the two jacobian rows for a given (orientational pose) and a camera and a static object point.
677 * 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>
678 * The resulting jacobian has the following form:
679 * <pre>
680 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
681 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
682 * </pre>
683 * 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>
684 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
685 * @param jx First row of the jacobian, with 11 column entries
686 * @param jy Second row of the jacobian, with 11 column entries
687 * @param pinholeCamera The pinhole camera to determine the jacobian values for
688 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
689 * @param objectPoint 3D object point to determine the jacobian for
690 */
691 static void calculateOrientationCameraJacobianRodrigues2x11(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const Vector3& objectPoint);
692
693 /**
694 * Calculates the two jacobian rows for a given (orientational pose) and a camera and a set of static object points.
695 * 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>
696 * The resulting jacobian has the following form:<br>
697 * <pre>
698 * | dfx / dwx, dfx / dwy, dfx / dwz, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy |
699 * | dfy / dwx, dfy / dwy, dfy / dwz, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy |
700 * </pre>
701 * 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>
702 * The jacobian calculation uses the Rodrigues rotation formula to determine the rotation derivatives.<br>
703 * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 11 columns
704 * @param pinholeCamera The pinhole camera to determine the jacobian values for
705 * @param flippedCamera_P_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
706 * @param objectPoints The accessor providing the 3D object points to determine the jacobian for
707 */
708 static void calculateOrientationCameraJacobianRodrigues2nx11(Scalar* jacobian, const PinholeCamera& pinholeCamera, const Pose& flippedCamera_P_world, const ConstIndexedAccessor<Vector3>& objectPoints);
709
710 /**
711 * 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.
712 * The resulting jacobian has the following form:
713 * <pre>
714 * | dfx / dk1, dfx / dk2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
715 * | dfy / dk1, dfy / dk2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
716 * </pre>
717 * @param jx First row of the jacobian, with 12 column entries
718 * @param jy Second row of the jacobian, with 12 column entries
719 * @param pinholeCamera The pinhole camera to determine the jacobian values for
720 * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
721 * @param objectPoint 3D object point to determine the jacobian for
722 * @tparam T The data type of the scalar, either 'float' or 'double'
723 */
724 template <typename T>
725 static void calculateJacobianCameraPoseRodrigues2x12(T* jx, T* jy, const PinholeCameraT<T>& pinholeCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint);
726
727 /**
728 * 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.
729 * The resulting jacobian has the following form:
730 * <pre>
731 * | dfx / dk1, dfx / dk2, dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dwx, dfx / dwy, dfx / dwz, dfx / dtx, dfx / dty, dfx / dtz |
732 * | dfy / dk1, dfy / dk2, dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dwx, dfy / dwy, dfy / dwz, dfy / dtx, dfy / dty, dfy / dtz |
733 * </pre>
734 * @param jx First row of the jacobian, with 12 column entries
735 * @param jy Second row of the jacobian, with 12 column entries
736 * @param pinholeCamera The pinhole camera to determine the jacobian values for
737 * @param flippedCamera_T_world Inverted and flipped pose (rotation and translation) to determine the jacobian for
738 * @param flippedCamera_P_world Inverted and flipped pose identical to 'flippedCamera_T_world'
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 * @param objectPoint 3D object point to determine the jacobian for
743 * @tparam T The data type of the scalar, either 'float' or 'double'
744 */
745 template <typename T>
746 static void calculateJacobianCameraPoseRodrigues2x12(T* jx, T* jy, const PinholeCameraT<T>& pinholeCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const PoseT<T>& flippedCamera_P_world, const VectorT3<T>& objectPoint, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz);
747
748 /**
749 * Calculates the entire jacobian matrix for an object point to image point transformation covering a flexible 6-DOF camera pose and a pinhole camera.
750 * The resulting jacobian has the following form:
751 * <pre>
752 * Jacobian Camera (parameter order as in PC_8_PARAMETERS):
753 * | dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2 |
754 * | dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2 |
755 *
756 * Jacobian Pose (parameters order as in Pose):
757 * | dfx / dtx, dfx / dty, dfx / dtz, dfx / dwx, dfx / dwy, dfx / dwz |
758 * | dfy / dtx, dfy / dty, dfy / dtz, dfy / dwx, dfy / dwy, dfy / dwz |
759 * </pre>
760 * @param pinholeCamera The pinhole camera to determine the jacobian values for
761 * @param flippedCamera_T_world Transformation between world and flipped camera, with flipped camera pointing towards the positive z-space with y-axis down
762 * @param objectPoint 3D object point to determine the jacobian for
763 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
764 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
765 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
766 * @param jacobianCameraX The resulting first row of the 2x8 Jacobian matrix for the camera parameters, must be valid
767 * @param jacobianCameraY The resulting second row of the 2x8 Jacobian matrix for the camera parameters, must be valid
768 * @param jacobianPoseX The resulting first 2x6 Jacobian matrix for the pose parameters, must be valid
769 * @param jacobianPoseY The resulting second 2x6 Jacobian matrix for the pose parameters, must be valid
770 * @see calculateRotationRodriguesDerivative().
771 * @tparam T The scalar data type, either 'float' or 'double'
772 */
773 template <typename T>
774 static void calculateJacobianCameraPoseRodrigues2x14IF(const PinholeCameraT<T>& pinholeCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz, T* jacobianCameraX, T* jacobianCameraY, T* jacobianPoseX, T* jacobianPoseY);
775
776 /**
777 * Calculates the two jacobian rows for a given (6-DOF pose) and a camera and a set of static object points.
778 * The resulting jacobian has the following form:
779 * <pre>
780 * Jacobian Camera (parameter order as in PC_8_PARAMETERS), Jacobian Pose (parameters order as in Pose):
781 * | dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dk1, dfx / dk2, dfx / dp1, dfx / dp2, dfx / dtx, dfx / dty, dfx / dtz, dfx / dwx, dfx / dwy, dfx / dwz |
782 * | dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dk1, dfy / dk2, dfy / dp1, dfy / dp2, dfy / dtx, dfy / dty, dfy / dtz, dfy / dwx, dfy / dwy, dfy / dwz |
783 * </pre>
784 * @param pinholeCamera The pinhole camera to determine the jacobian values for
785 * @param flippedCamera_T_world Transformation between world and flipped camera, with flipped camera pointing towards the positive z-space with y-axis down
786 * @param objectPoints The accessor providing the 3D object points to determine the jacobian for
787 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
788 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
789 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
790 * @param jacobian First element in the first row of the entire row aligned jacobian matrix, with 2 * numberObjectPoints rows and 14 columns
791 * @see calculateRotationRodriguesDerivative().
792 * @tparam T The scalar data type, either 'float' or 'double'
793 */
794 template <typename T>
795 static void calculateJacobianCameraPoseRodrigues2nx14IF(const PinholeCameraT<T>& pinholeCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const ConstIndexedAccessor<VectorT3<T>>& objectPoints, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz, T* jacobian);
796
797 /**
798 * Calculates the entire jacobian matrix for an object point to image point transformation covering a flexible 6-DOF camera pose and a fisheye camera.
799 * The resulting jacobian has the following form:
800 * <pre>
801 * Jacobian Camera (parameter order as in PC_12_PARAMETERS):
802 * | dfx / dFx, dfx / dFy, dfx / dmx, dfx / dmy, dfx / dk3, dfx / dk5, dfx / dk7, dfx / dk9, dfx / dk11, dfx / dk13, dfx / dp1, dfx / dp2 |
803 * | dfy / dFx, dfy / dFy, dfy / dmx, dfy / dmy, dfy / dk3, dfy / dk5, dfy / dk7, dfy / dk9, dfy / dk11, dfy / dk13, dfy / dp1, dfy / dp2 |
804 *
805 * Jacobian Pose (parameters order as in Pose):
806 * | dfx / dtx, dfx / dty, dfx / dtz, dfx / dwx, dfx / dwy, dfx / dwz |
807 * | dfy / dtx, dfy / dty, dfy / dtz, dfy / dwx, dfy / dwy, dfy / dwz |
808 * </pre>
809 * @param fisheyeCamera The fisheye camera to determine the jacobian values for
810 * @param flippedCamera_T_world Transformation between world and flipped camera, with flipped camera pointing towards the positive z-space with y-axis down
811 * @param objectPoint 3D object point to determine the jacobian for
812 * @param dwx Rotation matrix derived to wx, as determined by calculateRotationRodriguesDerivative()
813 * @param dwy Rotation matrix derived to wy, as determined by calculateRotationRodriguesDerivative()
814 * @param dwz Rotation matrix derived to wz, as determined by calculateRotationRodriguesDerivative()
815 * @param jacobianCameraX The resulting first row of the 2x12 Jacobian matrix for the camera parameters, must be valid
816 * @param jacobianCameraY The resulting second row of the 2x12 Jacobian matrix for the camera parameters, must be valid
817 * @param jacobianPoseX The resulting first 2x6 Jacobian matrix for the pose parameters, must be valid
818 * @param jacobianPoseY The resulting second 2x6 Jacobian matrix for the pose parameters, must be valid
819 * @see calculateRotationRodriguesDerivative().
820 * @tparam T The scalar data type, either 'float' or 'double'
821 */
822 template <typename T>
823 static void calculateJacobianCameraPoseRodrigues2x18IF(const FisheyeCameraT<T>& fisheyeCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& objectPoint, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz, T* jacobianCameraX, T* jacobianCameraY, T* jacobianPoseX, T* jacobianPoseY);
824
825 /**
826 * 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).
827 * This Jacobian can be used e.g., for additive image alignment.<br>
828 * The resulting jacobian has the following form:
829 * <pre>
830 * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7 |
831 * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7 |
832 * </pre>
833 * @param jx First row of the jacobian, with 8 column entries
834 * @param jy Second row of the jacobian, with 8 column entries
835 * @param x The horizontal coordinate to be used
836 * @param y The vertical coordinate to be used
837 * @param homography The homography for which the Jacobian will be determined, must have 1 in the lower right corner
838 * @see calculateHomographyJacobian2x9(), calculateIdentityHomographyJacobian2x8().
839 */
840 static void calculateHomographyJacobian2x8(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& homography);
841
842 /**
843 * 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).
844 * This Jacobian can be used e.g., for additive image alignment.<br>
845 * The resulting jacobian has the following form:
846 * <pre>
847 * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7, dfx / dh8 |
848 * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7, dfx / dh8 |
849 * </pre>
850 * @param jx First row of the jacobian, with 9 column entries
851 * @param jy Second row of the jacobian, with 9 column entries
852 * @param x The horizontal coordinate to be used
853 * @param y The vertical coordinate to be used
854 * @param homography The homography for which the Jacobian will be determined, must have 1 in the lower right corner
855 * @see calculateHomographyJacobian2x8(), calculateIdentityHomographyJacobian2x9().
856 */
857 static void calculateHomographyJacobian2x9(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& homography);
858
859 /**
860 * 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).
861 * This Jacobian can be used e.g., for inverse compositional image alignment..<br>
862 * The resulting jacobian has the following form:
863 * <pre>
864 * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7 |
865 * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7 |
866 * </pre>
867 * @param jx First row of the jacobian, with 8 column entries
868 * @param jy Second row of the jacobian, with 8 column entries
869 * @param x The horizontal coordinate to be used
870 * @param y The vertical coordinate to be used
871 * @see calculateIdentityHomographyJacobian2x9(), calculateHomographyJacobian2x8().
872 */
873 static void calculateIdentityHomographyJacobian2x8(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y);
874
875 /**
876 * 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).
877 * This Jacobian can be used e.g., for inverse compositional image alignment.<br>
878 * The resulting jacobian has the following form:
879 * <pre>
880 * | dfx / dh0, dfx / dh1, dfx / dh2, dfx / dh3, dfx / dh4, dfx / dh5, dfx / dh6, dfx / dh7, dfx / dh8 |
881 * | dfy / dh0, dfy / dh1, dfy / dh2, dfy / dh3, dfy / dh4, dfy / dh5, dfy / dh6, dfy / dh7, dfx / dh8 |
882 * </pre>
883 * @param jx First row of the jacobian, with 9 column entries
884 * @param jy Second row of the jacobian, with 9 column entries
885 * @param x The horizontal coordinate to be used
886 * @param y The vertical coordinate to be used
887 * @see calculateIdentityHomographyJacobian2x8(), calculateHomographyJacobian2x9().
888 */
889 static void calculateIdentityHomographyJacobian2x9(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y);
890
891 /**
892 * 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).
893 * This Jacobian can be used e.g., for additive image alignment.<br>
894 * The resulting jacobian has the following form:
895 * <pre>
896 * | dfx / ds0, dfx / ds1, dfx / ds2, dfx / ds3 |
897 * | dfy / ds0, dfy / ds1, dfy / ds2, dfy / ds3 |
898 * </pre>
899 * @param jx First row of the jacobian, with 4 column entries
900 * @param jy Second row of the jacobian, with 4 column entries
901 * @param x The horizontal coordinate to be used
902 * @param y The vertical coordinate to be used
903 * @param similarity The similarity for which the Jacobian will be determined, must have 1 in the lower right corner
904 */
905 static inline void calculateSimilarityJacobian2x4(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& similarity);
906
907 /**
908 * Determines the 2x2 Jacobian of distorting a normalized image point in a fisheye camera with radial and tangential distortion.
909 * The resulting jacobian has the following form:
910 * <pre>
911 * | dfx / dx, dfx / dy |
912 * | dfy / dx, dfy / dy |
913 * </pre>
914 * @param jx First row of the jacobian, with 2 column entries, must be valid
915 * @param jy Second row of the jacobian, with 2 column entries, must be valid
916 * @param x The horizontal coordinate of the normalized image point to be distorted
917 * @param y The vertical coordinate of the normalized image point to be distorted
918 * @param radialDistortion The six radial distortion parameters, must be valid
919 * @param tangentialDistortion The two radial distortion parameters, must be valid
920 * @tparam T The data type of a scalar, 'float' or 'double'
921 */
922 template <typename T>
923 static void calculateFisheyeDistortNormalized2x2(T* jx, T* jy, const T x, const T y, const T* radialDistortion, const T* tangentialDistortion);
924};
925
926template <typename T>
927inline void Jacobian::calculatePoseJacobianRodrigues2x6(T* jx, T* jy, const PinholeCameraT<T>& pinholeCamera, const PoseT<T>& flippedCamera_P_world, const VectorT3<T>& objectPoint, const bool distortImagePoint)
928{
929 SquareMatrixT3<T> dwx, dwy, dwz;
930 calculateRotationRodriguesDerivative(ExponentialMapT<T>(VectorT3<T>(flippedCamera_P_world.rx(), flippedCamera_P_world.ry(), flippedCamera_P_world.rz())), dwx, dwy, dwz);
931
932 calculatePoseJacobianRodrigues2x6(jx, jy, pinholeCamera, flippedCamera_P_world.transformation(), objectPoint, distortImagePoint, dwx, dwy, dwz);
933}
934
935inline void Jacobian::calculateObjectTransformation2x6(Scalar* jx, Scalar* jy, const PinholeCamera& pinholeCamera, const HomogenousMatrix4& extrinsicIF, const Pose& objectPose, const Vector3& objectPoint)
936{
937 SquareMatrix3 Rwx, Rwy, Rwz;
938 calculateRotationRodriguesDerivative(ExponentialMap(Vector3(objectPose.rx(), objectPose.ry(), objectPose.rz())), Rwx, Rwy, Rwz);
939
940 calculateObjectTransformation2x6(jx, jy, pinholeCamera, extrinsicIF, objectPose, objectPoint, Rwx, Rwy, Rwz);
941}
942
943inline void Geometry::Jacobian::calculateSimilarityJacobian2x4(Scalar* jx, Scalar* jy, const Scalar x, const Scalar y, const SquareMatrix3& similarity)
944{
945 ocean_assert(jx != nullptr && jy != nullptr);
946
947 ocean_assert_and_suppress_unused(Numeric::isEqual(similarity(2, 0), 0), similarity);
948 ocean_assert(Numeric::isEqual(similarity(2, 1), 0));
949 ocean_assert(Numeric::isEqual(similarity(2, 2), 1));
950
951 // Similarity:
952 // | s0 -s1 s2 |
953 // | s1 s0 s3 |
954 // | 0 0 1 |
955
956 // sx(x, y) = s0*x - s1*y + s2
957 // sy(x, y) = s1*x + s0*y + s3
958
959 // Jacobian: x -y 1 0
960 // y x 0 1
961
962 jx[0] = x;
963 jx[1] = -y;
964 jx[2] = 1;
965 jx[3] = 0;
966
967 jy[0] = y;
968 jy[1] = x;
969 jy[2] = 0;
970 jy[3] = 1;
971}
972
973template <typename T, typename TRotation>
974OCEAN_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)
975{
976 static_assert(std::is_same<TRotation, QuaternionT<T>>::value || std::is_same<TRotation, SquareMatrixT3<T>>::value, "Invalid rotation type!");
977
978 ocean_assert(anyCamera.isValid());
979 ocean_assert(jx != nullptr && jy != nullptr);
980
981#ifdef OCEAN_DEBUG
982 if constexpr (std::is_same<TRotation, QuaternionT<T>>::value)
983 {
984 ocean_assert(flippedCamera_R_translation.isValid());
985 }
986 else
987 {
988 ocean_assert(flippedCamera_R_translation.isOrthonormal());
989 }
990#endif
991
992 /**
993 * f = fC(fR(fT(X))
994 *
995 * with fC camera function, fR object point rotation function, fT object point translation function
996 *
997 * fR(fT(X)) = R(X + t)
998 */
999
1000 const VectorT3<T> translatedWorldObjectPoint = translation_T_world + worldObjectPoint;
1001 const VectorT3<T> flippedCameraObjectPoint = flippedCamera_R_translation * translatedWorldObjectPoint;
1002
1003 T jxPoint[3];
1004 T jyPoint[3];
1005
1006 // let's determine the left 2x3 sub-matrix first
1007 anyCamera.pointJacobian2x3IF(flippedCameraObjectPoint, jxPoint, jyPoint);
1008
1009 const VectorT3<T> dwx(Rwx * translatedWorldObjectPoint);
1010 const VectorT3<T> dwy(Rwy * translatedWorldObjectPoint);
1011 const VectorT3<T> dwz(Rwz * translatedWorldObjectPoint);
1012
1013 // now, we apply the chain rule to determine the 2x3 Jacobian
1014 jx[0] = jxPoint[0] * dwx[0] + jxPoint[1] * dwx[1] + jxPoint[2] * dwx[2];
1015 jx[1] = jxPoint[0] * dwy[0] + jxPoint[1] * dwy[1] + jxPoint[2] * dwy[2];
1016 jx[2] = jxPoint[0] * dwz[0] + jxPoint[1] * dwz[1] + jxPoint[2] * dwz[2];
1017
1018 jy[0] = jyPoint[0] * dwx[0] + jyPoint[1] * dwx[1] + jyPoint[2] * dwx[2];
1019 jy[1] = jyPoint[0] * dwy[0] + jyPoint[1] * dwy[1] + jyPoint[2] * dwy[2];
1020 jy[2] = jyPoint[0] * dwz[0] + jyPoint[1] * dwz[1] + jyPoint[2] * dwz[2];
1021}
1022
1023template <typename T>
1024OCEAN_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)
1025{
1026 ocean_assert(anyCamera.isValid());
1027 ocean_assert(flippedCamera_T_world.isValid());
1028 ocean_assert(jx != nullptr && jy != nullptr);
1029
1030 anyCamera.pointJacobian2x3IF(flippedCamera_T_world * worldObjectPoint, jx + 3, jy + 3);
1031
1032 const VectorT3<T> dwx(Rwx * worldObjectPoint);
1033 const VectorT3<T> dwy(Rwy * worldObjectPoint);
1034 const VectorT3<T> dwz(Rwz * worldObjectPoint);
1035
1036 // now, we apply the chain rule to determine the left 2x3 sub-matrix
1037 jx[0] = jx[3] * dwx[0] + jx[4] * dwx[1] + jx[5] * dwx[2];
1038 jx[1] = jx[3] * dwy[0] + jx[4] * dwy[1] + jx[5] * dwy[2];
1039 jx[2] = jx[3] * dwz[0] + jx[4] * dwz[1] + jx[5] * dwz[2];
1040
1041 jy[0] = jy[3] * dwx[0] + jy[4] * dwx[1] + jy[5] * dwx[2];
1042 jy[1] = jy[3] * dwy[0] + jy[4] * dwy[1] + jy[5] * dwy[2];
1043 jy[2] = jy[3] * dwz[0] + jy[4] * dwz[1] + jy[5] * dwz[2];
1044}
1045
1046template <typename T>
1047OCEAN_FORCE_INLINE void Jacobian::calculatePointJacobian2x3IF(const AnyCameraT<T>& anyCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const VectorT3<T>& worldObjectPoint, T* jx, T* jy)
1048{
1049 ocean_assert(anyCamera.isValid());
1050 ocean_assert(flippedCamera_T_world.isValid());
1051 ocean_assert(jx != nullptr && jy != nullptr);
1052
1053 // | Fx 0 | | df_distx_u df_distx_v | | 1/W 0 -U/W^2 |
1054 // | 0 Fy | * | df_disty_u df_disty_v | * | 0 1/W -V/W^2 | * R
1055
1056 T pointJacobian2x3[6];
1057 anyCamera.pointJacobian2x3IF(flippedCamera_T_world * worldObjectPoint, pointJacobian2x3 + 0, pointJacobian2x3 + 3);
1058
1059 jx[0] = pointJacobian2x3[0] * flippedCamera_T_world[0] + pointJacobian2x3[1] * flippedCamera_T_world[1] + pointJacobian2x3[2] * flippedCamera_T_world[2];
1060 jx[1] = pointJacobian2x3[0] * flippedCamera_T_world[4] + pointJacobian2x3[1] * flippedCamera_T_world[5] + pointJacobian2x3[2] * flippedCamera_T_world[6];
1061 jx[2] = pointJacobian2x3[0] * flippedCamera_T_world[8] + pointJacobian2x3[1] * flippedCamera_T_world[9] + pointJacobian2x3[2] * flippedCamera_T_world[10];
1062
1063 jy[0] = pointJacobian2x3[3] * flippedCamera_T_world[0] + pointJacobian2x3[4] * flippedCamera_T_world[1] + pointJacobian2x3[5] * flippedCamera_T_world[2];
1064 jy[1] = pointJacobian2x3[3] * flippedCamera_T_world[4] + pointJacobian2x3[4] * flippedCamera_T_world[5] + pointJacobian2x3[5] * flippedCamera_T_world[6];
1065 jy[2] = pointJacobian2x3[3] * flippedCamera_T_world[8] + pointJacobian2x3[4] * flippedCamera_T_world[9] + pointJacobian2x3[5] * flippedCamera_T_world[10];
1066}
1067
1068template <typename T>
1069void Jacobian::calculateJacobianCameraPoseRodrigues2nx14IF(const PinholeCameraT<T>& pinholeCamera, const HomogenousMatrixT4<T>& flippedCamera_T_world, const ConstIndexedAccessor<VectorT3<T>>& objectPoints, const SquareMatrixT3<T>& dwx, const SquareMatrixT3<T>& dwy, const SquareMatrixT3<T>& dwz, T* jacobian)
1070{
1071 ocean_assert(jacobian != nullptr && pinholeCamera.isValid());
1072 ocean_assert(flippedCamera_T_world.isValid());
1073
1074 for (size_t n = 0u; n < objectPoints.size(); ++n)
1075 {
1076 const VectorT3<T>& objectPoint = objectPoints[n];
1077
1078 calculateJacobianCameraPoseRodrigues2x14IF<T>(pinholeCamera, flippedCamera_T_world, objectPoint, dwx, dwy, dwz, jacobian + 0, jacobian + 14, jacobian + 8, jacobian + 22);
1079
1080 jacobian += 28;
1081 }
1082}
1083
1084template <typename T>
1085void Jacobian::calculateFisheyeDistortNormalized2x2(T* jx, T* jy, const T x, const T y, const T* radialDistortion, const T* tangentialDistortion)
1086{
1087 ocean_assert(jx != nullptr && jy != nullptr);
1088 ocean_assert(radialDistortion != nullptr && tangentialDistortion != nullptr);
1089
1090 const T& k3 = radialDistortion[0];
1091 const T& k5 = radialDistortion[1];
1092 const T& k7 = radialDistortion[2];
1093 const T& k9 = radialDistortion[3];
1094 const T& k11 = radialDistortion[4];
1095 const T& k13 = radialDistortion[5];
1096
1097 const T& p1 = tangentialDistortion[0];
1098 const T& p2 = tangentialDistortion[1];
1099
1100 const T x2 = x * x;
1101 const T y2 = y * y;
1102
1103 const T xy2 = x2 + y2;
1104
1105 const T r = NumericT<T>::sqrt(xy2);
1106 const T r3 = r * r * r;
1107
1108 const T t = NumericT<T>::atan(r);
1109 const T t2 = t * t;
1110 const T t3 = t2 * t;
1111 const T t4 = t3 * t;
1112 const T t5 = t4 * t;
1113 const T t6 = t5 * t;
1114 const T t7 = t6 * t;
1115 const T t8 = t7 * t;
1116 const T t9 = t8 * t;
1117 const T t10 = t9 * t;
1118 const T t11 = t10 * t;
1119 const T t12 = t11 * t;
1120 const T t13 = t12 * t;
1121
1122 const T term0 = k13 * t13 + k11 * t11 + k9 * t9 + k7 * t7 + k5 * t5 + k3 * t3 + t;
1123 const T term1 = 13 * k13 * t12 + 11 * k11 * t10 + 9 * k9 * t8 + 7 * k7 * t6 + 5 * k5 * t4 + 3 * k3 * t2 + 1;
1124
1125 const T term2 = (xy2 + 1) * term0;
1126 const T term3 = r3 * (xy2 + 1);
1127 const T invTerm3 = T(1) / term3;
1128
1129 const T xDistortion_dx = (xy2 * term2 - x2 * term2 + x2 * r * term1) * invTerm3;
1130 const T xDistortion_dy = (x * term1 * y) / (xy2 * (xy2 + 1)) - (x * y * term0) / r3;
1131
1132 //const T yDistortion_dx = (y * term1 * x) / (xy2 * (xy2 + 1)) - (y * x * term0) / r3; == xDistortion_dy
1133 const T& yDistortion_dx = xDistortion_dy;
1134 const T yDistortion_dy = (xy2 * term2 - y2 * term2 + y2 * r * term1) * invTerm3;
1135
1136 const T radialDistortionFactor = term0 / r;
1137
1138 const T rx = x * radialDistortionFactor;
1139 const T ry = y * radialDistortionFactor;
1140
1141 const T xTangential_dx = 6 * p1 * rx + 2 * p2 * ry + 1;
1142 const T xTangential_dy = 2 * p1 * ry + 2 * p2 * rx;
1143
1144 // const T yTangential_dx = 2 * p2 * rx + 2 * p1 * ry; // == yTangential_dx
1145 const T& yTangential_dx = xTangential_dy;
1146 const T yTangential_dy = 6 * p2 * ry + 2 * p1 * rx + 1;
1147
1148 // chain rule
1149 // | xTangential_dx xTangential_dy | | xDistortion_dx xDistortion_dy |
1150 // | yTangential_dx yTangential_dy | * | yDistortion_dx yDistortion_dy |
1151
1152 jx[0] = xTangential_dx * xDistortion_dx + xTangential_dy * yDistortion_dx;
1153 jx[1] = xTangential_dx * xDistortion_dy + xTangential_dy * yDistortion_dy;
1154
1155 jy[0] = yTangential_dx * xDistortion_dx + yTangential_dy * yDistortion_dx;
1156 jy[1] = yTangential_dx * xDistortion_dy + yTangential_dy * yDistortion_dy;
1157}
1158
1159}
1160
1161}
1162
1163#endif // META_OCEAN_GEOMETRY_JACOBIAN_H
Ocean::AnyCameraT
This class implements the abstract base class for all AnyCamera objects.
Definition AnyCamera.h:131
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:935
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:974
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::calculateJacobianCameraPoseRodrigues2x14IF
static void calculateJacobianCameraPoseRodrigues2x14IF(const PinholeCameraT< T > &pinholeCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz, T *jacobianCameraX, T *jacobianCameraY, T *jacobianPoseX, T *jacobianPoseY)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
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:1047
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:1024
Ocean::Geometry::Jacobian::calculateCameraJacobian2x12
static void calculateCameraJacobian2x12(const FisheyeCameraT< T > &fisheyeCamera, const VectorT2< T > &normalizedUndistortedImagePoint, T *jx, T *jy)
Calculates the two jacobian rows for the intrinsics of a given fisheye camera and image point.
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:1085
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::calculatePoseJacobianRodrigues2nx6
static void calculatePoseJacobianRodrigues2nx6(T *jacobian, const PinholeCameraT< T > &pinholeCamera, const PoseT< T > &flippedCamera_P_world, const VectorT3< T > *objectPoints, const size_t numberObjectPoints, const bool distortImagePoints)
Deprecated.
Ocean::Geometry::Jacobian::calculatePoseJacobianRodrigues2x6
static void calculatePoseJacobianRodrigues2x6(T *jx, T *jy, const PinholeCameraT< T > &pinholeCamera, const PoseT< T > &flippedCamera_P_world, const VectorT3< T > &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:927
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::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::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::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::calculateJacobianCameraPoseRodrigues2x12
static void calculateJacobianCameraPoseRodrigues2x12(T *jx, T *jy, const PinholeCameraT< T > &pinholeCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
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 the intrinsics of a given camera and image point.
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2x18IF
static void calculateJacobianCameraPoseRodrigues2x18IF(const FisheyeCameraT< T > &fisheyeCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz, T *jacobianCameraX, T *jacobianCameraY, T *jacobianPoseX, T *jacobianPoseY)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
Ocean::Geometry::Jacobian::calculateCameraJacobian2x7
static void calculateCameraJacobian2x7(Scalar *jx, Scalar *jy, const PinholeCamera &pinholeCamera, const Vector2 &normalizedImagePoint)
Calculates the two jacobian rows for the intrinsics of 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:943
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::calculatePoseZoomJacobianRodrigues2nx7
static void calculatePoseZoomJacobianRodrigues2nx7(T *jacobian, const PinholeCameraT< T > &pinholeCamera, const PoseT< T > &flippedCamera_P_world, const T zoom, const VectorT3< T > *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::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::calculatePoseJacobianRodrigues2x6
static void calculatePoseJacobianRodrigues2x6(T *jx, T *jy, const PinholeCameraT< T > &pinholeCamera, const HomogenousMatrixT4< T > &flippedCamera_P_world, const VectorT3< T > &objectPoint, const bool distortImagePoint, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz)
Deprecated.
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(T *jx, T *jy, const PinholeCameraT< T > &pinholeCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const PoseT< T > &flippedCamera_P_world, const VectorT3< T > &objectPoint, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz)
Calculates the entire jacobian matrix for an object point to image point transformation covering a fl...
Ocean::Geometry::Jacobian::calculatePoseJacobianRodriguesDampedDistortion2nx6
static void calculatePoseJacobianRodriguesDampedDistortion2nx6(T *jacobian, const PinholeCameraT< T > &pinholeCamera, const PoseT< T > &flippedCamera_P_world, const T dampingFactor, const VectorT3< T > *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::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::calculateCameraJacobian2x8
static void calculateCameraJacobian2x8(const PinholeCameraT< T > &pinholeCamera, const VectorT2< T > &normalizedUndistortedImagePoint, T *jx, T *jy)
Calculates the two jacobian rows for the intrinsics of a given camera and image point.
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(T *jx, T *jy, const PinholeCameraT< T > &pinholeCamera, const PoseT< T > &flippedCamera_P_world, const T zoom, const VectorT3< T > &objectPoint, const bool distortImagePoints)
Calculates the two jacobian rows for a given (flexible) pose with (flexible) zoom factor and one stat...
Ocean::Geometry::Jacobian::calculateJacobianCameraPoseRodrigues2nx14IF
static void calculateJacobianCameraPoseRodrigues2nx14IF(const PinholeCameraT< T > &pinholeCamera, const HomogenousMatrixT4< T > &flippedCamera_T_world, const ConstIndexedAccessor< VectorT3< T > > &objectPoints, const SquareMatrixT3< T > &dwx, const SquareMatrixT3< T > &dwy, const SquareMatrixT3< T > &dwz, T *jacobian)
Calculates the two jacobian rows for a given (6-DOF pose) and a camera and a set of static object poi...
Definition Jacobian.h:1069
Ocean::HomogenousMatrixT4
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
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:1620
Ocean::NumericT::sqrt
static T sqrt(const T value)
Returns the square root of a given value.
Definition Numeric.h:1537
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:2395
Ocean::PinholeCameraT::isValid
bool isValid() const
Returns whether this camera is valid.
Definition PinholeCamera.h:1762
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:89
Ocean::VectorT3
This class implements a vector with three elements.
Definition Vector3.h:97
Ocean::Scalar
float Scalar
Definition of a scalar type.
Definition Math.h:129
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::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