《视觉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在二号坐标系下的坐标.
分析题意可知：q的第一项是实部，且还未归一化。 q和t表达的是Tcw也就是世界坐标系到相机坐标系的变换关系
假设在世界坐标系中p点的坐标为P。

[q1 imes P + t1 = p1\ q2 imes P + t2 = p2 ]

由上两式分别解算出：

[P = q1^{-1} imes (p1 - t1)\ P = q2^{-1} imes (p2 - t2) ]

两式联立求解则得到：

[p2 = q2 imes q1^{-1} imes (p1 -t1) + t2 ]

如果用欧拉矩阵（设一号欧拉矩阵为T1，二号欧拉矩阵为T2）则有：

[p1 = T1 imes P\ p2 = T2 imes P ]

求解P：

[P = T1^{-1} imes p1\ P = T2^{-1} imes p2 ]

联立求解则有：

[p2 = T2 imes T1^{-1} imes 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;

    //四元数求解
    p2 = q2 * q1.inverse() * (p1 - t1) + t2;
    cout << "Quaterniond" << std::endl<<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);

    //欧拉矩阵求解
    p2 = T2 * T1.inverse() * p1;
    cout <<"Euler Matrix"<< std::endl<< p2.transpose() << endl;

    //旋转矩阵
    Eigen::Matrix3d R1 = Eigen::Matrix3d(q1);
    Eigen::Matrix3d R2 = Eigen::Matrix3d(q2);
    Eigen::Vector3d P_w = R1.inverse() * (p1 - t1);
    Eigen::Vector3d P_2 = R2 * P_w + t2;
    cout << "Rotation Matrix" << std::endl<< P_2.transpose() << endl;
}