参考网址:

坐标变换

由一道周作业题想到的坐标系变换_哔哩哔哩_bilibili

不同坐标系变换

1
2
3
4
5
6
7
Pose2D TransformFrom(const Pose2D& pose_new) const{
    float dx = pose_new.x - x;
    float dy = pose_new.y - y;
    return Pose2d(dx*std::cos(theta)+dy*std::sin(theta),
                -dx*std::sin(theta)+dy*std::cos(theta),
                pose_new.theta - theta);
}
1
2
3
4
5
Pose2D TransformAdd(const Pose2D& pose_delta) const{
    float dx = pose_delta.x*std::cos(theta)-pose_delta.y*std::sin(theta);
    float dy = pose_delta.x*std::sin(theta)+pose_delta.y*std::cos(theta);
    return Pose2d(x+dx, y+dy, theta+pose_delta.theta);
}