用C++,eigen,实现上一时刻在map下的位姿为(x,y,yaw),经过(delta x ,delta y,delta yaw)的运动,和误差补偿(x1,y1,yaw1)得到的位姿
时间: 2023-09-18 20:12:17 浏览: 71
基于C++实现的人体姿势检测
好的,这是一个比较具体的问题。根据您的需求,我可以给您提供以下的实现思路:
1. 定义位姿变量
首先,我们需要定义一个变量来存储上一个时刻的位姿。可以使用一个包含三个元素的向量来表示(x,y,yaw)。
```cpp
Eigen::Vector3d pose_last;
```
2. 进行运动模型预测
根据您的需求,我们需要进行运动模型预测,来得到经过(delta x, delta y, delta yaw)运动后的位姿。可以使用欧拉角旋转矩阵来实现,具体实现如下:
```cpp
Eigen::Vector3d delta(delta_x, delta_y, delta_yaw);
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(pose_last(2), Eigen::Vector3d::UnitZ()); // 以yaw轴旋转
Eigen::Vector3d delta_global = R * delta;
Eigen::Vector3d pose_pred = pose_last + delta_global;
```
3. 进行误差补偿
接下来,我们需要进行误差补偿,根据您的需求,我们需要使用(x1, y1, yaw1)进行补偿。具体实现如下:
```cpp
Eigen::Vector3d pose_correct(x1, y1, yaw1);
Eigen::Matrix3d R_correct;
R_correct = Eigen::AngleAxisd(- pose_pred(2), Eigen::Vector3d::UnitZ()); // 以yaw轴旋转
Eigen::Vector3d pose_correct_global = R_correct * pose_correct;
Eigen::Vector3d pose_final = pose_pred + pose_correct_global;
```
4. 更新上一时刻的位姿
最后,我们需要更新上一时刻的位姿为当前位姿,以备下一次计算使用。
```cpp
pose_last = pose_final;
```
综上所述,以上就是一个简单的使用 C++ 和 Eigen 库实现上一时刻位姿预测和误差补偿的实现思路。
阅读全文