[ROS][fix] Fix pose comparison with reference for slam test

Goal

Previously, NbFramesDropped increases when a frame of current slam result is not presented in reference frame. However, we'd like to detect dropped frame in current slam result w.r.t reference result.

Changes

• When comparing reference result with current slam result:
  • if a frame is not in reference result, a warning information is displayed
  • if a frame is not in current slam result w.r.t reference, consider it as a dropped frame, increase counter NbFramesDropped
• The comparison of the position difference and angle difference is based on relative transform between 2 frames.
• Frames need to have the same timestamp between reference and current slam result.