1

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

world_to_robot = {0, 4, 45} robot_to_object = {4, -4, -45}

从图中不难看出：

world_to_object = {4, 8, theta}，theta 有什么用？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;

这将为我在全局框架中为对象提供正确的新 x 和 y 姿势，但是 theta 是什么？

然后假设我有world_to_object和robot_to_object的场景。还假设我知道 world_to_object 的 theta，因为我们根据签名识别了对象并且存在绝对姿势。我想计算 world_to_robot。让我们使用与上面相同的姿势。

我知道要获得 world_to_robot，我需要进行由 world_to_object * object_to_robot 组成的转换。我没有 object_to_robot。我需要robot_to_object的倒数，它不是正方形的。这是我在这种情况下所做的：

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;

如果我使用 world_to_robot 朝向 theta，这似乎给出了正确的新 x 和 y 姿势。但是，可能存在提前不知道的情况。那么我将如何正确获得我的 theta 值？另外，我知道新的机器人姿势需要是 M_PI/2，但是我如何从我的最后一段代码段中得到这个。通常我会开始做一堆“atan2(y,x)”，但在上面的代码实现中缺少关键的“theta”。

4

0 回答 0