Eigen實現坐標轉換

（《視覺SLAM十四講》第三講習題7）設有小蘿卜一號和二號在世界坐標系中。一號位姿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 = q1^-1 × (p1 - t1)
• P = q2^-1 × (p2 - t2)

兩式聯立求解則得到：

p2 = q2 × q1^-1 × (p1 - t1) + t2

如果用歐拉矩陣（設一號歐拉矩陣為T1，二號歐拉矩陣為T2）則有：

• p1 = T1 × P
• p2 = T2 × P

求解P：

• P = T1^-1 × p1
• P = T2^-1 × p2

聯立求解則有：

p2 = T2 × T1^-1 × p1

以下則是用Eigen實現的代碼：

#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;

    //歐拉矩陣
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T1.rotate(q1.toRotationMatrix());
    T1.pretranslate(t1);
    T2.rotate(q2.toRotationMatrix());
    T2.pretranslate(t2);
    // cout << T1.matrix() << endl;
    // cout << T2.matrix() << endl;

    //歐拉矩陣求解
    p2 = T2 * T1.inverse() * p1;
    cout << p2.transpose() << endl;
}