踩坑日记

记Eigen::Isometry3d的奇怪用法

对于两个坐标系之间的关系，我们有相应的变换矩阵去描述:

1. 2D-2D: 可以用(3 imes3)的变换矩阵去计算坐标和向量在不同坐标系下的表示，变换矩阵的自由度是3
2. 3D-3D: 用(4 imes 4)的变换矩阵计算点和向量在不同坐标系下的表示，自由度是6

而变换矩阵同时也可以表示位姿，变换矩阵的乘法也对应着相对位姿的累计。

我们将坐标系(C_1)到坐标系(C_2)的变换记做(T_2^1),满足:

[p_1 = T_2^1 p_2 ]

其中(p_1)是某个向量或者点在坐标系(C_1)下的坐标表示，(p_2)是某个向量或者点在坐标系(C_2)下的坐标表示; 递推下去:

[p_1 = T_2^1T_3^2...T_n^{n-1}p_n = T_n^1p_n ]

这样就描述了变换矩阵的乘法(算是左乘，变换矩阵在左边)和坐标变换的关系.

对于2D-2D的形式，下图所示的变换:

对应的变换矩阵:

[egin{pmatrix} cos( heta) & -sin( heta) & t_x \ sin( heta)& cos( heta)& t_y \ 0 & 0 & 1 end{pmatrix} ]

在Eigen::Isometry3d是表示3D-3D的变换方式，下面的代码是正确的表示方式:

int main(){
    Vector3d point(0,0,0);
    Isometry3d t = Isometry3d::Identity();
    t.prerotate(AngleAxisd(M_PI/2,Vector3d(0,0,1)));



    t.pretranslate(Vector3d(2,1,0));
    Vector3d p2 = t*point;

    cout<<p2<<endl;

}

但如果将prerotate和pretranslate交换顺序就会得到奇怪的表示，平移向量变成了旋转之后表示。而对于理解平移旋转的顺序来说，先平移后旋转对应的平移量才是原始坐标系的；而先旋转的后平移对应的平移量是旧坐标系下在新坐标系下的投影。这与Isometry3d的方式恰恰是相反的。

就我的常用方式来说，先平移后旋转是合理的，而对应的正确的代码则是先prerotate后pretranslate.