Mastering Opencv ch4：SFM详解（三）

前面已经利用分解本质矩阵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=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];
}

其中IterativeLinearLSTriangulation(u,P,u1,P1);//进行迭代线性重构的函数形式：

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_ X_ = LinearLSTriangulation(u,P,u1,P1);计算出初始的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;
}

计算出初始的重构三维点后，用P0，P1矩阵的第二行与重构点坐标进行相乘，取行向量的第零个值作为权值。再次计算加权值的三维点重构。 
其中solve (Ax=B)用来求X。这样就得到了三维重构点X的三维坐标。 
然后再投射在图像上，使用 Mat_<double> xPt_img = KP1 * X;  
计算重投影误差：

double reprj_err = norm(xPt_img_-kp1);

计算出所有点的重投影误差后，求平均，作为该P1组合下的误差。若是大于100.0，则该P1是无效的。

double reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);

reproj_error1 > 100.0

下面看判断P1是否有效的另一个条件： 
!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进行三维显示的内容。话说结果图的三维显示还是很赞。