Some bugs removed, now normals are estimated quite good.
Added a PCL visualizer thread, moved normal optimization into a class.