视觉SLAM十四讲 3.6.2实际的坐标转换的例子

忠厚老实的陈同学 / 2023-09-05 / 原文

在这一讲中，作者没有给出详细的过程，作为初学者还是有点懵。

这道题是有两种解法的

四元数解法
欧拉矩阵解法

四元数解法

假设一个目标点 $p$在世界坐标系下的坐标是

\[P_W \]

利用四元数表示旋转，两个小萝卜坐标系下 $p$ 点坐标有如下公式：

\[p_1 = q_1 * P_w + t_1 \\ p_2 = q_2 * P_w + t_2 \]

以上公式实际上是Eigen的计算公式，经过了运算符重载，实际上，数学表达上与Eigen运算有所不同。
在Eigen中四元数表示旋转是

\[q*P \]

数学表达上应该是

\[q*P*q^{-1} \]

因为Eigen使用了运算符重载，减少了编写代码的麻烦。
由以上的坐标变换关系，可以推得：

\[p_2=q_2 ∗q_{1}^{−1} ∗(p_1 −t_1 )+t_2 \]

变换矩阵解法

若使用欧拉矩阵，我们设两个坐标的变换矩阵为T1和T2，有：

\[p_1 = T_1*P_w\\ p_2 = T_2*P_w \]

联立可以得到如下计算公式：

\[p_2 = T_2*T_1^{-1} *p_1 \]

code

void main()
{
    //定义两个小萝卜自身姿态的两个四元数
    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);

    //对两个四元数进行归一化
    q1.normalize();
    q2.normalize();

    //定义两个小萝卜的位置的两个三维坐标
    Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
    Eigen::Vector3d p1(0.5, 0, 0.2);//用来表示小萝卜一号坐标系下该点坐标（题干给出）
    Eigen::Vector3d p2; //用来表示小萝卜二号坐标系下该点坐标

    // 下面分别用四元数以及变换矩阵得到小萝卜二号的该点观测信息
    // 以下用四元数求解p2坐标
    p2 = q2 * q1.inverse() * (p1 - t1) + t2;//求解p2坐标
    //这里的inverse是相反的意思，也就是求q1的逆矩阵
    //公式为：p2 = q2  * q1^-1 * (p1 - t1) + t2
    cout << "四元数求得的p2坐标" << endl;
    cout << p2.transpose() << endl;//行向量显示，transpose是转置矩阵的意思

    //以下用欧拉矩阵求解p2坐标
    Eigen::Isometry3d T1w(q1), T2w(q2);//欧式变换矩阵Isometry（虽然称为3d，实质上是4＊4的矩阵）
    T1w.pretranslate(t1);//设置平移向量，我的理解是加入这个平移向量
    T2w.pretranslate(t2);

    Eigen::Vector3d p3;//用来表示小萝卜二号坐标系下该点坐标（变换矩阵方法）
    p3 = T2w * T1w.inverse() * p1;//求解p3坐标
    cout << "变换矩阵/欧拉矩阵求得的p2坐标" << endl;
    cout << p3.transpose() << endl;
}

以上两个输出结果是一致的。