Added waypoint markers to visual map