用带有测试例子的c++实现

以下是带有测试例子的完整的 C++ 实现代码：

#include <iostream>
#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;
}

int main() {
    // 机器人在世界坐标系下的位姿
    Eigen::Isometry3d T_world_robot = Eigen::Isometry3d::Identity();
    T_world_robot.translate(Eigen::Vector3d(1.0, 2.0, 3.0));
    T_world_robot.rotate(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitX()));
    T_world_robot.rotate(Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
    T_world_robot.rotate(Eigen::AngleAxisd(0.7, Eigen::Vector3d::UnitZ()));

    // 机器人坐标系下的世界坐标系
    Eigen::Isometry3d T_robot_world = worldToRobot(T_world_robot);

    // 输出结果
    std::cout << "机器人在世界坐标系下的位姿：" << std::endl << T_world_robot.matrix() << std::endl;
    std::cout << "机器人坐标系下的世界坐标系：" << std::endl << T_robot_world.matrix() << std::endl;

    return 0;
}

在这个例子中，我们先定义了一个机器人在世界坐标系下的位姿 $T_{world\_robot}$，然后调用 `worldToRobot` 函数求出机器人坐标系下的世界坐标系 $T_{robot\_world}$，最后输出结果。