Eigen四元数的坐标变换和坐标系变换

参考：
https://www.cnblogs.com/jerry323/p/9097264.html

一共有两个机器人，一号和二号。

一号位姿 q1 = [0.35, 0.2, 0.3, 0.1]，t1=[0.3, 0.1, 0.1]。
二号位姿 q2=[-0.5, 0.4, -0.1, 0.2], t2=[-0.1, 0.5, 0.3]。

某点在一号坐标系下坐标为p=[0.5, 0, 0.2]。求p在二号坐标系下的坐标。

假设在世界坐标系中p点的坐标为P。

用四元数做旋转则有（在Eigen中四元数旋转为q×v（左乘），数学中则为q×v×q^-1），进行世界坐标到机器人坐标的坐标变换：

• q1 × P + t1 = p1
• q2 × P + t2 = p2

两式联立消除P，得到：
p2 = q2 × q1^-1 × (p1 - t1) + t2

#include <iostream>

using namespace std;

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

int main()
{
    //四元数
    Eigen::Quaterniond q1 = Eigen::Quaterniond(0.35, 0.2, 0.3, 0.1).normalized();
    Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.5, 0.4, -0.1, 0.2).normalized();
    //平移向量
    Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);
    Eigen::Vector3d t2 = Eigen::Vector3d(-0.1, 0.5, 0.3);
    //目标向量
    Eigen::Vector3d p1 = Eigen::Vector3d(0.5, 0, 0.2);
    Eigen::Vector3d p2;

    //打印输出
    // cout << q1.coeffs() << "\n"
    //      << q2.coeffs() << "\n"
    //      << t1.transpose() << "\n"
    //      << t2.transpose() << endl;

    //四元数求解
    p2 = q2 * q1.inverse() * (p1 - t1) + t2;
    cout << p2.transpose() << endl;
}

记住！！！

坐标系变换是右乘！！！！！

坐标变换是左乘！！！！！

四元数既可以表示点变换，也可以表示系变换，关键看你选择怎么用。

总结

通过代码可以发现，，若A在B中的位姿是（ax, ay, az, aw）那么，首先w项优先

Eigen::Quaterniond(aw, ax, ay, az).normalized();

其次，获得B到A的坐标变换。