前面已經利用分解本質矩陣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;
}
}
}
}四種組合依次進行判斷，首先給定一種組合，我們有兩個由2D點匹配和P矩陣產生的關鍵等式：x=PX和x’=P’X，x和x’是匹配的二維點，X是兩個相機進行拍照的真實世界三維點。如果我們重寫這個等式，我們可以公式化爲一個線性方程系統，該系統可以解出X的值，X正是我們所期望尋找的值。假定X=(x,y,z,1)t（一個合理的點的假設，這些點離相機的中心不太近或者不太遠）產生一個形式爲AX=B的非齊次線性方程系統。
將獲得由兩個2維點產生的3D點的一個近似。還有一個要注意的事情是，2D點用齊次座標表示，意味着x和y的值後面追加一個1。我們必須確保這些點在標準化的座標系中，這意味着它們必須乘以先前的標定矩陣K.我們可能注意到我們簡單地利用KP矩陣來替代k矩陣和每一個點相乘（每一點乘以KP)。
通過計算重構所有當前兩幅圖像的點，計算在攝像機前面的重構點的佔比，判斷當前設置的P1是否正確。
//Triagulate points
double TriangulatePoints(const vector<KeyPoint>& pt_set1,
const vector<KeyPoint>& pt_set2,
const Mat& K,
const Mat& Kinv,
const Mat& distcoeff,
const Matx34d& P,
const Matx34d& P1,
vector<CloudPoint>& pointcloud,
vector<KeyPoint>& correspImg1Pt)
{
#ifdef __SFM__DEBUG__
vector<double> depths;
#endif
// pointcloud.clear();
correspImg1Pt.clear();
Matx44d P1_(P1(0,0),P1(0,1),P1(0,2),P1(0,3),
P1(1,0),P1(1,1),P1(1,2),P1(1,3),
P1(2,0),P1(2,1),P1(2,2),P1(2,3),
0, 0, 0, 1);
Matx44d P1inv(P1_.inv());
cout << "Triangulating...";
double t = getTickCount();
vector<double> reproj_error;
unsigned int pts_size = pt_set1.size();
#if 0
//Using OpenCV's triangulation
//convert to Point2f
vector<Point2f> _pt_set1_pt,_pt_set2_pt;
KeyPointsToPoints(pt_set1,_pt_set1_pt);
KeyPointsToPoints(pt_set2,_pt_set2_pt);
//undistort
Mat pt_set1_pt,pt_set2_pt;
undistortPoints(_pt_set1_pt, pt_set1_pt, K, distcoeff);
undistortPoints(_pt_set2_pt, pt_set2_pt, K, distcoeff);
//triangulate
Mat pt_set1_pt_2r = pt_set1_pt.reshape(1, 2);//通道數變爲1，行數爲2，爲什麼這麼幹？
Mat pt_set2_pt_2r = pt_set2_pt.reshape(1, 2);
Mat pt_3d_h(1,pts_size,CV_32FC4);
cv::triangulatePoints(P,P1,pt_set1_pt_2r,pt_set2_pt_2r,pt_3d_h);
//calculate reprojection
vector<Point3f> pt_3d;
convertPointsHomogeneous(pt_3d_h.reshape(4, 1), pt_3d);
cv::Mat_<double> R = (cv::Mat_<double>(3,3) << P(0,0),P(0,1),P(0,2), P(1,0),P(1,1),P(1,2), P(2,0),P(2,1),P(2,2));
Vec3d rvec; Rodrigues(R ,rvec);
Vec3d tvec(P(0,3),P(1,3),P(2,3));
vector<Point2f> reprojected_pt_set1;
projectPoints(pt_3d,rvec,tvec,K,distcoeff,reprojected_pt_set1);
for (unsigned int i=0; i<pts_size; i++) {
CloudPoint cp;
cp.pt = pt_3d[i];
pointcloud.push_back(cp);
reproj_error.push_back(norm(_pt_set1_pt[i]-reprojected_pt_set1[i]));
}
#else
Mat_<double> KP1 = K * Mat(P1);
#pragma omp parallel for num_threads(1)
for (int i=0; i<pts_size; i++) {//對每一個關鍵點，先轉化成齊次座標模式，然後在左邊乘以Kinv，得到帶標定參數的點。
Point2f kp = pt_set1[i].pt;
Point3d u(kp.x,kp.y,1.0);//齊次座標
Mat_<double> um = Kinv * Mat_<double>(u);
u.x = um(0); u.y = um(1); u.z = um(2);
Point2f kp1 = pt_set2[i].pt;
Point3d u1(kp1.x,kp1.y,1.0);
Mat_<double> um1 = Kinv * Mat_<double>(u1);
u1.x = um1(0); u1.y = um1(1); u1.z = um1(2);
Mat_<double> X = IterativeLinearLSTriangulation(u,P,u1,P1);//進行迭代線性重構
// cout << "3D Point: " << X << endl;
// Mat_<double> x = Mat(P1) * X;
// cout << "P1 * Point: " << x << endl;
// Mat_<double> xPt = (Mat_<double>(3,1) << x(0),x(1),x(2));
// cout << "Point: " << xPt << endl;
Mat_<double> xPt_img = KP1 * X; //reproject
// cout << "Point * K: " << xPt_img << endl;
Point2f xPt_img_(xPt_img(0)/xPt_img(2),xPt_img(1)/xPt_img(2));
#pragma omp critical
{
double reprj_err = norm(xPt_img_-kp1);
reproj_error.push_back(reprj_err);
CloudPoint cp;
cp.pt = Point3d(X(0),X(1),X(2));
cp.reprojection_error = reprj_err;
pointcloud.push_back(cp);
correspImg1Pt.push_back(pt_set1[i]);
#ifdef __SFM__DEBUG__
depths.push_back(X(2));
#endif
}
}
#endif
Scalar mse = mean(reproj_error);
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "Done. ("<<pointcloud.size()<<"points, " << t <<"s, mean reproj err = " << mse[0] << ")"<< endl;
//show "range image"
#ifdef __SFM__DEBUG__
{
double minVal,maxVal;
minMaxLoc(depths, &minVal, &maxVal);
Mat tmp(240,320,CV_8UC3,Scalar(0,0,0)); //cvtColor(img_1_orig, tmp, CV_BGR2HSV);
for (unsigned int i=0; i<pointcloud.size(); i++) {
double _d = MAX(MIN((pointcloud[i].z-minVal)/(maxVal-minVal),1.0),0.0);
circle(tmp, correspImg1Pt[i].pt, 1, Scalar(255 * (1.0-(_d)),255,255), CV_FILLED);
}
cvtColor(tmp, tmp, CV_HSV2BGR);
imshow("Depth Map", tmp);
waitKey(0);
destroyWindow("Depth Map");
}
#endif
return mse[0];
}
Mat_<double> IterativeLinearLSTriangulation(Point3d u, //homogenous image point (u,v,1)
Matx34d P, //camera 1 matrix
Point3d u1, //homogenous image point in 2nd camera
Matx34d P1 //camera 2 matrix
) {
double wi = 1, wi1 = 1;
Mat_<double> X(4,1);
Mat_<double> X_ = LinearLSTriangulation(u,P,u1,P1);
X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X(3) = 1.0;
for (int i=0; i<10; i++) { //Hartley suggests 10 iterations at most
//recalculate weights
double p2x = Mat_<double>(Mat_<double>(P).row(2)*X)(0);
double p2x1 = Mat_<double>(Mat_<double>(P1).row(2)*X)(0);
//breaking point
if(fabsf(wi - p2x) <= EPSILON && fabsf(wi1 - p2x1) <= EPSILON) break;
wi = p2x;
wi1 = p2x1;
//reweight equations and solve
Matx43d A((u.x*P(2,0)-P(0,0))/wi, (u.x*P(2,1)-P(0,1))/wi, (u.x*P(2,2)-P(0,2))/wi,
(u.y*P(2,0)-P(1,0))/wi, (u.y*P(2,1)-P(1,1))/wi, (u.y*P(2,2)-P(1,2))/wi,
(u1.x*P1(2,0)-P1(0,0))/wi1, (u1.x*P1(2,1)-P1(0,1))/wi1, (u1.x*P1(2,2)-P1(0,2))/wi1,
(u1.y*P1(2,0)-P1(1,0))/wi1, (u1.y*P1(2,1)-P1(1,1))/wi1, (u1.y*P1(2,2)-P1(1,2))/wi1
);
Mat_<double> B = (Mat_<double>(4,1) << -(u.x*P(2,3) -P(0,3))/wi,
-(u.y*P(2,3) -P(1,3))/wi,
-(u1.x*P1(2,3) -P1(0,3))/wi1,
-(u1.y*P1(2,3) -P1(1,3))/wi1
);
solve(A,B,X_,DECOMP_SVD);
X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X(3) = 1.0;
}
return X;
}Mat_<double> LinearLSTriangulation(Point3d u, //homogenous image point (u,v,1)
Matx34d P, //camera 1 matrix
Point3d u1, //homogenous image point in 2nd camera
Matx34d P1 //camera 2 matrix
)
{
//build matrix A for homogenous equation system Ax = 0
//assume X = (x,y,z,1), for Linear-LS method
//which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
// cout << "u " << u <<", u1 " << u1 << endl;
// Matx<double,6,4> A; //this is for the AX=0 case, and with linear dependence..
// A(0) = u.x*P(2)-P(0);
// A(1) = u.y*P(2)-P(1);
// A(2) = u.x*P(1)-u.y*P(0);
// A(3) = u1.x*P1(2)-P1(0);
// A(4) = u1.y*P1(2)-P1(1);
// A(5) = u1.x*P(1)-u1.y*P1(0);
// Matx43d A; //not working for some reason...
// A(0) = u.x*P(2)-P(0);
// A(1) = u.y*P(2)-P(1);
// A(2) = u1.x*P1(2)-P1(0);
// A(3) = u1.y*P1(2)-P1(1);
Matx43d A(u.x*P(2,0)-P(0,0), u.x*P(2,1)-P(0,1), u.x*P(2,2)-P(0,2),
u.y*P(2,0)-P(1,0), u.y*P(2,1)-P(1,1), u.y*P(2,2)-P(1,2),
u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1), u1.x*P1(2,2)-P1(0,2),
u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1), u1.y*P1(2,2)-P1(1,2)
);
Matx41d B(-(u.x*P(2,3) -P(0,3)),
-(u.y*P(2,3) -P(1,3)),
-(u1.x*P1(2,3) -P1(0,3)),
-(u1.y*P1(2,3) -P1(1,3)));
Mat_<double> X;
solve(A,B,X,DECOMP_SVD);
return X;
}其中solve (Ax=B)用來求X。這樣就得到了三維重構點X的三維座標。
然後再投射在圖像上，使用
Mat_<double> xPt_img = KP1
* X; 計算重投影誤差：
double reprj_err = norm(xPt_img_-kp1);double reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
reproj_error1 > 100.0!TestTriangulation(pcloud,P1,tmp_status)
bool TestTriangulation(const vector<CloudPoint>& pcloud, const Matx34d& P, vector<uchar>& status) {
vector<Point3d> pcloud_pt3d = CloudPointsToPoints(pcloud);//把CloudPoint轉化成Point3d
vector<Point3d> pcloud_pt3d_projected(pcloud_pt3d.size());
Matx44d P4x4 = Matx44d::eye();
for(int i=0;i<12;i++) P4x4.val[i] = P.val[i];//複製
perspectiveTransform(pcloud_pt3d, pcloud_pt3d_projected, P4x4);//這裏是應用P對三維點做透視變換
status.resize(pcloud.size(),0);
for (int i=0; i<pcloud.size(); i++) {
status[i] = (pcloud_pt3d_projected[i].z > 0) ? 1 : 0;//判斷z座標是否在攝像機前面
}
int count = countNonZero(status);//計數，得到在攝像機前面點的個數
double percentage = ((double)count / (double)pcloud.size());//攝像機前面的點在總點數中的佔比，少於75%就代表P1無效
cout << count << "/" << pcloud.size() << " = " << percentage*100.0 << "% are in front of camera" << endl;
if(percentage < 0.75)
return false; //less than 75% of the points are in front of the camera
//check for coplanarity of points檢查共平面的點
if(false) //not這裏沒有執行
{
cv::Mat_<double> cldm(pcloud.size(),3);
for(unsigned int i=0;i<pcloud.size();i++) {
cldm.row(i)(0) = pcloud[i].pt.x;
cldm.row(i)(1) = pcloud[i].pt.y;
cldm.row(i)(2) = pcloud[i].pt.z;
}
cv::Mat_<double> mean;
cv::PCA pca(cldm,mean,CV_PCA_DATA_AS_ROW);
int num_inliers = 0;
cv::Vec3d nrm = pca.eigenvectors.row(2); nrm = nrm / norm(nrm);
cv::Vec3d x0 = pca.mean;
double p_to_plane_thresh = sqrt(pca.eigenvalues.at<double>(2));
for (int i=0; i<pcloud.size(); i++) {
Vec3d w = Vec3d(pcloud[i].pt) - x0;
double D = fabs(nrm.dot(w));
if(D < p_to_plane_thresh) num_inliers++;
}
cout << num_inliers << "/" << pcloud.size() << " are coplanar" << endl;
if((double)num_inliers / (double)(pcloud.size()) > 0.85)
return false;
}
return true;
}這個函數的主要目的是對三維點用於P1進行透視變換，經過透視變換後的點還在攝像機前面的數量佔總點數的比率大於75%表示P1有效。
以上就是判斷P1是哪種組合的分析，可能不是很清楚，只是略記一下。想起來再補充。
有個疑問是第一個條件已經基於KP1計算重投影誤差了，爲什麼第二個條件還要進行基於P1的透視變換，判斷是否在攝像機前面。這樣是不是重複，難道相差一個K標定參數，就有必要了。
我感覺是K很重要，直接決定了重投影投射在圖像上的座標，而沒有K的P1透視變換隻是把三維點從世界座標系變換到相機座標系，這樣就可判斷是不是在相機前面了，這樣纔會有必要。
先分析到這吧，以後再分析，光束平差法BA進行多視圖重建，以及PCL進行三維顯示的內容。話說結果圖的三維顯示還是很贊。