Adding bundle adjustment functionality