如何求 Eigen::Vector3d 表示的 2d 位姿的相对关系
时间: 2024-02-09 12:10:48 浏览: 35
Eigen::Vector3d 表示的是三维空间中的一个点的坐标,如果要将其表示为二维坐标系下的位姿,需要指定一个平面,例如 $z=0$ 的平面,然后将其投影到这个平面上。
假设有两个二维位姿 $p_1$ 和 $p_2$,其中 $p_1$ 表示为 $(x_1,y_1,\theta_1)$,$p_2$ 表示为 $(x_2,y_2,\theta_2)$,其中 $x$ 和 $y$ 表示位置坐标,$\theta$ 表示姿态角度。如果要求它们之间的相对关系,可以按以下步骤进行:
1. 计算 $p_1$ 和 $p_2$ 的位置向量 $t_1 = (x_1, y_1)$ 和 $t_2 = (x_2, y_2)$。
2. 计算 $t_2$ 相对于 $t_1$ 的平移向量 $v = t_2 - t_1$。
3. 计算 $p_2$ 相对于 $p_1$ 的旋转角度 $\theta = \theta_2 - \theta_1$。
4. 构造出相对位姿的变换矩阵,可以使用 Eigen 库的 Isometry2d 类来表示变换矩阵。
代码示例:
```c++
#include <Eigen/Core>
#include <Eigen/Geometry>
Eigen::Vector3d p1, p2; // 两个二维位姿
Eigen::Vector2d t1 = p1.segment(0, 2); // 位置向量 t1
Eigen::Vector2d t2 = p2.segment(0, 2); // 位置向量 t2
// 计算 t2 相对于 t1 的平移向量
Eigen::Vector2d v = t2 - t1;
// 计算 p2 相对于 p1 的旋转角度
double theta = p2[2] - p1[2];
// 构造出相对位姿的变换矩阵
Eigen::Isometry2d relative_pose = Eigen::Isometry2d::Identity();
relative_pose.rotate(Eigen::Rotation2Dd(theta));
relative_pose.translation() = t1;
relative_pose.translate(v);
```
这样就可以得到从 $p_1$ 到 $p_2$ 的相对位姿了。