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