我想使用 OpenCV 进行一些运动结构。
到目前为止,我已经有了基本矩阵和基本矩阵。
有了基本矩阵,我正在做 SVD 来获得 R 和 T。
我的问题是,我有 2 个可能的 R 解决方案和 2 个可能的 T 解决方案,这导致整体姿势有 4 个解决方案,其中 4 个解决方案中只有一个是正确的。
我怎样才能找到正确的解决方案?
这是我的代码:
private void calculateRT(Mat E, Mat R, Mat T){
Mat w = new Mat();
Mat u = new Mat();
Mat vt = new Mat();
Mat diag = new Mat(3,3,CvType.CV_64FC1);
double[] diagVal = {1,0,0,0,1,0,0,0,1};
diag.put(0, 0, diagVal);
Mat newE = new Mat(3,3,CvType.CV_64FC1);
Core.SVDecomp(E, w, u, vt, Core.DECOMP_SVD);
Core.gemm(u, diag, 1, vt, 1, newE);
Core.SVDecomp(newE, w, u, vt, Core.DECOMP_SVD);
publishProgress("U: " + u.dump());
publishProgress("W: " + w.dump());
publishProgress("vt:" + vt.dump());
double[] W_Values = {0,-1,0,1,0,0,0,0,1};
Mat W = new Mat(new Size(3,3), CvType.CV_64FC1);
W.put(0, 0, W_Values);
double[] Wt_values = {0,1,0-1,0,0,0,0,1};
Mat Wt = new Mat(new Size(3,3), CvType.CV_64FC1);
Wt.put(0,0,Wt_values);
Mat R1 = new Mat();
Mat R2 = new Mat();
// u * W * vt = R
Core.gemm(u, Wt, 1, vt, 1, R2);
Core.gemm(u, W, 1, vt, 1, R1);
publishProgress("R: " + R.dump());
// +- T (2 possible solutions for T)
Mat T1 = new Mat();
Mat T2 = new Mat();
// T = u.t
u.col(2).copyTo(T1);
publishProgress("T : " + T.dump());
Core.multiply(T, new Scalar(-1.0, -1.0, -1.0), T2);
// TODO Here I have to find the correct combination for R1 R2 and T1 T2
}