Performance improvements

[jmirabel]

Here are some ideas for improving the tracker, both in terms of quality and computational time.

• initialization step:
  • the forward kinematics could give a good idea of where in the image to look for tag. The image can then be cropped before running the AprilTag detector.
• when tracking,
  • use the velocity requested by SoT to provide a better initial pose to the tracker.
  • using the AprilTag tracker:
    • crop the image according to the previous position.
    • add a filter to avoid the high frequencies oscillation.
  • using the ModelBased tracker, check from time to time the pose using the AprilTags.
• add a pose checker that computes the difference between the expected pose and the computed one.