目標物位置轉換工具

以下是本儲存庫所欲解決的問題：

已知機器人偵測到以相機位置為原點的目標物相對座標，另外已有機器人的絕對座標與絕對指向（以四元數表示）。以上述條件換算出目標物的絕對座標。

基本概念

以四元數進行座標旋轉

三維空間中，已知一四元數 $q=w+xi+yj+zk$ 對應機器人之指向。對應此四元數對應的 $3 \times 3$ 旋轉矩陣為

$$R_q = \begin{pmatrix} 1-2y^2-2z^2 & 2xy-2wz & 2wz+2wy \\\ 2xy+2wz & 1-2x^2-2z^2 & 2yz-2wz \\\ 2xz-2wy & 2yz+2wz & 1-2x^2-2y^2 \end{pmatrix}$$

齊次坐標

我們可以把已知的某目標物相對位置向量（以相機為原點） $\vec{p_r}=(p_x,p_y,p_z)$ 表示為齊次坐標 $(p,1)$，以矩陣表示為

$$P_r = \begin{pmatrix} p_x \\\ p_y \\\ p_z \\\ 1 \end{pmatrix}$$

齊次變換矩陣

已知相對坐標系原點在絕對坐標系的位置為 $t=(t_x,t_y,t_z)$，將其與前面四元數所得出的旋轉矩陣 $R_q$ 結合，可得完整的 $4 \times 4$ 齊次變換矩陣

$$T = \begin{pmatrix} & R_q & & t \\\ 0 & 0 & 0 & 1 \end{pmatrix}$$

，此矩陣已同時包含了旋轉與平移之操作。可直接以 $T \cdot P_r$ 求得具備目標物絕對坐標的齊次坐標 $(p_g, 1)$。