几何运算练习（使用 Eigen/Geometry 计算⼀个具体的例子，欧拉角，旋转向量）

设有⼩萝⼘ 1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为： q1 = [0:55; 0:3; 0:2; 0:2]; t1 =[0:7; 1:1; 0:2]T（q 的第⼀项为实部）。这⾥的 q 和 t 表达的是 Tcw，也就是世界到相机的变换关系。⼩萝⼘⼆号的位姿为 q2 = [-0:1; 0:3; -0:7; 0:2]; t2 = [-0:1; 0:4; 0:8]T。现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐标系下，坐标为 p1 = [0:5; -0:1; 0:2]T，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交你的程序。

1.题目基础知识

题中的q1和q2是四元数，一共四个项，第一项为实部，后三项为虚部，具体结构如下（懒得敲公式了，直接粘图）：

其中角θ为旋转的角度大小，nx，ny，nz为旋转轴的xyz坐标。

q表示了旋转，t表示平移，组合为小萝卜的位姿

四元数使用方法：

其中q为四元数，

p为原始坐标为