Added option to optimize intrinsics in BA step, and fixed bug that happens in a corner case for EquidistantCamera.