오원석
오원석
It seems that reset function has been called at that time due to failure detection.
@SSZ1 I think the reason for the two paths are different is varying yaw offset and anc_ecef value.
How frequently your imu and lidar data published?
I think two codes are conducting the same thing if origin_P0 has same vector3d value with para_Pose[0][0~2]