[visibility] Bresenham's line algorithm

Implementation + unit test.
Add (x,y,z,theta) point
Robot model handles sensor position.

Namely sensor's (x,y,z,t) relative position.
[Visibility] Visibility link between two points
[Visibility] Unit test.
[visibility] cleanup previous commits
[visibility] store heightmap reference localy