Question

我有一个相机，可以从略微不同的位置和方向拍摄同一场景的两张照片。

对于我的SfM方法，我正在进行必要的分解，这是完美的，因为我得到了相关相机姿势的4种可能的解决方案。我假设第一台摄像机位于世界空间的原点。

问题是当我试图找到R1 / R2和T1 / T2的正确组合时。我的想法是，我将之前发现的所有优秀比赛进行三角测量。这将给出一个世界空间/第一个摄像机空间中的点（x，y，z，w）列表。

现在我想将此点的副本转换为第二个图像的系统，并检查原始点和复制转换点的z值是否为正。如果大多数点发生这种情况，R和T的使用组合是正确的，但它仍然找到错误的组合，因为整流会产生错误的结果（从硬编码和测试我知道R2和T2是正确的，但它找到R1和T2）

这是我的代码：

/**
 * Transforms the given point into the target camera system 
 * and returns the z value
 * @param point Point to transform
 * @param system Camera system to transform to
 * @return double z value
 */
private Mat transformPointIntoSystem(Mat point, Mat system){
    double[] vals = {point.get(0, 0)[0], point.get(1, 0)[0] ,point.get(2, 0)[0] ,1};
    Mat point_homo = new Mat(4,1,CvType.CV_32F);
    point_homo.put(0, 0, vals);

    Mat dst = new Mat(3,4,CvType.CV_32F);
    // Multiply the matrix * vector
    Core.gemm(system, point_homo, 1, dst, 0, dst);

    return dst; 
}

 private void findCorrectRT(Mat R1, Mat R2, Mat T1, Mat T2, Mat R, Mat T, MatOfPoint2f objLeft, MatOfPoint2f objRight){
    Mat res = new Mat(); // Result mat for triangulation

    Mat P1 = new Mat(4,4,CvType.CV_32F);
    double[] diagVal = {1,0,0,0,
                        0,1,0,0,
                        0,0,1,0};       
    P1.put(0, 0, diagVal);


    int[] max = new int[4];
    for(int i = 0; i < max.length; i++)
        max[i] = 0;

    Mat P2 = buildCameraMatrix(R1, T1);     
    Calib3d.triangulatePoints(P1, P2, objLeft, objRight, res);      
    publishProgress(res.size().width + " , " + res.size().height);
    for(int i = 0; i < res.size().width; i++){
        Mat X1 = transformPointIntoSystem(res.col(i), P1);
        Mat X2 = transformPointIntoSystem(X1, P2);

        if(X1.get(2, 0)[0] >= 0 && X2.get(2, 0)[0] >= 0){
            max[0] += 1;
        }
    }

    P2 = buildCameraMatrix(R1, T2);     
    Calib3d.triangulatePoints(P1, P2, objLeft, objRight, res);      
    publishProgress(res.size().width + " , " + res.size().height);
    for(int i = 0; i < res.size().width; i++){
        Mat X1 = transformPointIntoSystem(res.col(i), P1);
        Mat X2 = transformPointIntoSystem(X1, P2);

        if(X1.get(2, 0)[0] >= 0 && X2.get(2, 0)[0] >= 0){
            max[1] += 1;
        }
    }

    P2 = buildCameraMatrix(R2, T1);     
    Calib3d.triangulatePoints(P1, P2, objLeft, objRight, res);      
    publishProgress(res.size().width + " , " + res.size().height);
    for(int i = 0; i < res.size().width; i++){
        Mat X1 = transformPointIntoSystem(res.col(i), P1);
        Mat X2 = transformPointIntoSystem(X1, P2);

        if(X1.get(2, 0)[0] >= 0 && X2.get(2, 0)[0] >= 0){
            max[2] += 1;
        }
    }

    P2 = buildCameraMatrix(R2, T2);     
    Calib3d.triangulatePoints(P1, P2, objLeft, objRight, res);      
    publishProgress(res.size().width + " , " + res.size().height);
    for(int i = 0; i < res.size().width; i++){
        Mat X1 = transformPointIntoSystem(res.col(i), P1);
        Mat X2 = transformPointIntoSystem(X1, P2);

        if(X1.get(2, 0)[0] >= 0 && X2.get(2, 0)[0] >= 0){
            max[3] += 1;
        }
    }


    // find the index of the maximal value in this array
    int largest = max[0], index = 0;
    for (int i = 1; i < max.length; i++) {
      if ( max[i] >= largest ) {
          largest = max[i];
          index = i;
       }
    }

    switch(index){
    case 0: // R1 T1    
        publishProgress("R1 and T1");
        R1.copyTo(R);
        T1.copyTo(T);
        break;

    case 1: // R1 T2
        publishProgress("R1 and T2");
        R1.copyTo(R);
        T2.copyTo(T);
        break;

    case 2: // R2 T1
        publishProgress("R2 and T1");
        R2.copyTo(R);
        T1.copyTo(T);
        break;

    case 3: // R2 T2
        publishProgress("R2 and T2");
        R2.copyTo(R);
        T2.copyTo(T);
        break;
    }

}

有人可以帮我这个吗？

从一个摄像系统到另一个系统的点

0 个答案: