Разложение существенной матрицы дает неверные матрицы вращения и перевода

Я использую OpenCV 2.6 с C ++ для трехмерной реконструкции из 2 видов, для которых мне нужно найти матрицы поворота и перевода, чтобы перейти к триангуляции.
Но я не могу получить правильные матрицы вращения и перевода.

Я использую эти изображения для реконструкции, но я получаю R = [I] и T = [0,0,0.99] (приблизительные значения) и, следовательно, полностью искажен триангулированная трехмерная структура.
Ниже приведен фрагмент кода, который я использую для F, E и SVD.
Что не так с кодом, который приводит к этим ошибкам?
Я не смог найти подходящий код opencv2-c ++ на github, пожалуйста, дайте мне такие источники (если есть)

// FUNDAMENTAL MATRIX

OutputArray mask = noArray();
Mat F = findFundamentalMat(obj_points, scene_points, CV_LMEDS, 3, 0.99, mask);double det = determinant(F);SVD decompf = SVD(F);

Mat Uf = decompf.u;
Mat Lambdaf = decompf.w;
Mat Vtf = decompf.vt;Mat new_Lambdaf = (Mat_<double>(3, 3) << Lambdaf.at<double>(0, 0), 0, 0, 0, Lambdaf.at<double>(1, 0), 0, 0, 0, 0);

F = Uf*new_Lambdaf*Vtf;

// ESSENTIAL MATRIXMat E = F*K;
E = (K.t())*E;// SVDE.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 R1 = U*W*Vt;
Mat R2 = U*W.t()*Vt;cout << "\n\n\nR1: \n" << R1 << endl;
cout << "\n\n\nR2: \n" << R2 << endl;Mat T1 = (Mat_<float>(3, 1) << U.at<float>(0, 2), U.at<float>(1, 2), U.at<float>(2, 2));
Mat T2 = -1 * T1;