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 coordfloat 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
Point odom_coord; is the odom coord
Point ego_coord; is the ego coordfloat ego_coord.x = trans.x + odom_coord.x * std::cos(trans.angle) - odom_coord.y * std::sin(trans.angle);
float ego_coord.y