已知世界坐标系下机器人位姿，求机器人坐标系下世界坐标系，用c++实现

假设机器人在世界坐标系下的位姿为 $T_{world\_robot}$，其中 $T_{world\_robot}$ 是一个 $4\times4$ 的齐次变换矩阵，其中前三行前三列为旋转矩阵 $R_{world\_robot}$，最后一列为平移向量 $t_{world\_robot}$。我们可以通过以下方式求出机器人坐标系下的世界坐标系：

1. 机器人坐标系下的原点在世界坐标系下的坐标为 $t_{world\_robot}$，因此机器人坐标系下的 $x$ 轴、$y$ 轴、$z$ 轴在世界坐标系下的坐标分别为 $T_{world\_robot}$ 的第一列、第二列、第三列。

2. 机器人坐标系下的 $x$ 轴、$y$ 轴、$z$ 轴在世界坐标系下的坐标形成的旋转矩阵为 $R_{robot\_world}$，其中 $R_{robot\_world}=R_{world\_robot}^{-1}$，即 $R_{robot\_world}$ 是 $R_{world\_robot}$ 的逆矩阵。

3. 机器人坐标系下的世界坐标系在世界坐标系下的坐标为机器人坐标系下的原点在世界坐标系下的坐标，即 $t_{world\_robot}$。

以下是实现代码：

#include <Eigen/Core>
#include <Eigen/Geometry>

Eigen::Isometry3d worldToRobot(const Eigen::Isometry3d& T_world_robot) {
    Eigen::Isometry3d T_robot_world = T_world_robot.inverse();
    return T_robot_world;
}

其中 `Eigen::Isometry3d` 是 Eigen 库中的齐次变换矩阵类型。