题目回顾:
设有小萝卜一号和小萝卜二号位于世界坐标系中,小萝卜一号的位姿为:q1=[0.35,0.2,0.3,0.1],
t2=[0.3,0.1,0.1]^T (q的第一项为实部。请你把q归一化后在进行计算)。这里的q和t的表达的是Tcw,也就是世界到相机的变换关系。小萝卜二号的位姿为q2=[-0.5,0.4,-0.1,0.2],t=[-0.1,0.5,0.3]^T.现在,小萝卜一号看到某个点在自身的坐标系下,坐标为p=[0.5,0,0.2]^T ,求该向量在小萝卜二号坐标系下的坐标,请编程实现此事。解:pw:某个点在世界坐标系下的坐标T_1w :表示世界坐标系到小萝卜一号坐标系的变换关系T_2w:表示世界坐标系到小萝卜二号坐标系的变换关系P2 :表示该点在小萝卜二号坐标系下的坐标(即为所求)单位四元数到旋转矩阵R的变化关系可参考书上55页。之后变换矩阵T=[R t] [0 1]由变换关系可列出下面的式子: p = T_1w * Pw 可解出来pwp2=T_2W*pW 带入上式解出来的Pw即可求出来p2具体代码实现如下:
1 #include2 #include 3 4 //包含几何模块 5 #include 6 using namespace std; 7 8 int main(int argc,char **argv) 9 {10 /*变量定义*/11 Eigen::Quaterniond Q1(0.2,0.3,0.1,0.35); //四元数的表示(w ,x,y,z)12 Eigen::Quaterniond Q2(0.4,-0.1,0.2,-0.5);13 Eigen::Vector3d t1(0.3,0.1,0.1);14 Eigen::Vector3d t2(-0.1,0.5,0.3);15 Eigen::Vector3d p(0.5,0,0.2); //在一号小萝卜下的坐标16 Eigen::Vector3d pw ; //世界坐标17 Eigen::Vector3d p2; //求在二号小萝卜的坐标 p218 19 /*欧氏变换矩阵使用Eigen::Isometry */20 Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();21 Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();22 23 /*归一化*/24 Q1.normalize();25 Q2.normalize();26 27 /*输出归一化参数*/28 // cout<<"Q1 is "< < < << endl < << endl< <