Skip to content

Geometry

Geometric utilities.

compose_homogeneous_transform(translation, rotation)

Compose a homogeneous transform from a translation column vector \(\mathbf{t}\) and rotation matrix \(\mathbf{R}\).

\[ \mathbf{T} = \left[\begin{array}{c:c} \mathbf{R} & \mathbf{t} \\ \hdashline 0 & 1 \end{array}\right] \]

Parameters:

Name Type Description Default
translation ndarray

The translation \(\mathbf{t}\) as a 3x1 numpy array.

 required
rotation ndarray

The rotation \(\mathbf{R}\) as a 3x3 numpy array.

 required

Returns:

Type Description
ndarray

The homogeneous transform \(\mathbf{T}\) as a 4x4 numpy array.
Source code in src/compas_camcal/util/geometry.py 
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
def compose_homogeneous_transform(translation: np.ndarray, rotation: np.ndarray) -> np.ndarray:
    """
    Compose a homogeneous transform from a translation column vector $\\mathbf{t}$ and rotation matrix $\\mathbf{R}$.

    $$
    \\mathbf{T} = \\left[\\begin{array}{c:c}
    \\mathbf{R} & \\mathbf{t} \\\\ \\hdashline
    0 & 1
    \\end{array}\\right]
    $$

    Args:
        translation: The translation $\\mathbf{t}$ as a 3x1 numpy array.
        rotation: The rotation $\\mathbf{R}$ as a 3x3 numpy array.

    Returns:
        The homogeneous transform $\\mathbf{T}$ as a 4x4 numpy array.
    """
    return np.concatenate(
        (np.concatenate((rotation, translation), axis=1), np.array([[0, 0, 0, 1]])), axis=0
    )

decompose_homogeneous_transform(transform)

Decompose a homogeneous transform into a translation column vector \(\mathbf{t}\) and a rotation matrix \(\mathbf{R}\).

\[ \begin{align*} \mathbf{t} & = \mathbf{T}[1, 2, 3; 4] \\ \mathbf{R} & = \mathbf{T}[1, 2, 3; 1, 2, 3] \end{align*} \]

Parameters:

Name Type Description Default
transform ndarray

The homogeneous transform \(\mathbf{T}\) as a 4x4 numpy array.

 required

Returns:

Type Description
ndarray

The translation \(\mathbf{t}\) as a 3x1 numpy array.
ndarray

The rotation \(\mathbf{R}\) as a 3x3 numpy array.
Source code in src/compas_camcal/util/geometry.py 
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
def decompose_homogeneous_transform(transform: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    """
    Decompose a homogeneous transform into a translation column vector $\\mathbf{t}$ and a rotation matrix $\\mathbf{R}$.

    $$
    \\begin{align*}
    \\mathbf{t} & = \\mathbf{T}[1, 2, 3; 4] \\\\
    \\mathbf{R} & = \\mathbf{T}[1, 2, 3; 1, 2, 3]
    \\end{align*}
    $$

    Args:
        transform: The homogeneous transform $\\mathbf{T}$ as a 4x4 numpy array.

    Returns:
        The translation $\\mathbf{t}$ as a 3x1 numpy array.
        The rotation $\\mathbf{R}$ as a 3x3 numpy array.
    """
    return transform[:3, 3], transform[:3, :3]

euler_to_rotation_matrix(angle_1, angle_2, angle_3)

Convert Euler angles to a rotation matrix.

This function follows the Tait-Bryan angles convention:

\[ \mathbf{R} = \mathbf{Z}_1 \mathbf{Y}_2 \mathbf{X}_3 \]

Parameters:

Name Type Description Default
angle_1 float

\(\theta_1\) (for \(\mathbf{Z}_1\))

 required
angle_2 float

\(\theta_2\) (for \(\mathbf{Y}_2\))

 required
angle_3 float

\(\theta_3\) (for \(\mathbf{X}_3\))

 required
Source code in src/compas_camcal/util/geometry.py 
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
def euler_to_rotation_matrix(angle_1: float, angle_2: float, angle_3: float) -> np.ndarray:
    """
    Convert Euler angles to a rotation matrix.

    This function follows the Tait-Bryan angles convention:

    $$
    \\mathbf{R} = \\mathbf{Z}_1 \\mathbf{Y}_2 \\mathbf{X}_3
    $$

    Args:
        angle_1: $\\theta_1$ (for $\\mathbf{Z}_1$)
        angle_2: $\\theta_2$ (for $\\mathbf{Y}_2$)
        angle_3: $\\theta_3$ (for $\\mathbf{X}_3$)
    """
    return z_rotation_matrix(angle_1) @ y_rotation_matrix(angle_2) @ x_rotation_matrix(angle_3)

quaternion_to_rotation_matrix(r, i, j, k)

Convert a quaternion \(\mathbf{q} = q_r + q_i \mathbf{i} + q_j \mathbf{j} + q_k \mathbf{k}\) to a rotation matrix.

Directly computes:

\[ \mathbf{R} = \begin{bmatrix} 1 - 2 \sin(q_j^2 + q_k^2) & 2 \sin(q_i q_j - q_k q_r) & 2 \sin(q_i q_k + q_j q_r) \\ 2 \sin(q_i q_j + q_k q_r) & 1 - 2 \sin(q_i^2 + q_k^2) & 2 \sin(q_j q_k - q_i q_r) \\ 2 \sin(q_i q_k - q_j q_r) & 2 \sin(q_j q_k + q_i q_r) & 1 - 2 \sin(q_i^2 + q_j^2) \end{bmatrix} \]

Parameters:

Name Type Description Default
r float

The real part of the quaternion \(q_r\).

 required
i float

The imaginary part of the quaternion \(q_i\).

 required
j float

The imaginary part of the quaternion \(q_j\).

 required
k float

The imaginary part of the quaternion \(q_k\).

 required

Returns:

Type Description
ndarray

The rotation matrix \(\mathbf{R}\) as a 3x3 numpy array.
Source code in src/compas_camcal/util/geometry.py 
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
def quaternion_to_rotation_matrix(r: float, i: float, j: float, k: float) -> np.ndarray:
    """
    Convert a quaternion $\\mathbf{q} = q_r + q_i \\mathbf{i} + q_j \\mathbf{j} + q_k \\mathbf{k}$ to a rotation matrix.

    Directly computes:

    $$
    \\mathbf{R} = \\begin{bmatrix}
    1 - 2 \\sin(q_j^2 + q_k^2) & 2 \\sin(q_i q_j - q_k q_r) & 2 \\sin(q_i q_k + q_j q_r) \\\\
    2 \\sin(q_i q_j + q_k q_r) & 1 - 2 \\sin(q_i^2 + q_k^2) & 2 \\sin(q_j q_k - q_i q_r) \\\\
    2 \\sin(q_i q_k - q_j q_r) & 2 \\sin(q_j q_k + q_i q_r) & 1 - 2 \\sin(q_i^2 + q_j^2)
    \\end{bmatrix}
    $$

    Args:
        r: The real part of the quaternion $q_r$.
        i: The imaginary part of the quaternion $q_i$.
        j: The imaginary part of the quaternion $q_j$.
        k: The imaginary part of the quaternion $q_k$.

    Returns:
        The rotation matrix $\\mathbf{R}$ as a 3x3 numpy array.
    """
    return np.array(
        [
            [1 - 2 * np.sin(j**2 + k**2), 2 * np.sin(i * j - k * r), 2 * np.sin(i * k + j * r)],
            [2 * np.sin(i * j + k * r), 1 - 2 * np.sin(i**2 + k**2), 2 * np.sin(j * k - i * r)],
            [2 * np.sin(i * k - j * r), 2 * np.sin(j * k + i * r), 1 - 2 * np.sin(i**2 + j**2)],
        ],
        dtype=np.float64,
    )

rotation_matrix_to_euler(matrix)

Convert a rotation matrix to Euler angles.

\[ \begin{align*} \theta_1 & = \arctan\left(\frac{\mathbf{R}_{21}}{\mathbf{R}_{11}}\right) \\ \theta_2 & = \arctan\left(\frac{-\mathbf{R}_{31}}{\sqrt{1 - \mathbf{R}_{31}^2}}\right) \\ \theta_3 & = \arctan\left(\frac{\mathbf{R}_{32}}{\mathbf{R}_{33}}\right) \end{align*} \]

Parameters:

Name Type Description Default
matrix ndarray

The rotation matrix \(\mathbf{R}\) as a 3x3 numpy array.

 required

Returns:

Type Description
float

\(\theta_1\)
float

\(\theta_2\)
float

\(\theta_3\)
Source code in src/compas_camcal/util/geometry.py 
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
def rotation_matrix_to_euler(matrix: np.ndarray) -> Tuple[float, float, float]:
    """
    Convert a rotation matrix to Euler angles.

    $$
    \\begin{align*}
    \\theta_1 & = \\arctan\\left(\\frac{\\mathbf{R}_{21}}{\\mathbf{R}_{11}}\\right) \\\\
    \\theta_2 & = \\arctan\\left(\\frac{-\\mathbf{R}_{31}}{\\sqrt{1 - \\mathbf{R}_{31}^2}}\\right) \\\\
    \\theta_3 & = \\arctan\\left(\\frac{\\mathbf{R}_{32}}{\\mathbf{R}_{33}}\\right)
    \\end{align*}
    $$

    Args:
        matrix: The rotation matrix $\\mathbf{R}$ as a 3x3 numpy array.

    Returns:
        $\\theta_1$
        $\\theta_2$
        $\\theta_3$
    """
    angle_1 = np.arctan2(matrix[1, 0], matrix[0, 0])
    angle_2 = np.arctan2(-matrix[2, 0], np.sqrt(1 - matrix[2, 0] ** 2))
    angle_3 = np.arctan2(matrix[2, 1], matrix[2, 2])
    return angle_1, angle_2, angle_3

rotation_matrix_to_quaternion(matrix)

Convert a rotation matrix to a quaternion.

\[ \begin{align*} q_r & = \frac{1}{2} \sqrt{1 + \mathbf{R}_{11} + \mathbf{R}_{22} + \mathbf{R}_{33}} \\ q_i & = \frac{\mathbf{R}_{32} - \mathbf{R}_{23}}{4 q_r} \\ q_j & = \frac{\mathbf{R}_{13} - \mathbf{R}_{31}}{4 q_r} \\ q_k & = \frac{\mathbf{R}_{21} - \mathbf{R}_{12}}{4 q_r} \end{align*} \]

Parameters:

Name Type Description Default
matrix ndarray

The rotation matrix \(\mathbf{R}\) as a 3x3 numpy array.

 required

Returns:

Type Description
float

\(q_r\)
float

\(q_i\)
float

\(q_j\)
float

\(q_k\)
Source code in src/compas_camcal/util/geometry.py 
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
def rotation_matrix_to_quaternion(matrix: np.ndarray) -> Tuple[float, float, float, float]:
    """
    Convert a rotation matrix to a quaternion.

    $$
    \\begin{align*}
    q_r & = \\frac{1}{2} \\sqrt{1 + \\mathbf{R}_{11} + \\mathbf{R}_{22} + \\mathbf{R}_{33}} \\\\
    q_i & = \\frac{\\mathbf{R}_{32} - \\mathbf{R}_{23}}{4 q_r} \\\\
    q_j & = \\frac{\\mathbf{R}_{13} - \\mathbf{R}_{31}}{4 q_r} \\\\
    q_k & = \\frac{\\mathbf{R}_{21} - \\mathbf{R}_{12}}{4 q_r}
    \\end{align*}
    $$

    Args:
        matrix: The rotation matrix $\\mathbf{R}$ as a 3x3 numpy array.

    Returns:
        $q_r$
        $q_i$
        $q_j$
        $q_k$
    """
    r = np.sqrt(1 + matrix[0, 0] + matrix[1, 1] + matrix[2, 2]) / 2
    i = (matrix[2, 1] - matrix[1, 2]) / (4 * r)
    j = (matrix[0, 2] - matrix[2, 0]) / (4 * r)
    k = (matrix[1, 0] - matrix[0, 1]) / (4 * r)
    return r, i, j, k

x_rotation_matrix(angle)

Create a rotation matrix about the x-axis.

\[ \mathbf{X} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} \]

Parameters:

Name Type Description Default
angle float

The angle \(\theta\) in radians.

 required

Returns:

Type Description
ndarray

The rotation matrix \(\mathbf{X}\) as a 3x3 numpy array.
Source code in src/compas_camcal/util/geometry.py 
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
def x_rotation_matrix(angle: float) -> np.ndarray:
    """
    Create a rotation matrix about the x-axis.

    $$
    \\mathbf{X} = \\begin{bmatrix}
        1 & 0 & 0 \\\\
        0 & \\cos\\theta & -\\sin\\theta \\\\
        0 & \\sin\\theta & \\cos\\theta
    \\end{bmatrix}
    $$

    Args:
        angle: The angle $\\theta$ in radians.

    Returns:
        The rotation matrix $\\mathbf{X}$ as a 3x3 numpy array.
    """
    sx = np.sin(angle)
    cx = np.cos(angle)
    return np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]], dtype=np.float64)

y_rotation_matrix(angle)

Create a rotation matrix about the y-axis.

\[ \mathbf{Y} = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \]

Parameters:

Name Type Description Default
angle float

The angle \(\theta\) in radians.

 required

Returns:

Type Description
ndarray

The rotation matrix \(\mathbf{Y}\) as a 3x3 numpy array.
Source code in src/compas_camcal/util/geometry.py 
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
def y_rotation_matrix(angle: float) -> np.ndarray:
    """
    Create a rotation matrix about the y-axis.

    $$
    \\mathbf{Y} = \\begin{bmatrix}
        \\cos\\theta & 0 & \\sin\\theta \\\\
        0 & 1 & 0 \\\\
        -\\sin\\theta & 0 & \\cos\\theta
    \\end{bmatrix}
    $$

    Args:
        angle: The angle $\\theta$ in radians.

    Returns:
        The rotation matrix $\\mathbf{Y}$ as a 3x3 numpy array.
    """
    sy = np.sin(angle)
    cy = np.cos(angle)
    return np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]], dtype=np.float64)

z_rotation_matrix(angle)

Create a rotation matrix about the z-axis.

\[ \mathbf{Z} = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

Parameters:

Name Type Description Default
angle float

The angle \(\theta\) in radians.

 required

Returns:

Type Description
ndarray

The rotation matrix \(\mathbf{Z}\) as a 3x3 numpy array.
Source code in src/compas_camcal/util/geometry.py 
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
def z_rotation_matrix(angle: float) -> np.ndarray:
    """
    Create a rotation matrix about the z-axis.

    $$
    \\mathbf{Z} = \\begin{bmatrix}
        \\cos\\theta & -\\sin\\theta & 0 \\\\
        \\sin\\theta & \\cos\\theta & 0 \\\\
        0 & 0 & 1
    \\end{bmatrix}
    $$

    Args:
        angle: The angle $\\theta$ in radians.

    Returns:
        The rotation matrix $\\mathbf{Z}$ as a 3x3 numpy array.
    """
    sz = np.sin(angle)
    cz = np.cos(angle)
    return np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]], dtype=np.float64)