[fix] Complete different cases in GetTworld

• 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.