please highlight, where the current problem is.
(e.g. KeyPoint.pt is already a Point2f)
everything below that looks broken. you probably should convert all keypoints to 2d points before (not only 4), and do calculations on those, not the actual kp.