四元数与三维向量相乘运算

0. Eigen/四元数/欧拉角/旋转矩阵 相关系列文章

1. SLAM——之Eigen入门（矩阵运算及几何模块）
2. SLAM——之Eigen函数库，一个相对复杂的EIgen使用实例
3. SLAM——Eigen函数库:矩阵块运算，block操作
4. 欧拉角和旋转矩阵相互转换
5. 四元数与三维向量相乘运算
6. 四元数求导

1. 四元数和向量相乘可以表示这个向量按照这个四元数进行旋转之后得到的新的向量。

四元数乘以向量的运算规则为：q * v = （q） *（ v） *（ q−1）；
通常四元ss被记为(w,x,y,z)或(x,y,z,w)，以下q表示四元数，v表示向量，那么四元数和向量相乘的运算法则表示为：
q x v = (q) x (v) x (q-1)

2. 计算实例

例如：向量 c(0,0,10) 绕着Y轴旋转90°，得到新的向量是 c(10,0,0)。

例： q = (√2/2 , 0 ， √2/2 ， 0);
这里需要将三维向量v扩充为四元数(0,v)， 如c(0,0,10)变为c(0,0,0,10)；
q-1 是四元数q的逆，求逆过程如下：
共轭四元数：q*=(w,-x,-y,-z) 则为 （√2/2 , 0 ，-√2/2 ， 0）
四元数的模：N(q) = √(x2 + y2 + z2+ w2 )，通常四元数表示为单位四元数，所以模等于1
四元数的逆： q-1 = q*/N(q) 则为 （√2/2 , 0 ，-√2/2 ， 0）

按照上述计算公式： q * v = q * v * q−1

(√2/2 , 0 ， √2/2 ， 0) * （0 , 0，0 , 10） = （0,5√2，0，5√2）

（0,5√2，0，5√2） * （√2/2 , 0 ， -√2/2 ， 0）=（0,10,0,0）；

将最后得到的四元数（0,10,0,0）
四元数的乘法公式：

按照上述公式可得到新四元数（0,10,0,0），则旋转后新坐标为(10,0,0)
按上述方法计算所得到的新四元数的首项一定等于0。

3. c++ 语言实现：

输入输出说明：q_in为输入四元数对应的vec,依次对伊你该四元数坐标(w,x,y,z)，v_in为对应的三维向量，v_out为v_in对应的载当前四元数旋转下输出的三维向量

//The Sandwich Product
void MathTest_qvq(float q_in[4], float v_in[3], float v_out[3])
{
#if 0//q-1 v q
    float q_inverse[4];
    float q_normal[4];
    MathTest_Q_normal(q_in, q_normal);
    MathTest_q_inverse(q_normal, q_inverse);

    float q_formV[4];
    q_formV[0] = 0;
    q_formV[1] = v_in[0];
    q_formV[2] = v_in[1];
    q_formV[3] = v_in[2];

    //q-1 v q 
    float tmp_q[4];
    float q_for_v_out[4];
    MathTest_q1_x_q2(q_inverse, q_formV, tmp_q);
    MathTest_q1_x_q2(tmp_q, q_normal, q_for_v_out);
    v_out[0] = q_for_v_out[1];
    v_out[1] = q_for_v_out[2];
    v_out[2] = q_for_v_out[3];
#else  //q v q-1
    float q_inverse[4];
    float q_normal[4];
    MathTest_Q_normal(q_in, q_normal);
    MathTest_q_inverse(q_normal, q_inverse);

    float q_formV[4];
    q_formV[0] = 0;
    q_formV[1] = v_in[0];
    q_formV[2] = v_in[1];
    q_formV[3] = v_in[2];

    //q v q-1 
    float tmp_q[4];
    float q_for_v_out[4];
    MathTest_q1_x_q2(q_normal, q_formV, tmp_q);
    MathTest_q1_x_q2(tmp_q, q_inverse, q_for_v_out);
    v_out[0] = q_for_v_out[1];
    v_out[1] = q_for_v_out[2];
    v_out[2] = q_for_v_out[3];
#endif
    if (0)//out log
    {
        for (int i = 0; i < 4; i++)
        {
            std::cout << q_for_v_out[i] << "   ";
        }
        std::cout << std::endl;
    }

}

4. 四元数插值与均值（姿态平滑）