Skip to content

[fix] Complete different cases in GetTworld

Tong Fu requested to merge fix/FixGetTworld into master
  • If the LogStates is empty, the function returns TworldInit.
  • If time < 0, the function returns the pose of the last state in the LogStates. (default value of the function)
  • If the time is before the first state of the LogStates, the function returns the pose of the first state and a warning.

After these checks, it remains the cases that time is after the first state:

  • If IMU has data, the function returns imu pose at the input time.
  • Otherwise, search the two iterators which point states before and after the input time:
    • If external poses are available, deduce the pose at the input time with external pose measurement.
    • If the pose can not be deduced by external pose measurements, check firstly whether the input time has been reached or not. If not, return last pose in the LogStates.
    • Otherwise, do linear interpolation between two states around the input time.
Edited by Tong Fu

Merge request reports