Could you just change your odometry system or add a layer, such that the initial angle is as close as possible to the previous one?
Otherwise, you could change your solver initialization by shifting all angles by \pm\pi if this happens.
Could you just change your odometry system or add a layer, such that the initial angle is as close as possible to the previous one?
Otherwise, you could change your solver initialization by shifting all angles by \pm\pi if this happens.