halcon计算两个四元数的角度

要计算两个四元数之间的角度，可以使用四元数的点积。具体来说，如果 $q_1$ 和 $q_2$ 是两个四元数，则它们的点积可以表示为：

$$
q_1 \cdot q_2 = q_1^* q_2 = w_1 w_2 + x_1 x_2 + y_1 y_2 + z_1 z_2
$$

其中，$q_1^*$ 表示 $q_1$ 的共轭四元数，$w$、$x$、$y$ 和 $z$ 分别表示四元数中的实部和虚部。

然后，可以使用以下公式计算两个四元数的角度（单位为弧度）：

$$
\theta = 2\cos^{-1}\left(\min\left(1, \frac{q_1 \cdot q_2}{\|q_1\| \|q_2\|}\right)\right)
$$

其中，$\|q\|$ 表示四元数 $q$ 的范数（也称为模），可以表示为：

$$
\|q\| = \sqrt{w^2 + x^2 + y^2 + z^2}
$$

需要注意的是，在实际计算中，可能需要处理四元数的符号问题，以确保结果落在正确的区间内。