Points form one camera system to another

edit

Hi, I am having some troubles doing a simple transformation. My setup is that I am having one camera and taking two pictures with the same camera of the same scene but with slithly different angle and position. For my SfM approach I am doing the essential decomposition which works perfectly as I am getting the 4 possible solutions for the relative camera pose. I assume that the first camera is located at the origin of the world-space.

The problem is when I try to find the correct comination of R1/R2 and T1/T2. My idea is that I triangulate all good matches found before. This will give me a list of points (x,y,z,w) in the world space/ first-camera-space.

Now I want to transform a copy of this point into the system of the 2nd camera and check if the z-value of the original point and the copied-transformed-Point are positiv. If this happens for most of the points, the used combination of R and T is the correct one, but it still finds the wrong combination as the rectification produces wrong results (from hardcoding and testing I know that R2 and T2 are correct, but it finds R1 and T2)

Here is my code:

/**
 * 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 ...