Got basic ar_tools fiducial tracking working