Я работаю над проектом по оценке местоположения БПЛА (квадрокоптера) с использованием метода оптического потока. В настоящее время у меня есть код, который используетfarneback алгоритм от OpenCV. Текущий код работает нормально, когда камера всегда указывает на землю.

Теперь я хочу добавить поддержку в случае, когда камера не направлена ​​прямо вниз - это означает, что у квадрокоптера теперь есть шаг / крен / рыскание (углы Эйлера). Квадрокоптеры углов Эйлера известны, и я ищу метод для вычисления и применения необходимого преобразования на основе известных текущих углов Эйлера. Таким образом, полученное изображение будет таким, как если бы оно было снято сверху (см. Изображение ниже).

Я нашел методы, которые вычисляют преобразование при наличии 2 наборов (источник и назначение) из 4 углов с помощьюfindHomography или жеgetPerspectiveTransform функции от OpenCV. Но я не смог найти никаких методов, которые могли бы сделать это, зная только угол Эйлера (потому что я не знаю corenrs целевого изображения).

Поэтому мой вопрос в том, какой метод я могу использовать и как сделать так, чтобы кадр был таким, как если бы он был взят сверху, используя только углы Эйлера и высоту камеры от земли, если это необходимо?

Для того, чтобы продемонстрировать, что мне нужно:

Соответствующая часть моего текущего кода ниже:

for(;;)
{
    Mat m, disp, warp;
    vector<Point2f> corners;
    // take out frame- still distorted
    cap >> origFrame;
    // undistort the frame using the calibration parameters
    cv::undistort(origFrame, undistortFrame, cameraMatrix, distCoeffs, noArray());
    // lower the process effort by transforming the picture to gray
    cvtColor(undistortFrame, gray, COLOR_BGR2GRAY);

    if( !prevgray.empty() )
    {
        // calculate flow
        calcOpticalFlowFarneback(prevgray, gray, uflow, 0.5, 3/*def 3 */, 10/* def 15*/, 3, 3, 1.2 /* def 1.2*/, 0);
        uflow.copyTo(flow);

        // get average
        calcAvgOpticalFlow(flow, 16, corners);

        // calculate range of view - 2*tan(fov/2)*distance
        rovX = 2*0.44523*distanceSonar*100;     // 2 * tan(48/2) * dist(cm)
        rovY = 2*0.32492*distanceSonar*100;     // 2 * tan(36/2) * dist(cm)

        // calculate final x, y location
        location[0] += (currLocation.x/WIDTH_RES)*rovX;
        location[1] += (currLocation.y/HEIGHT_RES)*rovY;
    }
    //break conditions
    if(waitKey(1)>=0)
        break;
    if(end_run)
        break;
    std::swap(prevgray, gray);
}  

ОБНОВИТЬ:

После успешного добавления поворота мне все еще нужно отцентрировать изображение (и не выходить за рамки окна, как показано ниже). Я думаю, мне нужен какой-то перевод.Я хочу, чтобы центр исходного изображения находился в центре целевого изображения. Как я могу добавить это?

Функция вращения, которая работает:

void rotateFrame(const Mat &input, Mat &output, Mat &A , double roll, double pitch, double yaw){
    Mat Rx = (Mat_<double>(3, 3) <<
              1,          0,           0,
              0, cos(roll), -sin(roll),
              0, sin(roll),  cos(roll));
    Mat Ry = (Mat_<double>(3, 3) <<
              cos(pitch), 0, sin(pitch),
              0, 1,          0,
              -sin(pitch), 0,  cos(pitch));
    Mat Rz = (Mat_<double>(3, 3) <<
              cos(yaw), -sin(yaw), 0,
              sin(yaw),  cos(yaw), 0,
              0,          0,           1);

    Mat R = Rx*Ry*Rz;
    Mat trans = A*R*A.inv();

    warpPerspective(input, output, trans, input.size());
}

Когда я запускаю его сrotateFrame(origFrame, processedFrame, cameraMatrix, 0, 0, 0); Я получаю изображение, как ожидалось:

Но когда я бегу с 10 градусов в кренеrotateFrame(origFrame, processedFrame, cameraMatrix, 20*(M_PI/180), 0, 0);, Изображение выходит из окна фрейма: