Improving PCAutonomousGP
Refactoring point cloud tools
Small refactoring and improvements
More PCL functions
Fixing cMg problems and refactoring