Add visualization markers
Merged code from robot