Matlab code for bundle_adjustment