Added function to get a binary map from a point cloud
Finished Initialization (sec. VII) and visualization included
Finished Measurement Based Update Function