Simple MATLAB example of bundle adjustment on toy dataset