Трехмерная реконструкция планарных маркеров в OpenCV

Question

Я пытаюсь выполнить 3D реконструкцию (структура из движения) из нескольких изображений плоских маркеров. Я очень новичок в MVG и openCV.

Насколько я понял, я должен сделать следующие шаги:

1. Определите соответствующие 2D угловые точки на одном изображении.
2. Рассчитайте позу камеры для первого изображения, используя cv::solvePNP(предполагая, что начало координат находится в центре маркера).
3. Повторите 1 и 2 для второго изображения.
4. Оцените относительное движение камеры с помощью Rot_relative = R2 - R1, Trans_relative = T2-T1.
5. Теперь предположим, что первая камера является исходной конструкцией Матрицы проекции 3x4 для обоих видов, P1 =[I|0]*CameraMatrix(известная по калибровке) и P2 = [Rot_relative |Trans_relative ].
6. Используйте созданные матрицы проекций и 2D угловые точки для триангуляции трехмерной координаты с помощью cv::triangulatePoints(P1,P2,point1,point2,OutMat)
7. Трехмерная координата может быть найдена путем деления каждой строки OutMat на 4-ую строку.
8. Я надеялся сохранить свое "первое представление" в качестве источника и повторить n представлений, повторяя шаги с 1 по 7(я полагаю, это называется Global SFM).

Я надеялся получить (n-1) трехмерные точки углов с "Первым видом в качестве источника", который мы могли бы оптимизировать, используя Bundle Adjustment.

Но результат, который я получаю, очень разочаровывает, что вычисленные 3D-точки сильно смещены.

Это вопросы:

1. Есть ли что-то не так с шагами, за которыми я следовал?

2. Должен ли я использовать cv::findHomography() и cv::degposeHomographyMat(), чтобы найти относительное движение камеры?

3. Должны ли нормализоваться и неискажаться точка1 и точка2 в cv:: triangulatePoints (P1, P2, точка1, точка2, OutMat)? Если да, то как следует интерпретировать "Outmat"?

Пожалуйста, если у вас есть понимание этой темы, вы можете указать на мою ошибку?

PS Я пришел к пониманию выше, прочитав "Геометрия MultiView в Computer Vision"

Пожалуйста, найдите фрагмент кода ниже:

cv::Mat Reconstruction::Triangulate(std::vector<cv::Point2f> 
ImagePointsFirstView, std::vector<cv::Point2f>ImagePointsSecondView)
{   
    cv::Mat rVectFirstView, tVecFristView;
    cv::Mat rVectSecondView, tVecSecondView;
    cv::Mat RotMatFirstView = cv::Mat(3, 3, CV_64F);
    cv::Mat RotMatSecondView = cv::Mat(3, 3, CV_64F);

    cv::solvePnP(RealWorldPoints, ImagePointsFirstView, cameraMatrix, distortionMatrix, rVectFirstView, tVecFristView);
    cv::solvePnP(RealWorldPoints, ImagePointsSecondView, cameraMatrix, distortionMatrix, rVectSecondView, tVecSecondView);



    cv::Rodrigues(rVectFirstView, RotMatFirstView);
    cv::Rodrigues(rVectSecondView, RotMatSecondView);


    cv::Mat RelativeRot = RotMatFirstView-RotMatSecondView ;
    cv::Mat RelativeTrans = tVecFristView-tVecSecondView ;

    cv::Mat RelativePose; 

    cv::hconcat(RelativeRot, RelativeTrans, RelativePose);


    cv::Mat ProjectionMatrix_0 = cameraMatrix*cv::Mat::eye(3, 4, CV_64F);

    cv::Mat ProjectionMatrix_1 = cameraMatrix* RelativePose;
    cv::Mat X;
    cv::undistortPoints(ImagePointsFirstView, ImagePointsFirstView, cameraMatrix, distortionMatrix, cameraMatrix);
    cv::undistortPoints(ImagePointsSecondView, ImagePointsSecondView, cameraMatrix, distortionMatrix, cameraMatrix);

    cv::triangulatePoints(ProjectionMatrix_0, ProjectionMatrix_1, ImagePointsFirstView, ImagePointsSecondView, X);
    X.row(0) = X.row(0) / X.row(3);
    X.row(1) = X.row(1) / X.row(3);
    X.row(2) = X.row(2) / X.row(3);



    return X;
}