Path visualization in Gazebo

Elsewhere it is called "breadcrumbs", but for some strange reasons, in SLAM the term "porcupine droppings" is used more and more often. Anyway, the reason I use this rather non-elegant approach is a bug in Gazebo: any attempt to use Ogre engine to modify texture either results in a crash or produces no visual effect. It still remains a mystery to me, and I am probably going it revisit it at some point.

The tiny track visualization engine is implemented in a PathVisualizer class. Let's see how it works:

Here is the result of a simulation with ground truth path (red) and Kalman poath (yellow):

This was for GPS case, and tracks in a more familiar form look like:

Note that Kalman filter does an adequate job of tracking all 3 coordinates, including z. However, z coordinate will still have errors, and if, due to the error, it is below the surface of a 2.5d mesh, the marker for this point will not be visible. To fix it, I am using the surface function, that returns exact Z coordinate for x,y of a point. This trick is only used for visualization, so the markers never go "underground".

I am not going over details of how this track visualizer is connected to the rest of a code as this is trivial and well documented in code below.