SFM: Added code to obtain optimal rotation matrix