[fix] Fix UndistortWithLogStates

• In case the undistortion was not possible, the transform to base was not performed leading to inconsistency in the output if the Lidar calibration is provided