SciPy mặc định đọc quaternion theo thứ tự (x, y, z, w). Phép hợp thành r2 * r1 thực hiện r1 trước rồi đến r2. Với camera, cần xác định thêm phép quay đang đưa tọa độ từ camera sang world hay theo chiều ngược lại.
Bài này dùng scipy.spatial.transform.Rotation để kiểm tra ba điều đó bằng vector và ma trận cụ thể. Ví dụ camera có cả góc quay lẫn vị trí, kèm phép đổi hệ tọa độ thuận/ngược trên 1.000 điểm. Bạn có thể đọc trước tứ nguyên số Quaternion và cách biểu diễn phép quay 3D để hiểu nền tảng.
Ghi quy ước trước khi tạo camera
Ví dụ chọn hai hệ tọa độ tay phải. Hệ world có $x$ hướng Đông, $y$ hướng Bắc và $z$ hướng lên. Hệ camera có $x$ sang phải, $y$ xuống dưới và $z$ hướng ra trước ống kính. Các tọa độ vị trí tính bằng mét.
Đặt $R_{wc}$ là ma trận chuyển tọa độ từ camera sang world. Chữ đầu chỉ hệ đích, chữ sau chỉ hệ nguồn. Vector $t_{wc}$ là vị trí gốc camera biểu diễn trong world. Với vector cột, một điểm có hai cách biểu diễn liên hệ bởi:
$$ p_w = R_{wc}p_c + t_{wc}. $$Một điểm p_c trong mã có shape (3,). Một tập $N$ điểm có shape (N, 3), mỗi hàng là một điểm. Với kiểu lưu theo hàng này, phép nhân ma trận tương ứng trong NumPy là points_camera @ R_wc.T + t_wc.
Rotation.apply nhận một vector (3,) hoặc nhiều vector (N, 3). Khi đối tượng Rotation chứa một phép quay, cùng phép quay được áp dụng cho tất cả các điểm trong mảng. Tài liệu SciPy 1.15.2 về apply
xyzw và wxyz cho hai phép quay khác nhau nếu đọc nhầm
Ta tạo phép quay 90° quanh trục $z$. Mã gọi R.from_euler("z", 90, degrees=True) và in ra hai thứ tự thành phần.
| Biểu diễn | Giá trị làm tròn |
|---|---|
xyzw, mặc định |
[0, 0, 0.707107, 0.707107] |
wxyz, scalar first |
[0.707107, 0, 0, 0.707107] |
Nếu dữ liệu đầu vào là wxyz, dùng R.from_quat(q, scalar_first=True). Với xyzw, dùng mặc định. SciPy chuẩn hóa quaternion đầu vào về độ dài 1, nhưng chính đối số scalar_first quyết định cách hiểu các thành phần. Tài liệu from_quat
Khi đưa mảng wxyz ở trên vào from_quat theo mặc định, kết quả trở thành phép quay 90° quanh $x$. Mã kiểm tra bằng vector đơn vị $[1,0,0]$:
| Cách đọc quaternion | Vector đầu ra |
|---|---|
| Đọc đúng phép quay quanh $z$ | $[0,1,0]$ |
Đọc wxyz thành xyzw |
$[1,0,0]$ |
Góc của phép quay tương đối giữa hai kết quả là 120°. Mảng đọc sai vẫn có độ dài 1 và vẫn tạo một ma trận quay hợp lệ. Vì vậy, hãy kiểm tra một vector chuẩn có hướng đầu ra đã biết khi nối hai thư viện.
Khi so sánh hai quaternion, cũng có thể chuyển sang ma trận hoặc đo góc quay tương đối. Mã cuối bài xác nhận $q$ và $-q$ cho cùng ma trận quay, dù hai mảng thành phần có dấu ngược nhau.
Thứ tự nhân quyết định hướng cuối
Đặt rx là phép quay 90° quanh $x$, rz là phép quay 90° quanh $z$. Trong SciPy, rz * rx áp dụng rx trước rồi rz. Ma trận tương ứng là $R_zR_x$. Tài liệu về phép hợp thành Rotation
Với vector ban đầu $[0,0,1]$, chương trình in ra:
| Trình tự | Biểu thức | Kết quả làm tròn |
|---|---|---|
| Quay quanh $x$, rồi quanh $z$ | (rz * rx).apply(v) |
$[1,0,0]$ |
| Quay quanh $z$, rồi quanh $x$ | (rx * rz).apply(v) |
$[0,-1,0]$ |
np.testing.assert_allclose kiểm tra thêm rằng (rz * rx).apply(v) khớp với rz.apply(rx.apply(v)). Sai số Frobenius giữa ma trận hợp thành và tích rz.as_matrix() @ rx.as_matrix() trong lần chạy là $4.930 \times 10^{-32}$.
Tất cả góc truyền vào from_euler trong mã đều có degrees=True. Giá trị mặc định của API là radian. Với chuỗi nhiều trục, chữ thường như "xyz" biểu diễn quay quanh các trục cố định, còn chữ hoa như "XYZ" biểu diễn quay quanh các trục gắn với vật thể. Ví dụ này tạo từng phép quay một trục rồi nhân tường minh để thấy rõ thứ tự. Tài liệu from_euler
Quay vector và đổi hệ trục
Giữ hệ world cố định rồi quay vật lý vector $[1,0,0]$ thêm 90° quanh $z$, ta nhận $[0,1,0]$. Đây là cách nhìn active rotation.
Nếu giữ nguyên vector vật lý đó và quay hệ trục thêm 90° quanh $z$, tọa độ của vector trong hệ trục mới là $[0,-1,0]$. Đây là cách nhìn passive, dùng ma trận nghịch đảo. Trong mã, hai phép tính lần lượt là rz.apply(v) và rz.apply(v, inverse=True).
Đối với một ma trận quay, nghịch đảo bằng chuyển vị. Tên biến và hệ tọa độ đầu vào quyết định cách diễn giải thao tác. Một đối tượng Rotation cung cấp phép toán quay, còn ý nghĩa camera/world và gốc tọa độ do chương trình định nghĩa.
Đặt camera rồi đưa các điểm sang world
Ban đầu, camera nhìn về hướng Đông và có hướng lên trùng với $+z$ của world. Do đó, các trục camera biểu diễn trong world lần lượt là:
- Trục phải $x_c$ hướng về Nam, có tọa độ $[0,-1,0]$.
- Trục xuống $y_c$ có tọa độ $[0,0,-1]$.
- Trục trước $z_c$ hướng về Đông, có tọa độ $[1,0,0]$.
Ghép ba vector này thành ba cột của ma trận base_wc trong mã. Sau đó quay hướng camera thêm 30° quanh trục $z$ cố định của world bằng R.from_euler("z", 30, degrees=True) * R.from_matrix(base_wc). Camera nhìn chếch từ Đông về Bắc, còn hướng lên vẫn trùng với $+z$ world.
Ma trận $R_{wc}$ thu được, làm tròn sáu chữ số, là:
1[[ 0.5 0. 0.866025]
2 [-0.866025 0. 0.5 ]
3 [ 0. -1. 0. ]]
Đặt camera tại $t_{wc}=[2,-1,1.5]$. Điểm nằm thẳng trước ống kính 2 m có tọa độ camera $[0,0,2]$. Áp dụng r_wc.apply(camera_points) + t_wc cho bốn điểm, ta có:
| Tọa độ camera | Tọa độ world, làm tròn |
|---|---|
| $[0,0,2]$ | $[3.732051,0,1.5]$ |
| $[1,0,2]$ | $[4.232051,-0.866025,1.5]$ |
| $[0,1,2]$ | $[3.732051,0,0.5]$ |
| $[0,0,0]$ | $[2,-1,1.5]$ |
Điểm gốc camera đi tới đúng vị trí camera trong world. Tăng thành phần $y_c$ thêm 1 m làm độ cao world giảm từ 1,5 xuống 0,5 m, khớp với quy ước $y_c$ hướng xuống.
Đổi ngược về camera và kiểm tra sai số
Từ phương trình thuận, trừ vị trí camera rồi áp dụng phép quay nghịch đảo:
$$ p_c = R_{wc}^{T}(p_w-t_{wc}) = R_{cw}p_w+t_{cw}, \qquad R_{cw}=R_{wc}^{T}, \qquad t_{cw}=-R_{wc}^{T}t_{wc}. $$Trong mã, chiều ngược là r_wc.inv().apply(world_points - t_wc). Rotation.inv() tạo phép quay nghịch đảo. Khi ghép với phép quay ban đầu, ta thu phép đồng nhất. Tài liệu inv
Ở ví dụ này, $t_{cw}$ làm tròn là $[-1.866025,1.5,-1.232051]$. Nó có cả bước quay vị trí camera sang hệ mới, nên cần tính bằng công thức trên thay vì chỉ đảo dấu $t_{wc}$.
Chương trình dùng seed 2026 để tạo thêm 1.000 điểm, đổi camera → world → camera và so lại với đầu vào. Kết quả chạy bằng Python 3.10.11, NumPy 2.2.5, SciPy 1.15.2:
| Kiểm tra | Kết quả |
|---|---|
| Sai số Euclid lớn nhất sau hành trình thuận/ngược | $2.318 \times 10^{-15}$ |
| Chuẩn Frobenius của $R_{wc}^{T}R_{wc}-I$ | $7.692 \times 10^{-16}$ |
| Định thức $R_{wc}$ | 1,000000000000 |
Mã đặt ngưỡng tuyệt đối $10^{-12}$ cho các kiểm tra này. Những sai số rất nhỏ xác nhận tính nhất quán của phép quay và phép đổi ngược trong ví dụ. Để kiểm tra quy ước camera, hãy giữ thêm các điểm có ý nghĩa hình học đã biết, như gốc camera và điểm thẳng trước ống kính. Cả một phép biến đổi có hướng sai lẫn nghịch đảo của nó vẫn có thể đưa điểm trở về chỗ cũ.
Mã Python tái lập đầy đủ
Lưu mã thành experiment.py rồi chạy python3 experiment.py. Các ma trận và vector được in với sáu chữ số thập phân để dễ đọc. Những phép kiểm tra dùng giá trị dấu phẩy động đầy đủ, trước khi làm tròn.
1import platform
2
3import numpy as np
4import scipy
5from scipy.spatial.transform import Rotation as R
6
7
8def main():
9 np.set_printoptions(precision=6, suppress=True)
10 print(f"Python {platform.python_version()}, NumPy {np.__version__}, SciPy {scipy.__version__}")
11
12 v = np.array([1.0, 0.0, 0.0])
13 rx = R.from_euler("x", 90, degrees=True)
14 rz = R.from_euler("z", 90, degrees=True)
15 q_xyzw = rz.as_quat()
16 q_wxyz = rz.as_quat(scalar_first=True)
17 right_xyzw = R.from_quat(q_xyzw)
18 right_wxyz = R.from_quat(q_wxyz, scalar_first=True)
19 wrong_order = R.from_quat(q_wxyz)
20 print("z90 xyzw:", q_xyzw)
21 print("z90 wxyz:", q_wxyz)
22 print("correct z90 applied to x:", right_wxyz.apply(v))
23 print("wxyz read as xyzw, applied to x:", wrong_order.apply(v))
24 print("wrong-order angular error, degrees:",
25 (wrong_order.inv() * rz).magnitude() * 180 / np.pi)
26 np.testing.assert_allclose(right_xyzw.as_matrix(), right_wxyz.as_matrix(), atol=1e-12, rtol=0)
27 np.testing.assert_allclose(R.from_quat(-q_xyzw).as_matrix(), rz.as_matrix(), atol=1e-12, rtol=0)
28
29 forward = np.array([0.0, 0.0, 1.0])
30 print("x90 then z90, forward:", (rz * rx).apply(forward))
31 print("z90 then x90, forward:", (rx * rz).apply(forward))
32 composition_error = np.linalg.norm(
33 (rz * rx).as_matrix() - rz.as_matrix() @ rx.as_matrix()
34 )
35 print(f"composition matrix Frobenius residual: {composition_error:.3e}")
36 np.testing.assert_allclose(
37 (rz * rx).apply(forward), rz.apply(rx.apply(forward)), atol=1e-12
38 )
39 print("active z90, fixed world axes:", rz.apply(v))
40 print("same fixed vector in axes rotated z90:", rz.apply(v, inverse=True))
41
42 base_wc = np.array([[0.0, 0.0, 1.0],
43 [-1.0, 0.0, 0.0],
44 [0.0, -1.0, 0.0]])
45 r_wc = R.from_euler("z", 30, degrees=True) * R.from_matrix(base_wc)
46 t_wc = np.array([2.0, -1.0, 1.5])
47 camera_points = np.array([[0.0, 0.0, 2.0],
48 [1.0, 0.0, 2.0],
49 [0.0, 1.0, 2.0],
50 [0.0, 0.0, 0.0]])
51 matrix_wc = r_wc.as_matrix()
52 world_points = r_wc.apply(camera_points) + t_wc
53 recovered = r_wc.inv().apply(world_points - t_wc)
54 matrix_cw = matrix_wc.T
55 t_cw = -matrix_cw @ t_wc
56 print("R_wc:\n", matrix_wc, sep="")
57 print("t_wc:", t_wc)
58 print("camera points -> world points:\n", world_points, sep="")
59 print("R_cw:\n", matrix_cw, sep="")
60 print("t_cw:", t_cw)
61 np.testing.assert_allclose(world_points, camera_points @ matrix_wc.T + t_wc, atol=1e-12, rtol=0)
62 np.testing.assert_allclose(recovered, camera_points, atol=1e-12, rtol=0)
63 np.testing.assert_allclose(world_points @ matrix_cw.T + t_cw, camera_points, atol=1e-12, rtol=0)
64
65 rng = np.random.default_rng(2026)
66 points = rng.normal(size=(1000, 3))
67 roundtrip = r_wc.inv().apply(r_wc.apply(points) + t_wc - t_wc)
68 max_l2_error = np.linalg.norm(roundtrip - points, axis=1).max()
69 orthogonality_error = np.linalg.norm(matrix_wc.T @ matrix_wc - np.eye(3))
70 print(f"1000-point roundtrip max L2 residual: {max_l2_error:.3e}")
71 print(f"R_wc orthogonality Frobenius residual: {orthogonality_error:.3e}")
72 print(f"det(R_wc): {np.linalg.det(matrix_wc):.12f}")
73 assert max_l2_error < 1e-12
74 assert orthogonality_error < 1e-12
75 np.testing.assert_allclose(np.linalg.det(matrix_wc), 1.0, atol=1e-12, rtol=0)
76
77
78if __name__ == "__main__":
79 main()
Đọc thêm trên blog
Nếu muốn nối phép quay với một hệ đại số rộng hơn, bài đại số Clifford, vector, số phức và quaternion giải thích mối liên hệ giữa các cấu trúc này.
Bình luận