Fixed an error in the computation of the corrected pose