在c ++中使用SVD查找转换矩阵

Question

我试图使用SVD找到图像的平移和旋转。找到我用过模板匹配的2张图片之间的对应点。匹配是完美的，但正在计算的矩阵如下：

essential matrix [-2.039822534788701e-019, 0.002807993863143686, 0.4097425267405184;
  -0.002807993863143689, 4.920343025661717e-018, -0.3264091934071822;
  -0.443438453098242, 0.3601051197649062, 1];


SVD components:
u [0.2992078577907173, -0.7163016363498457, 0.6303860908989213;
  -0.2371264394987113, 0.5841077281400203, 0.7762662002288702;
  0.9242541368730539, 0.3817461560901925, -0.004915565421039324]
vt [-0.3294400799571679, 0.2686417102753414, 0.9051523988910125;
  -0.7094007484184662, 0.5622100799500159, -0.425053413285809;
  -0.623072878450936, -0.7821454196649983, 0.005360096634284935]
w [1.242058875140318;
  0.24093729282062;
  4.072587793310116e-019]


Rotation matrix
[-0.8410132227937885, -0.1324074233048742, 0.5245617535998686;
  -0.1230242455085329, -0.8973836265311757, -0.4237542470003247;
  -0.5268413367448126, 0.4209167389015554, -0.7384221724795504]
Translation matrix
[-0.2145634591193726, 0.1688602189163624, -0.8498187651237489;
  0.176149983599124, -0.1386627332926497, 0.6923530041855173;
  0.3013810351332639, -0.2425071529534591, 0.3532261924120222]

这是我计算平移和旋转矩阵的方法：

cv::SVD decomp = cv::SVD(fundamentalMatrix);
cv::Mat R1 = decomp.u * cv::Mat(Wt) * decomp.vt;
cv::Mat t1 = decomp.u * cv::Mat(W) * cv::Mat(w3) * ut;

公式的来源：https://en.wikipedia.org/wiki/Essential_matrix https://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication

但是根据公式，我在翻译矩阵中的对角线应该得到0。

图像中的平移恰好是x轴上的12个像素和y轴上的6个像素。

但是计算出的值很远......

编辑：为了澄清，我正在尝试使用连续帧中的立体图像来估计3D中的相机旋转和平移。