齐次变换矩阵的旋转和平移顺序

先给出结论：齐次变换矩阵在进行坐标变换时，先进行旋转操作，再进行平移操作。

#include <eigen3/Eigen/Geometry>
#include <iostream>
int main()
{
    using Transform3d = Eigen::Transform<double, 3, Eigen::Isometry>;
    Eigen::Vector3d vec(1,1,1);
    Transform3d trans=Transform3d::Identity();
    trans.translation()=Eigen::Vector3d(1,2,3);
    Transform3d rot(Eigen::AngleAxisd(M_PI_2,Eigen::Vector3d(1,2,3)));
    Transform3d tf(rot);
    tf.translation()=Eigen::Vector3d(1,2,3);

    std::cout<<"---------------first rotate then translate-------------"<<std::endl;
    std::cout<< trans*rot*vec<<std::endl;

    std::cout<<"---------------first translate then rotate-------------"<<std::endl;
    std::cout<< rot*trans*vec<<std::endl;

    std::cout<<"---------------transform-------------"<<std::endl;
    std::cout<< tf*vec<<std::endl;
    return 0;
}

运行结果：

从数学计算角度解释：
令
$$trans=\left(\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 1& z\\ 0& 0& 0& 1\end{array}\right)$$

$$\left(\begin{array}{cccc}{r}_{11}& r_{11}& r_{13}& 0\\ r_{21}& r_{22}& r_{23}& 0\\ r_{31}& r_{32}& r_{33}& 0\\ 0& 0& 0& 1\end{array}\right)$$
先平移后旋转
$$rotate\ast trans=\left(\begin{array}{cccc}{r}_{11}& r_{11}& r_{13}& r_{11}x+r_{11}y+r_{13}z\\ r_{21}& r_{22}& r_{23}& r_{21}x+r_{22}y+r_{23}z\\ r_{31}& r_{32}& r_{33}& r_{31}x+r_{32}y+r_{33}z\\ 0& 0& 0& 1\end{array}\right)$$
先旋转后平移
$$trans\ast rotate=\left(\begin{array}{cccc}{r}_{11}& r_{11}& r_{13}& x\\ r_{21}& r_{22}& r_{23}& y\\ r_{31}& r_{32}& r_{33}& z\\ 0& 0& 0& 1\end{array}\right)$$

可以看出先旋转时，旋转操作并不会影响旋转前的平移坐标。

如果先平移再旋转的话，平移操作会改变旋转轴的坐标。