Skip to content

Orientation estimation

axang2rotm(axang)

Convert axis-angle representation to rotation matrix.

Parameters:

Name Type Description Default
axang ndarray

Input array of axis-angle representations with shape (..., 4), where the first three elements are the axis of rotation and the last element is the angle of rotation in radians.

 required

Returns:

Type Description
ndarray

np.ndarray: Rotation matrix corresponding to the input axis-angle representations.

The function computes the rotation matrix using Rodrigues' rotation formula.

Source code in kielmat/utils/quaternion.py 
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
def axang2rotm(axang: np.ndarray) -> np.ndarray:
    """
    Convert axis-angle representation to rotation matrix.

    Args:
        axang (np.ndarray): Input array of axis-angle representations with shape (..., 4),
            where the first three elements are the axis of rotation
            and the last element is the angle of rotation in radians.

    Returns:
        np.ndarray: Rotation matrix corresponding to the input axis-angle representations.

    The function computes the rotation matrix using Rodrigues' rotation formula.
    """

    # Cast array to float
    axang = np.asarray(axang, float)
    assert axang.shape[-1] == 4

    # Extract axis and angle
    axis = axang[..., :3]
    angle = axang[..., 3]

    # Normalize axis
    axis /= np.linalg.norm(axis, axis=-1, keepdims=True)

    # Compute rotation matrix using Rodrigues' rotation formula
    cos_theta = np.cos(angle)
    sin_theta = np.sin(angle)
    cross_prod_matrix = np.zeros((*axis.shape[:-1], 3, 3), dtype=float)
    cross_prod_matrix[..., 0, 1] = -axis[..., 2]
    cross_prod_matrix[..., 0, 2] = axis[..., 1]
    cross_prod_matrix[..., 1, 0] = axis[..., 2]
    cross_prod_matrix[..., 1, 2] = -axis[..., 0]
    cross_prod_matrix[..., 2, 0] = -axis[..., 1]
    cross_prod_matrix[..., 2, 1] = axis[..., 0]
    rotation_matrix = (
        np.eye(3, dtype=float) * cos_theta[..., None]
        + sin_theta[..., None] * cross_prod_matrix
        + (1 - cos_theta[..., None]) * np.einsum("...i,...j->...ij", axis, axis)
    )

    return rotation_matrix

quat2axang(q)

Convert a quaternion to axis-angle representation.

Parameters:

Name Type Description Default
q ndarray

Input quaternion array of shape (..., 4).

 required

Returns:

Type Description
ndarray

np.ndarray: Axis-angle representation array of shape (..., 4), where the first three elements are the axis of rotation and the last element is the angle of rotation in radians.

The function normalizes the input quaternion, calculates the angle of rotation, and computes the axis of rotation in the axis-angle representation.

Note: The input quaternion array is expected to have the last dimension of size 4.

Source code in kielmat/utils/quaternion.py 
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
def quat2axang(q: np.ndarray) -> np.ndarray:
    """
    Convert a quaternion to axis-angle representation.

    Args:
        q (np.ndarray): Input quaternion array of shape (..., 4).

    Returns:
        np.ndarray: Axis-angle representation array of shape (..., 4),
            where the first three elements are the axis of rotation
            and the last element is the angle of rotation in radians.

    The function normalizes the input quaternion, calculates the angle of rotation,
    and computes the axis of rotation in the axis-angle representation.

    Note: The input quaternion array is expected to have the last dimension of size 4.
    """

    # Cast array to float
    q = np.asarray(q, float)
    assert q.shape[-1] == 4

    # Normalize the quaternion
    q = quatnormalize(q)

    # Calculate the angle of rotation
    axang = np.zeros_like(q)
    axang[..., 3] = 2.0 * np.arccos(q[..., 0])
    axang[..., :3] = np.where(
        np.sin(axang[..., 3] / 2.0) > _EPS,
        q[..., 1:] / np.sin(axang[..., 3] / 2.0),
        np.array([0.0, 0.0, 1.0]),
    )
    return axang

quat2rotm(q, scalar_first=True, channels_last=True)

Convert quaternion(s) to rotation matrix.

Parameters:

Name Type Description Default
q ndarray

Input quaternion(s) as a NumPy array. The last dimension must have size 4.

 required
scalar_first bool

If True, the quaternion is assumed to be in scalar-first order (default is True).

 True
channels_last bool

If True, the last dimension represents the quaternion channels (default is True). If False, the quaternion channels are assumed to be in the first dimension.

 True

Returns:

Type Description
ndarray

np.ndarray: Rotation matrix corresponding to the input quaternion(s).

Raises:

Type Description
AssertionError

If the last dimension of the input array q does not have size 4.
Notes

The conversion is based on the formula: R = | 1 - 2q2^2 - 2q3^2 2(q1q2 - q3q0) 2(q1q3 + q2q0) | | 2(q1q2 + q3q0) 1 - 2q1^2 - 2q3^2 2(q2q3 - q1q0) | | 2(q1q3 - q2q0) 2(q1q0 + q2q3) 1 - 2q1^2 - 2q2^2 |

References

Wikipedia: https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion

Examples:

>>> quaternion = np.array([1.0, 0.0, 0.0, 0.0])
>>> rotation_matrix = quat2rotm(quaternion)
Source code in kielmat/utils/quaternion.py 
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
def quat2rotm(
    q: np.ndarray, scalar_first: bool = True, channels_last: bool = True
) -> np.ndarray:
    """
    Convert quaternion(s) to rotation matrix.

    Args:
        q (np.ndarray): Input quaternion(s) as a NumPy array. The last dimension must have size 4.
        scalar_first (bool, optional): If True, the quaternion is assumed to be in scalar-first order (default is True).
        channels_last (bool, optional): If True, the last dimension represents the quaternion channels (default is True).
            If False, the quaternion channels are assumed to be in the first dimension.

    Returns:
        np.ndarray: Rotation matrix corresponding to the input quaternion(s).

    Raises:
        AssertionError: If the last dimension of the input array `q` does not have size 4.

    Notes:
        >>> The conversion is based on the formula:
        >>> R = | 1 - 2*q2^2 - 2*q3^2    2*(q1*q2 - q3*q0)    2*(q1*q3 + q2*q0) |
            | 2*(q1*q2 + q3*q0)    1 - 2*q1^2 - 2*q3^2    2*(q2*q3 - q1*q0) |
            | 2*(q1*q3 - q2*q0)    2*(q1*q0 + q2*q3)    1 - 2*q1^2 - 2*q2^2 |

    References:
        Wikipedia: https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion

    Examples:
        >>> quaternion = np.array([1.0, 0.0, 0.0, 0.0])
        >>> rotation_matrix = quat2rotm(quaternion)
    """

    # Cast array to float
    q = np.asarray(q, float)
    assert q.shape[-1] == 4

    # Derive rotation matrix from quaternion
    R = np.zeros(q.shape[:-1] + (3, 3), float)
    R[..., 0, 0] = 1 - 2 * q[..., 2] ** 2 - 2 * q[..., 3] ** 2
    R[..., 0, 1] = 2 * (q[..., 1] * q[..., 2] - q[..., 3] * q[..., 0])
    R[..., 0, 2] = 2 * (q[..., 1] * q[..., 3] + q[..., 2] * q[..., 0])
    R[..., 1, 0] = 2 * (q[..., 1] * q[..., 2] + q[..., 3] * q[..., 0])
    R[..., 1, 1] = 1 - 2 * q[..., 1] ** 2 - 2 * q[..., 3] ** 2
    R[..., 1, 2] = 2 * (q[..., 2] * q[..., 3] - q[..., 1] * q[..., 0])
    R[..., 2, 0] = 2 * (q[..., 1] * q[..., 3] - q[..., 2] * q[..., 0])
    R[..., 2, 1] = 2 * (q[..., 1] * q[..., 0] + q[..., 2] * q[..., 3])
    R[..., 2, 2] = 1 - 2 * q[..., 1] ** 2 - 2 * q[..., 2] ** 2
    return R

quatconj(q, scalar_first=True, channels_last=True)

Compute the quaternion conjugate.

This function calculates the conjugate of a quaternion, which is obtained by negating the imaginary (vector) parts while keeping the real (scalar) part unchanged.

Parameters:

Name Type Description Default
q ndarray

Input array of quaternions with shape (..., 4).

 required
scalar_first bool

If True, assumes the scalar part is the first element. If False, assumes the scalar part is the last element. Default is True.

 True
channels_last bool

If True, assumes the channels are the last dimension. If False, assumes the channels are the second-to-last dimension. Default is True.

 True

Returns:

Type Description
ndarray

np.ndarray: Quaternion conjugate with the same shape as the input array.
Notes
  • The input array is cast to float before the computation.
  • If channels_last is False, the input array is transposed to switch channels and time axis.
  • If scalar_first is False, the scalar part is moved to the last element.
Quaternion Conjugate Formula

q_conj = [w, -x, -y, -z]

Source code in kielmat/utils/quaternion.py 
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
def quatconj(
    q: np.ndarray, scalar_first: bool = True, channels_last: bool = True
) -> np.ndarray:
    """
    Compute the quaternion conjugate.

    This function calculates the conjugate of a quaternion, which is obtained by negating
    the imaginary (vector) parts while keeping the real (scalar) part unchanged.

    Args:
        q (np.ndarray): Input array of quaternions with shape (..., 4).
        scalar_first (bool, optional): If True, assumes the scalar part is the first element.
            If False, assumes the scalar part is the last element. Default is True.
        channels_last (bool, optional): If True, assumes the channels are the last dimension.
            If False, assumes the channels are the second-to-last dimension. Default is True.

    Returns:
        np.ndarray: Quaternion conjugate with the same shape as the input array.

    Notes:
        - The input array is cast to float before the computation.
        - If channels_last is False, the input array is transposed to switch channels and time axis.
        - If scalar_first is False, the scalar part is moved to the last element.

    Quaternion Conjugate Formula:
        >>> q_conj = [w, -x, -y, -z]
    """

    # Cast array to float
    q = np.asarray(q, float)

    if not channels_last:
        # Take the tranpose, i.e. switch channels and time axis
        q = q.T

    if not scalar_first:
        # Put the scalar first
        q_tmp = q.copy()
        q[..., 0] = q_tmp[..., -1]
        q[..., 1:] = q_tmp[..., :-1]
        del q_tmp

    # Negate the vector part of the quaternion
    q_out = q.copy()
    q_out[..., 1:] *= -1

    if not scalar_first:
        # Put scalar part back in last channel
        q_tmp = q_out.copy()
        q_out[..., -1] = q_tmp[..., 0]
        q_out[..., :-1] = q_tmp[..., 1:]
        del q_tmp

    if not channels_last:
        # Switch channels and time axis back
        q_out = q_out.T
    return q_out

quatinv(q, scalar_first=True, channels_last=True)

Compute the inverse of quaternions.

This function calculates the inverse of quaternions by first computing the conjugate and then normalizing the conjugate.

Parameters:

Name Type Description Default
q ndarray

Input array of quaternions with shape (..., 4).

 required
scalar_first bool

If True, assumes the scalar part is the first element. If False, assumes the scalar part is the last element. Default is True.

 True
channels_last bool

If True, assumes the channels are the last dimension. If False, assumes the channels are the second-to-last dimension. Default is True.

 True

Returns:

Type Description
ndarray

np.ndarray: Inverse of quaternions with the same shape as the input array.
Notes
  • The input array is cast to float before the computation.
  • If channels_last is False, the input array is transposed to switch channels and time axis.
Quaternion Inverse Calculation

The inverse of a quaternion q is obtained by first calculating its conjugate and then normalizing it: q_inv = normalize(conjugate(q))

Source code in kielmat/utils/quaternion.py 
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
def quatinv(
    q: np.ndarray, scalar_first: bool = True, channels_last: bool = True
) -> np.ndarray:
    """
    Compute the inverse of quaternions.

    This function calculates the inverse of quaternions by first computing the conjugate
    and then normalizing the conjugate.

    Args:
        q (np.ndarray): Input array of quaternions with shape (..., 4).
        scalar_first (bool, optional): If True, assumes the scalar part is the first element.
            If False, assumes the scalar part is the last element. Default is True.
        channels_last (bool, optional): If True, assumes the channels are the last dimension.
            If False, assumes the channels are the second-to-last dimension. Default is True.

    Returns:
        np.ndarray: Inverse of quaternions with the same shape as the input array.

    Notes:
        - The input array is cast to float before the computation.
        - If channels_last is False, the input array is transposed to switch channels and time axis.

    Quaternion Inverse Calculation:
        >>> The inverse of a quaternion q is obtained by first calculating its conjugate and then normalizing it:
        >>> q_inv = normalize(conjugate(q))
    """

    # Cast array to float
    q = np.asarray(q, float)

    # Compute the quaternion conjugate
    qconj = quatconj(q, scalar_first=scalar_first, channels_last=channels_last)

    # Normalize the quaternion conjugate
    qout = quatnormalize(qconj)
    return qout

quatmultiply(q1, q2=None, scalar_first=True, channels_last=True)

Multiply two sets of quaternions.

This function performs quaternion multiplication on two sets of quaternions. Quaternions are 4-dimensional vectors of the form [w, x, y, z], where 'w' is the scalar (real) part, and 'x', 'y', and 'z' are the vector (imaginary) parts.

Parameters:

Name Type Description Default
q1 ndarray

Input array of quaternions with shape (..., 4).

 required
q2 ndarray

Input array of quaternions with shape (..., 4). If None, q2 is set to q1, making it a self-multiplication. Default is None.

 None
scalar_first bool

If True, assumes the scalar part is the first element. If False, assumes the scalar part is the last element. Default is True.

 True
channels_last bool

If True, assumes the channels are the last dimension. If False, assumes the channels are the second-to-last dimension. Default is True.

 True

Returns:

Type Description
ndarray

np.ndarray: Result of quaternion multiplication with the same shape as the input arrays.

Raises:

Type Description
AssertionError

If the last dimension of q1 and q2 is not 4.
Notes
  • If q2 is None, this function performs self-multiplication (q1 * q1).
  • The input arrays are cast to float before the computation.
  • If channels_last is False, the input arrays are transposed to switch channels and time axis.
Quaternion Conjugate Formula

q3 = [w1w2 - x1x2 - y1y2 - z1z2, w1x2 + x1w2 + y1z2 - z1y2, w1y2 - x1z2 + y1w2 + z1x2, w1z2 + x1y2 - y1x2 + z1w2]

Source code in kielmat/utils/quaternion.py 
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
def quatmultiply(
    q1: np.ndarray,
    q2: Optional[np.ndarray] = None,
    scalar_first: bool = True,
    channels_last: bool = True,
) -> np.ndarray:
    """
    Multiply two sets of quaternions.

    This function performs quaternion multiplication on two sets of quaternions.
    Quaternions are 4-dimensional vectors of the form [w, x, y, z], where 'w' is the
    scalar (real) part, and 'x', 'y', and 'z' are the vector (imaginary) parts.

    Args:
        q1 (np.ndarray): Input array of quaternions with shape (..., 4).
        q2 (np.ndarray, optional): Input array of quaternions with shape (..., 4).
            If None, q2 is set to q1, making it a self-multiplication. Default is None.
        scalar_first (bool, optional): If True, assumes the scalar part is the first element.
            If False, assumes the scalar part is the last element. Default is True.
        channels_last (bool, optional): If True, assumes the channels are the last dimension.
            If False, assumes the channels are the second-to-last dimension. Default is True.

    Returns:
        np.ndarray: Result of quaternion multiplication with the same shape as the input arrays.

    Raises:
        AssertionError: If the last dimension of q1 and q2 is not 4.

    Notes:
        - If q2 is None, this function performs self-multiplication (q1 * q1).
        - The input arrays are cast to float before the computation.
        - If channels_last is False, the input arrays are transposed to switch channels and time axis.

    Quaternion Conjugate Formula:
        >>> q3 = [w1w2 - x1x2 - y1y2 - z1z2,
            w1x2 + x1w2 + y1z2 - z1y2,
            w1y2 - x1z2 + y1w2 + z1x2,
            w1z2 + x1y2 - y1x2 + z1w2]
    """

    # Parse input quaternions
    q2 = q1.copy() if q2 is None else q2

    # Cast arrays to float
    q1 = np.asarray(q1, float)
    q2 = np.asarray(q2, float)

    if not channels_last:
        # Take the tranpose, i.e. switch channels and time axis
        q1 = q1.T
        q2 = q2.T

    if not scalar_first:
        # Put the scalar first
        q1_tmp = q1.copy()
        q1[..., 0] = q1_tmp[..., -1]
        q1[..., 1:] = q1_tmp[..., :-1]
        del q1_tmp

        q2_tmp = q2.copy()
        q2[..., 0] = q2_tmp[..., -1]
        q2[..., 1:] = q2_tmp[..., :-1]
        del q2_tmp

    # Align shapes
    if q1.shape != q2.shape:
        q1, q2 = np.broadcast_arrays(q1, q2)
    assert q1.shape[-1] == 4

    # Multiply the quaternions
    q3 = np.zeros(q1.shape, float)
    q3[..., 0] = (
        q1[..., 0] * q2[..., 0]
        - q1[..., 1] * q2[..., 1]
        - q1[..., 2] * q2[..., 2]
        - q1[..., 3] * q2[..., 3]
    )
    q3[..., 1] = (
        q1[..., 0] * q2[..., 1]
        + q1[..., 1] * q2[..., 0]
        + q1[..., 2] * q2[..., 3]
        - q1[..., 3] * q2[..., 2]
    )
    q3[..., 2] = (
        q1[..., 0] * q2[..., 2]
        - q1[..., 1] * q2[..., 3]
        + q1[..., 2] * q2[..., 0]
        + q1[..., 3] * q2[..., 1]
    )
    q3[..., 3] = (
        q1[..., 0] * q2[..., 3]
        + q1[..., 1] * q2[..., 2]
        - q1[..., 2] * q2[..., 1]
        + q1[..., 3] * q2[..., 0]
    )

    if not scalar_first:
        # Put scalar part back in last channel
        q3_tmp = q3.copy()
        q3[..., -1] = q3_tmp[..., 0]
        q3[..., :-1] = q3_tmp[..., 1:]
        del q3_tmp

    if not channels_last:
        # Switch channels and time axis back
        q3 = q3.T
    return q3

quatnorm(q, channels_last=True)

Calculate the norm (magnitude) of quaternions.

This function computes the norm (magnitude) of quaternions along the specified axis, which represents the length of the quaternion vector.

Parameters:

Name Type Description Default
q ndarray

Input array of quaternions with shape (..., 4).

 required
channels_last bool

If True, assumes the channels are the last dimension. If False, assumes the channels are the first dimension. Default is True.

 True

Returns:

Type Description
ndarray

np.ndarray: Norm of quaternions along the specified axis with the same shape as the input array.
Notes
  • The input array is cast to float before the computation.
  • If channels_last is False, the input array is transposed to switch channels and time axis.
Quaternion Norm Calculation

The norm of a quaternion q is calculated as follows: norm(q) = sqrt(w^2 + x^2 + y^2 + z^2)

Source code in kielmat/utils/quaternion.py 
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
def quatnorm(q: np.ndarray, channels_last: bool = True) -> np.ndarray:
    """
    Calculate the norm (magnitude) of quaternions.

    This function computes the norm (magnitude) of quaternions along the specified axis,
    which represents the length of the quaternion vector.

    Args:
        q (np.ndarray): Input array of quaternions with shape (..., 4).
        channels_last (bool, optional): If True, assumes the channels are the last dimension.
            If False, assumes the channels are the first dimension. Default is True.

    Returns:
        np.ndarray: Norm of quaternions along the specified axis with the same shape as the input array.

    Notes:
        - The input array is cast to float before the computation.
        - If channels_last is False, the input array is transposed to switch channels and time axis.

    Quaternion Norm Calculation:
        >>> The norm of a quaternion q is calculated as follows:
        >>> norm(q) = sqrt(w^2 + x^2 + y^2 + z^2)
    """

    # Cast array to float
    q = np.asarray(q, float)

    # Calculate the quaternion norm
    norm = (
        np.linalg.norm(q, axis=-1, keepdims=True)
        if channels_last
        else np.linalg.norm(q, axis=0, keepdims=True)
    )
    return norm

quatnormalize(q, channels_last=True)

Normalize quaternions.

This function normalizes quaternions by dividing each quaternion by its magnitude (norm). The result is a unit quaternion with the same orientation as the original quaternion.

Parameters:

Name Type Description Default
q ndarray

Input array of quaternions with shape (..., 4).

 required
channels_last bool

If True, assumes the channels are the last dimension. If False, assumes the channels are the second-to-last dimension. Default is True.

 True

Returns:

Type Description
ndarray

np.ndarray: Normalized quaternions with the same shape as the input array.
Notes
  • The input array is cast to float before the computation.
  • If channels_last is False, the input array is transposed to switch channels and time axis.
Quaternion Normalization

The normalization of a quaternion q is performed by dividing each element of q by its norm: q_normalized = q / norm(q)

Source code in kielmat/utils/quaternion.py 
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
def quatnormalize(q: np.ndarray, channels_last: bool = True) -> np.ndarray:
    """
    Normalize quaternions.

    This function normalizes quaternions by dividing each quaternion by its magnitude (norm).
    The result is a unit quaternion with the same orientation as the original quaternion.

    Args:
        q (np.ndarray): Input array of quaternions with shape (..., 4).
        channels_last (bool, optional): If True, assumes the channels are the last dimension.
            If False, assumes the channels are the second-to-last dimension. Default is True.

    Returns:
        np.ndarray: Normalized quaternions with the same shape as the input array.

    Notes:
        - The input array is cast to float before the computation.
        - If channels_last is False, the input array is transposed to switch channels and time axis.

    Quaternion Normalization:
        >>> The normalization of a quaternion q is performed by dividing each element of q by its norm:
        >>> q_normalized = q / norm(q)
    """

    # Cast array to float
    q = np.asarray(q, float)

    # Calculate the norm
    norm = quatnorm(q, channels_last=channels_last)

    # Divide each quaternion by its norm
    q_out = q / norm
    return q_out

rotm2quat(R, method='auto')

Convert a 3x3 rotation matrix to a quaternion.

Source: - https://github.com/dlaidig/qmt/blob/0fa8d32eb461e14d78e9ddbd569664ea59bcea19/qmt/functions/quaternion.py#L1004

Parameters:

Name Type Description Default
R ndarray

A rotation matrix with shape (3, 3).

 required
scalar_first bool

If True, sets the first element as the scalar part. If False, sets the last element as the scalar part is the last element. Default is True.

 required
channels_last bool

If True, assumes the channels are the last dimension. If False, assumes the channels are the first dimension. Default is True.

 required

Returns:

Type Description
ndarray

np.ndarray: The quaternion corresponding to the rotation matrix.

Raises:

Type Description
AssertionError

If the shape of R is not (3, 3).
Notes
  • If q2 is None, this function performs self-multiplication (q1 * q1).
  • The input arrays are cast to float before the computation.
  • If channels_last is False, the input arrays are transposed to switch channels and time axis.
Source code in kielmat/utils/quaternion.py 
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
def rotm2quat(R: np.ndarray, method: int | str = "auto") -> np.ndarray:
    """
    Convert a 3x3 rotation matrix to a quaternion.

    Source:
    - https://github.com/dlaidig/qmt/blob/0fa8d32eb461e14d78e9ddbd569664ea59bcea19/qmt/functions/quaternion.py#L1004

    Args:
        R (np.ndarray): A rotation matrix with shape (3, 3).
        scalar_first (bool, optional): If True, sets the first element as the scalar part.
            If False, sets the last element as the scalar part is the last element. Default is True.
        channels_last (bool, optional): If True, assumes the channels are the last dimension.
            If False, assumes the channels are the first dimension. Default is True.

    Returns:
        np.ndarray: The quaternion corresponding to the rotation matrix.

    Raises:
        AssertionError: If the shape of R is not (3, 3).

    Notes:
        - If q2 is None, this function performs self-multiplication (q1 * q1).
        - The input arrays are cast to float before the computation.
        - If channels_last is False, the input arrays are transposed to switch channels and time axis.
    """

    # Cast array to float
    R = np.asarray(R, float)
    assert R.ndim >= 2 and R.shape[-2:] == (3, 3)

    w_sq = (1 + R[..., 0, 0] + R[..., 1, 1] + R[..., 2, 2]) / 4
    x_sq = (1 + R[..., 0, 0] - R[..., 1, 1] - R[..., 2, 2]) / 4
    y_sq = (1 - R[..., 0, 0] + R[..., 1, 1] - R[..., 2, 2]) / 4
    z_sq = (1 - R[..., 0, 0] - R[..., 1, 1] + R[..., 2, 2]) / 4

    q = np.zeros(R.shape[:-2] + (4,), float)
    if method == "auto":  # use the largest value to avoid numerical problems
        methods = np.argmax(np.array([w_sq, x_sq, y_sq, z_sq]), axis=0)
    elif method == "copysign":
        q[..., 0] = np.sqrt(w_sq)
        q[..., 1] = np.copysign(np.sqrt(x_sq), R[..., 2, 1] - R[..., 1, 2])
        q[..., 2] = np.copysign(np.sqrt(y_sq), R[..., 0, 2] - R[..., 2, 0])
        q[..., 3] = np.copysign(np.sqrt(z_sq), R[..., 1, 0] - R[..., 0, 1])
    elif method not in (0, 1, 2, 3):
        raise RuntimeError('invalid method, must be "copysign", "auto", 0, 1, 2 or 3')

    if method == 0 or method == "auto":
        ind = methods == 0 if method == "auto" else slice(None)
        q[ind, 0] = np.sqrt(w_sq[ind])
        q[ind, 1] = (R[ind, 2, 1] - R[ind, 1, 2]) / (4 * q[ind, 0])
        q[ind, 2] = (R[ind, 0, 2] - R[ind, 2, 0]) / (4 * q[ind, 0])
        q[ind, 3] = (R[ind, 1, 0] - R[ind, 0, 1]) / (4 * q[ind, 0])
    if method == 1 or method == "auto":
        ind = methods == 1 if method == "auto" else slice(None)
        q[ind, 1] = np.sqrt(x_sq[ind])
        q[ind, 0] = (R[ind, 2, 1] - R[ind, 1, 2]) / (4 * q[ind, 1])
        q[ind, 2] = (R[ind, 1, 0] + R[ind, 0, 1]) / (4 * q[ind, 1])
        q[ind, 3] = (R[ind, 0, 2] + R[ind, 2, 0]) / (4 * q[ind, 1])
    if method == 2 or method == "auto":
        ind = methods == 2 if method == "auto" else slice(None)
        q[ind, 2] = np.sqrt(y_sq[ind])
        q[ind, 0] = (R[ind, 0, 2] - R[ind, 2, 0]) / (4 * q[ind, 2])
        q[ind, 1] = (R[ind, 1, 0] + R[ind, 0, 1]) / (4 * q[ind, 2])
        q[ind, 3] = (R[ind, 2, 1] + R[ind, 1, 2]) / (4 * q[ind, 2])
    if method == 3 or method == "auto":
        ind = methods == 3 if method == "auto" else slice(None)
        q[ind, 3] = np.sqrt(z_sq[ind])
        q[ind, 0] = (R[ind, 1, 0] - R[ind, 0, 1]) / (4 * q[ind, 3])
        q[ind, 1] = (R[ind, 0, 2] + R[ind, 2, 0]) / (4 * q[ind, 3])
        q[ind, 2] = (R[ind, 2, 1] + R[ind, 1, 2]) / (4 * q[ind, 3])
    return q