【自动驾驶】点与向量从ego系转odometry系

1.点从ego系转odometry系（ego -> odometry）

struct Point
{
    float x;
    float y;
    float angle;
};
Point trans; // is the odom to ego transform
Point odom_coord; is the odom coord
Point ego_coord; is the ego coord

float odom_coord.x = (ego_coord.x - trans.x) * std::cos(trans.angle) + (ego_coord.y - trans.y) * std::sin(angle);
float odom_coord.y = -(ego_coord.x - trans.x) * std::sin(trans.angle) + (ego_coord.y - trans.y) * std::cos(angle);
float odom_coord.angle = ego_coord.angle + trans.angle;

2. 点从odometry系转ego系（odometry -> ego）

struct Point
{
    float x;
    float y;
    float angle;
};
Point trans; // is the odom to ego transform
Poi