前面已经利用分解本质矩阵E得到两幅图像的姿态R1,R2,t1,t2.
如何把这四个值组合成P1,得到正确的解,接下来就要分析。
1:利用三维重构求取在摄像机前面的重构点的所占比,得出最高占比的那个P1,就是要求得解。
P1 = Matx34d(R1(0,0), R1(0,1), R1(0,2), t1(0),
R1(1,0), R1(1,1), R1(1,2), t1(1),
R1(2,0), R1(2,1), R1(2,2), t1(2));
cout << "Testing P1 " << endl << Mat(P1) << endl;
vector<CloudPoint> pcloud,pcloud1; vector<KeyPoint> corresp;
double reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
double reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);
vector<uchar> tmp_status;
//check if pointa are triangulated --in front-- of cameras for all 4 ambiguations
if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
P1 = Matx34d(R1(0,0), R1(0,1), R1(0,2), t2(0),
R1(1,0), R1(1,1), R1(1,2), t2(1),
R1(2,0), R1(2,1), R1(2,2), t2(2));
cout << "Testing P1 "<< endl << Mat(P1) << endl;
pcloud.clear(); pcloud1.clear(); corresp.clear();
reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);
if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
if (!CheckCoherentRotation(R2)) {
cout << "resulting rotation is not coherent\n";
P1 = 0;
return false;
}
P1 = Matx34d(R2(0,0), R2(0,1), R2(0,2), t1(0),
R2(1,0), R2(1,1), R2(1,2), t1(1),
R2(2,0), R2(2,1), R2(2,2), t1(2));
cout << "Testing P1 "<< endl << Mat(P1) << endl;
pcloud.clear(); pcloud1.clear(); corresp.clear();
reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);
if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
P1 = Matx34d(R2(0,0), R2(0,1), R2(0,2), t2(0),
R2(1,0), R2(1,1), R2(1,2), t2(1),
R2(2,0), R2(2,1), R2(2,2), t2(2));
cout << "Testing P1 "<< endl << Mat(P1) << endl;
pcloud.clear(); pcloud1.clear(); corresp.clear();
reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);
if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
cout << "Shit." << endl;
return false;
}
}
}
}
这里主要用了两个函数TriangulatePoints和TestTriangulation。
四种组合依次进行判断,首先给定一种组合,我们有两个由2D点匹配和P矩阵产生的关键等式:x