Поворот назад точек из повернутого изображения в OpenCV

У меня проблемы с вращением. Я хочу сделать следующее:

• Повернуть изображение
• Обнаружить особенности на повернутом изображении (точки)
• Повернуть точки назад, чтобы я мог получить координаты точек, соответствующие исходному изображению

I ' м немного застрял на третьей ступеньке.

Мне удалось повернуть изображение с помощью следующего кода:

cv::Mat M(2, 3, CV_32FC1);
cv::Point2f center((float)dst_img.rows / 2.0f, (float)dst_img.cols / 2.0f);
M = cv::getRotationMatrix2D(center, rotateAngle, 1.0);
cv::warpAffine(dst_img, rotated, M, cv::Size(rotated.cols, rotated.rows));

Я пытаюсь повернуть точки с помощью этого кода:

float xp = r.x * std::cos( PI * (-rotateAngle) / 180 ) - r.y * sin(PI * (rotateAngle) / 180);
float yp = r.x * sin(PI * (-rotateAngle) / 180) + r.y * cos(PI * (rotateAngle) / 180);

Это не значит, что работать, но точки не возвращаются должным образом на изображение. Есть зачет.

Спасибо за вашу помощь