First version of code.
Now reading XML file and presention distortion values.
Successfully undistorting.
Removed expand, using getOptimalCameraMatrix now...