Ước lượng ma trận quay vector A thành vector B trong không gian 3 chiều

Giả sử biết trước vector \textbf{a} và vector \textbf{b} trong không gian 3D với \Vert \textbf{a} \Vert = \Vert \textbf{b} \Vert, và ta muốn ước lượng ma trận quay M sao cho \textbf{b} = M\textbf{a}. Yêu cầu này đôi khi xuất hiện khi làm các ứng dụng 3D, hoặc trong bước calibration dữ liệu 3D.

Tất nhiên vì ta muốn xác định ma trận quay, nên \textbf{a}\textbf{b} phải có cùng độ dài: \Vert \textbf{a} \Vert = \Vert \textbf{b} \Vert. Trong trường hợp \Vert \textbf{a} \Vert \neq \Vert \textbf{b} \Vert, ta có thể tính thêm một phép tỉ lệ, tuy nhiên bài này chỉ bàn tới phép quay.

Tóm lại cho 2 vector \textbf{a}, \textbf{b} với \Vert \textbf{a} \Vert = \Vert \textbf{b} \Vert, nhận xét rằng:

  • Trục của phép quay từ \textbf{a} đến \textbf{b} là tích có hướng (cross product) của 2 vector: \textbf{n} = \textbf{a} \times \textbf{b}
  • Cosine của góc giữa 2 vector là tính vô hướng (dot product) chia cho tích độ dài của chúng: \cos\theta= \frac{\textbf{a} \cdot \textbf{b}}{\Vert \textbf{a} \Vert . \Vert \textbf{b} \Vert}

Mặt khác, phép quay một góc \theta quanh trục \textbf{n} được biểu diễn dưới dạng quaternion như sau:

\displaystyle \textbf{q} = \left[\begin{matrix}\cos\frac{\theta}{2}\\ \left(\sin\frac{\theta}{2}\right)\frac{1}{\Vert \textbf{n}\Vert}\textbf{n}\end{matrix}\right]

Như vậy chỉ cần tính được \theta\textbf{n} thì sẽ tính được quaternion trên. Sau khi tính được quaternion, ta hoàn toàn tính được ma trận quay bằng cách dùng công thức chuyển đổi ở đây.

Đoạn code sau đây tính quaternion để quay từ vector v1 thành vector v2:

import numpy as np
from numpy.linalg import norm

def estimate(v1, v2):
    '''Estimate the rotation quaternion needed in order to rotate vector v1 into v2.
    It is assumed that v1 and v2 has the same length (i.e. their Euclidean norms are equal),
    otherwise the rotation quaternion can not be determined correctly.
    '''
    assert type(v1) is np.ndarray and v1.shape == (3, )
    assert type(v2) is np.ndarray and v2.shape == (3, )
    assert round(norm(v1), 5) == round(norm(v2), 5), 'v1 and v2 must have the same length'

    n = np.cross(v1, v2)
    n /= norm(n)
    xHalf = np.arccos(v1.dot(v2)/(norm(v1)*norm(v2)))*0.5
    sinxHalf = np.sin(xHalf)
    return np.asarray([np.cos(xHalf), sinxHalf*n[0], sinxHalf*n[1], sinxHalf*n[2]])

Như thường lệ, code này (và một số hàm khác liên quan đến quaternion) có thể xem trên github.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s