設有小蘿蔔一號和小蘿蔔二號位於世界座標系中。
小蘿蔔一號的位姿爲: 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>