地图坐标系修改

[hujiax380]

请问如果我想将地图坐标系设置为初始时刻的机体坐标系而不是imu坐标系，需要做哪些修改呢，需要将imu数据转换到body系吗

[xuankuzcr]

可以在gravityAlignment()里这样改，T_Body_IMU是IMU系相对于Body系的外参。

_state.pos_end = T_Body_IMU * _state.pos_end;
_state.rot_end = T_Body_IMU * _state.rot_end;
_state.vel_end = T_Body_IMU * _state.vel_end;
_state.gravity = T_Body_IMU * _state.gravity;

[hujiax380]

可以在gravityAlignment()里这样改，T_Body_IMU是IMU系相对于Body系的外参。

_state.pos_end = T_Body_IMU * _state.pos_end; _state.rot_end = T_Body_IMU * _state.rot_end; _state.vel_end = T_Body_IMU * _state.vel_end; _state.gravity = T_Body_IMU * _state.gravity;

感谢郑博！改成这样之后发现定位输出的坐标轴和body系对应了，但是初始的角度不是从0001开始的，另外视觉模块看起来匹配的点数变得很少，如果想让初始姿态从单位阵开始，还需要改哪里吗