Added skeleton visualization code. Waiting on ParticleFilter.