Towards Visibility-based PRM

1. Sample() has been modified. Now it does not add the milestone to the roadmap.
2. Connect() has also been modified. It is the responsible now to check the visibility and add the milestone to the appropriate set (Guards/ Connections).