cleanup voxel_grid package

The voxel_grid_main.cpp was basically left over sample code, but doesn't really add anything over what is in the test. Did copy the one relevant piece of example code (which was commented out) into the test code for safe keeping. This reduces the amount of code to later maintain. Also updated cmake formatting while at it.