C++ 改造后的新机器人的航向

假设我有这样的2d姿势（角度以度为单位）：

世界机器人{0,4,45} robot_to_object={4，-4，-45}

从图表中很容易看出：

world_to_object={4,8，θ}，θ的值是多少？atan2（8，4）？如果对象有一个世界标题怎么办

最终，这将需要以代码实现并可用于任何姿势列表的格式完成。所以我将有一个C++实现，

Eigen::Matrix<float, 3, 1> world_to_robot;
Eigen::Matrix<float, 3, 1> world_to_object;
Eigen::Matrix<float, 3, 1> robot_to_object;

// My omniscient poses below from my fake scenario
world_to_robot << 0, 4, M_PI/2;
robot_to_object << 4, -4, -M_PI/4;
wold_to_object << 4, 8, some_theta;

Eigen::Matrix<float, 3, 3> rotationM;
rotationM << cos(M_PI/2), -sin(M_PI/2), 0,
             sin(M_PI/2,   cos(M_PI/2), 4,
                  0,             0,     1;

Eigen::Matrix<float, 3, 1> robot_to_objectM;
robot_to_objectM << 4, -4, 1;

Eigen::Matrix<float, 3, 1> calculated_world_to_object = rotationM*robot_to_objectM;
  

Eigen:：矩阵世界机器人；
特征：：矩阵世界到对象；
矩阵机器人对物体；
//下面是我的假场景中的全知姿势
如果
theta
以度为单位，则
cos（theta）
是一个问题。要将度转换为弧度，请按2*pi/360.0缩放。只有我们人类才能真正用学位做任何事情。在解释中，我在姿势中加入了度数，但在代码中我使用了弧度（参见M_PI/2和M_PI/4）。但为了澄清是的，θ应该是弧度。我不知道θ是什么，这是我的主要问题。现在它是可变的占位符。如果
theta
以度为单位，那么
cos（theta）
就是一个问题。要将度转换为弧度，请按2*pi/360.0缩放。只有我们人类才能真正用学位做任何事情。在解释中，我在姿势中加入了度数，但在代码中我使用了弧度（参见M_PI/2和M_PI/4）。但为了澄清是的，θ应该是弧度。我不知道θ是什么，这是我的主要问题。目前它是可变的占位符。
Eigen::Matrix<float, 3, 3> rotationM;
rotationM << cos(theta), -sin(theta), 4,
             sin(M_PI/2,  cos(theta), 8,
                  0,           0,     1;    // What is theta? Just the world_to_object theta?

Eigen::Matrix<float, 3, 1> robot_to_objectM;
robot_to_objectM << -4, 4, 1;               // Note: I inverted the signs for the translation

Eigen::Matrix<float, 3, 1> calculated_world_to_robot = rotationM*robot_to_objectM;