高翔--视觉SLAM 14讲 --3.6.2实际坐标变换的例子

四元素 q1 q2
平移向量 t1 t2

相机坐标系下观测的点 PR1

求：相机坐标系下的观测点PR2

由刚体运动 坐标系变换之间相差一个 欧式变换矩阵可以知道：

PR1 = T1 * Pw T1 是世界坐标系到相机坐标系的变换矩阵， Pw 是 世界坐标系基础系
PR2 =T2 * Pw

联系两个等式 PR2 = T2 * T1.inverse() * PR1

详细步骤如代码所示：

在这里插入代码片
```#include <iostream>

#include<vector>
#include<algorithm>
#include<Eigen/Core>
#include<Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
    q1.normalize(); // 归一化
    
    q2.normalize();
 
    
    Vector3d t1(0.3, 0.1, 0.1);
    Vector3d t2(-0.1, 0.5, 0.3);

    Isometry3d T1w(q1), T2w(q2);  // 构建  4* 4 的欧式变换矩阵

    T1w.pretranslate(t1); // 添加平移变量

    T2w.pretranslate(t2);

    

    Vector3d p1(0.5, 0, 0.2);
    Vector3d p2 = T2w * T1w.inverse() * p1; // 

    cout << endl << p2 << endl;
    cout << endl << p2.transpose() << endl;

    
    return 0;
}