设有小萝卜一号和小萝卜二号位于世界坐标系中。
小萝卜一号的位姿为: q 1 = [0.55, 0.3, 0.2, 0.2], t 1 =[0.7, 1.1, 0.2] T
小萝卜二号的位姿为 q 2 = [−0.1, 0.3, −0.7, 0.2], t 2 = [−0.1, 0.4, 0.8] T 。
小萝卜一号坐标为 p 1 = [0.5, −0.1, 0.2] T ,
求该向量在小萝卜二号坐标系下的坐标。

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