Неправильная матрица перевода из SVD Essential Matrix для трехмерной реконструкции с использованием 2 изображений

Я пытаюсь найти 3D-модель из 2 изображений, снятых с одной и той же камеры, используя OpenCV с C ++. Я последовал за этот метод. Я все еще не могу исправить ошибку в вычислениях R и T.

Изображение 1: с удаленным фоном для устранения несоответствий

Изображение 2: Переведено только в направлении X по сравнению с изображением 1 с удаленным фоном для устранения несоответствий

Я нашел матрицу внутренней камеры (K) с помощью MATLAB Toolbox. Я нашел это быть:

K =

[3058.8    0    -500

0       3057.3   488

0       0         1]

Все ключевые точки сопоставления изображений (с использованием SIFT и BruteForce Matching, Mismatches Eliminated) были выровнены по центру изображения следующим образом:

obj_points.push_back(Point2f(keypoints1[symMatches[i].queryIdx].pt.x - image1.cols / 2, -1 * (keypoints1[symMatches[i].queryIdx].pt.y - image1.rows / 2)));
scene_points.push_back(Point2f(keypoints2[symMatches[i].trainIdx].pt.x - image1.cols / 2, -1 * (keypoints2[symMatches[i].trainIdx].pt.y - image1.rows / 2)));

Из точечных соответствий я узнал, что фундаментальная матрица использует RANSAC в OpenCV

Фундаментальная матрица:

[0             0      -0.0014

0              0       0.0028

0.00149    -0.00572      1  ]

Essential Matrix получено с использованием:

E = (camera_Intrinsic.t())*f*camera_Intrinsic;

E получил:

[ 0.0094        36.290       1.507

-37.2245        -0.6073      14.71

-1.3578         -23.545     -0.442]

СВД Е:

E.convertTo(E, CV_32F);
Mat W = (Mat_<float>(3, 3) << 0, -1, 0, 1, 0, 0, 0, 0, 1);
Mat Z = (Mat_<float>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 0);

SVD decomp = SVD(E);
Mat U = decomp.u;
Mat Lambda = decomp.w;
Mat Vt = decomp.vt;

Новая основная матрица для эпиполярного ограничения:

Mat diag = (Mat_<float>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 0);
Mat new_E = U*diag*Vt;

SVD new_decomp = SVD(new_E);

Mat new_U = new_decomp.u;
Mat new_Lambda = new_decomp.w;
Mat new_Vt = new_decomp.vt;

Вращение от СВД:

Mat R1 = new_U*W*new_Vt;
Mat R2 = new_U*W.t()*new_Vt;

Перевод из СВД:

Mat T1 = (Mat_<float>(3, 1) << new_U.at<float>(0, 2), new_U.at<float>(1, 2), new_U.at<float>(2, 2));
Mat T2 = -1 * T1;

Я получал матрицы R, чтобы быть:

R1:

[ -0.58      -0.042     0.813
-0.020       -0.9975    -0.066
0.81        -0.054     0.578]

R2:

[  0.98     0.0002      0.81
-0.02      -0.99      -0.066
0.81      -0.054      0.57 ]

Матрицы перевода:

T1:

[0,543

-0,030

0,838]

T2:

[-0,543

0.03

-0,83]

Пожалуйста, уточните, где есть ошибка.

Эти 4 набора матрицы P2 R | T с P1 = [I] дают неверные триангулированные модели.

Кроме того, я думаю, что полученная T-матрица неверна, так как предполагалось, что это будет только сдвиг x и отсутствие сдвига z.

При попытке с таким же образом image1 = image2 -> я получил T = [0,0,1]. Что означает Tz = 1? (где нет сдвига по оси z, поскольку оба изображения одинаковы)

И нужно ли совмещать координаты моей ключевой точки с центром изображения или с основным фокусом, полученным при калибровке?