坐标旋转

1、x,y,yaw为笛卡尔坐标系

#include <vector>
#include <iostream>
#include <cmath>

typedef struct Point
{
    double x;
    double y;
    double z;
    double yaw;
    double speed;
    double curvature;
} Point;

// transform point into the coordinate of current_pose
inline Point calcRelativeCoordinate(Point point, Point current_pose)
{
    double dx = point.x - current_pose.x;
    double dy = point.y - current_pose.y;

    Point tf_point;
    tf_point.x = dx * std::cos(current_pose.yaw) + dy * std::sin(current_pose.yaw);
    tf_point.y = dx * (-std::sin(current_pose.yaw)) + dy * std::cos(current_pose.yaw);
    tf_point.yaw = point.yaw - current_pose.yaw;
    return tf_point;
}

// calculation absolute coordinate of point on current_pose frame
inline Point calcAbsoluteCoordinate(Point tf_point, Point current_pose)
{
    Point point;
    point.x = current_pose.x + tf_point.x * std::cos(current_pose.yaw) + tf_point.y * (-std::sin(current_pose.yaw));;
    point.y = current_pose.y + tf_point.x * std::sin(current_pose.yaw) + tf_point.y * std::cos(current_pose.yaw);
    point.yaw = current_pose.yaw + tf_point.yaw;
    return point;
}