# Points form one camera system to another

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), point.get(1, 0) ,point.get(2, 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;
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 && X2.get(2, 0) >= 0){
max += 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 && X2.get(2, 0) >= 0){
max += 1;
}
}

P2 = buildCameraMatrix(R2, T1);
Calib3d.triangulatePoints(P1, P2, objLeft, objRight, res);
publishProgress(res.size().width + " , " + res.size().height);
for(int i ...
edit retag close merge delete