经典坐标变换案例代码剖析

题目：

设有小萝卜一号和小萝卜二号位于世界坐标系中。记世界坐标系为W，小萝卜们的坐标系为R1和

R2.小萝卜一号的位姿为q2=[0.35,0.2,0.3,0.1]T,t1=[0.3,0.1,0.1]T,小萝卜二号的位姿为q2=[-0.5,0.4,-0.1,0.2]T,t2=[0.3,0.1,0.1]T ，这里的q和t表示的都是 T_Rk,w,k=1,2,也就是世界坐标系到相机坐标系的变换关系，现在小萝卜一号看到某个点在自身的坐标系下坐标为P_R1=[0.5,0,0.2]T,求该向量在小萝卜二号坐标系下的坐标。

解题思路：

原理：若存在世界坐标系T_w，机器人坐标系T_R。一个点的世界坐标P_w，机器人坐标系下P_R，则。

知道了原理，问题就很容易解决了，首先我们需要将已知的坐标转换到世界坐标系下，再将其转换到小萝卜二号坐标系下。其关键点就是求出T_w1（小萝卜一号坐标系到世界坐标系）和T_2w（世界坐标系到二号坐标系）的变换矩阵即可。根据题目我们已经知道了T_1w和T_2w，而T_1w求逆就能得到T_w1。

根据上面的原理得到最后计算的公式，

。

计算代码及注释：

#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),t2(-0.1,0.5,0.3);
    Vector3d p1(0.5,0,0.2);
    //将四元数代表的旋转赋值给变换矩阵
    Isometry3d T1w(q1),T2w(q2);
    //把平移向量赋值给变换矩阵
    //先旋转再平移（但平移是按照旋转之前的坐标轴平移）
    T1w.pretranslate(t1);//得到小萝卜一号到世界坐标系的变换矩阵
    T2w.pretranslate(t2);//得到小萝卜二号到世界坐标系的变换矩阵

    Vector3d p2=T2w*T1w.inverse()*p1;
    cout<<endl<<p2.transpose()<<endl;
    return 0;
}