Added the 3d point calculation needed to solve the PnP