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