Eigen二维位姿运算

int main() {
    Eigen::Vector2d p = {1, 1};
    // 定义一个变换矩阵
    auto t = Eigen::Transform<double, 2, Eigen::Isometry>::Identity();
    std::cout << "单位变换矩阵: " << std::endl << t.matrix() << std::endl;

    // 定义一个旋转矩阵
    double theta = EIGEN_PI / 2;
    Eigen::Matrix2d rMat;
    rMat << cos(theta), -sin(theta),
            sin(theta), cos(theta);
    std::cout << "旋转90度矩阵: " << std::endl << rMat << std::endl;
    // 直接矩阵变换
    std::cout << "逆时针旋转: " << std::endl << (rMat * p).transpose() << std::endl;
    std::cout << "顺时针旋转: " << std::endl << (rMat.inverse() * p).transpose() << std::endl;
    std::cout << "顺时针旋转: " << std::endl << (p.transpose() * rMat) << std::endl;

    // 将旋转矩阵应用到变换矩阵上
    Eigen::Rotation2Dd r(theta);
    std::cout << "旋转90度矩阵: " << std::endl << r.matrix() << std::endl;
    t.rotate(r); // 等价于t * r
//    t.prerotate(r); // 等价于r * t
    std::cout << "变换矩阵: " << std::endl << t.matrix() << std::endl;
    std::cout << "变换矩阵应用到p上(逆时针): " << std::endl << (t * p).transpose() << std::endl;
    std::cout << "变换矩阵应用到p上(顺时针): " << std::endl << (t.inverse() * p).transpose() << std::endl;

    // 定义平移向量
    Eigen::Vector2d translate = {0, 1};
    // 将平移向量应用到变换矩阵上，注意此时变换矩阵的物理含义是先平移，再旋转
    t.translate(translate);
    std::cout << "变换矩阵: " << std::endl << t.matrix() << std::endl;
    std::cout << "变换矩阵应用到p上(逆时针): " << std::endl << (t * p).transpose() << std::endl;
    std::cout << "变换矩阵应用到p上(顺时针，平移向量也逆了): " << std::endl << (t.inverse() * p).transpose() << std::endl;
}

单位变换矩阵:
1 0 0
0 1 0
0 0 1
旋转90度矩阵:
6.12323e-17          -1
          1 6.12323e-17
逆时针旋转:
-1  1
顺时针旋转:
 1 -1
顺时针旋转:
 1 -1
旋转90度矩阵:
6.12323e-17          -1
          1 6.12323e-17
变换矩阵:
6.12323e-17          -1           0
          1 6.12323e-17           0
          0           0           1
变换矩阵应用到p上(逆时针):
-1  1
变换矩阵应用到p上(顺时针):
 1 -1
变换矩阵:
6.12323e-17          -1          -1
          1 6.12323e-17 6.12323e-17
          0           0           1
变换矩阵应用到p上(逆时针):
-2  1
变换矩阵应用到p上(顺时针):
 1 -2