Hi, my first time post on here so go easy ;)
ive been playing around for a week or so with OpenCV using the Java bindings and havnt managed to do what ive been trying too through maybe lack of understanding so i'll go right ahead with what i'm trying to do.
basically im trying to use a 3D model i have of a planar object in xy (z=0), i have several 3D points on my object and i can see it in a frame of video. i then click on the the points on the screen generating a list of 2D screen space points to go with each corresponding 3D model point giving me a Map of 2D to 3D points correspondances.
after i have my 2d and 3d point correspondances i am then trying to derive the real world position of the camera that is looking at them as well as the field of view (in x and y) as well as pan, tilt and roll of the camera.
so first i tried running my 2D and corresponding 3D points through :
Mat initialCamMatrix = Calib3d.initCameraMatrix2D(worldPoints, imagePoints, imageSize, aspect);
with an image size of Size imageSize = new Size(1920,1080) and the corresponding aspect ratio.
i thought this would give me an initial camera matrix where the fx and fy components could be used to calculate the field of view :
double[] fy = initialCamMatrix.get(1, 1);
double fovy = 2.0 * Math.atan(1080.0 / (2.0 * fy[0])) * 180.0 / Math.PI;
unfortunately this doesn't seem to give me the correct field of view of the camera (which i know as im comparing it to a way we know works and has been extensively tested)
so ive kind of obviously gone wrong already at this point but i thought i'd carry on with the theory to at least get my thinking down.
so i then move on to :
MatOfDouble distCoeffs = new MatOfDouble(tmp);
Mat rvec = new Mat(3,1, CvType.CV_32FC1, new Scalar(0));
Mat tvec = new Mat(3,1, CvType.CV_32FC1, new Scalar(0));
Calib3d.solvePnP(worldpts, imagepts, initialCamMatrix, distCoeffs, rvec, tvec, false, Calib3d.P3P);
to try and get the rotation and translation vectors so i can calculate the pan, tilt, roll, x, y, z of the camera by using :
Mat R = new Mat();
Calib3d.Rodrigues(rvec, R);
Mat cameraRotationVector = new Mat();
Calib3d.Rodrigues(R.t(), cameraRotationVector);
System.out.println("OpenCV Mat data for cameraRotationVector :\n" + cameraRotationVector.dump());
double[] A = cameraRotationVector.get(0,0);
double[] B = cameraRotationVector.get(1,0);
double[] C = cameraRotationVector.get(2,0);
vector rot = new vector(Math.toDegrees(A[0]), Math.toDegrees(B[0]), Math.toDegrees(C[0]));
and for the camera position :
Mat transpose = R.t();
Mat inversR = transpose.inv();
System.out.println("OpenCV Mat data for inversR :\n" + inversR.dump());
//Mat cameraTranslationVector = inversR.mul(tvec);
/**
* c[0] = a[0][0]*b[0] + a[0][1]*b[1] + a[0][2]*b[2]
c[1] = a[1][0]*b[0] + a[1][1]*b[1] + a[1][2]*b[2]
c[2] = a[2][0]*b[0] + a[2][1]*b[1] + a[2][2]*b[2]
*/
double[] result = new double[3];
double[][] first3x3 = new double[3][3];
double[] second3x1 = new double[3];
first3x3[0][0] = inversR.get(0, 0)[0];
first3x3[1][0] = inversR.get(1, 0)[0];
first3x3[2][0] = inversR.get(2, 0)[0];
first3x3[0][1] = inversR.get(0, 1)[0];
first3x3[1][1] = inversR.get(1, 1)[0];
first3x3[2][1] = inversR.get(2, 1)[0];
first3x3[0][2] = inversR.get(0, 2)[0];
first3x3[1][2] = inversR.get(1, 2)[0];
first3x3[2][2] = inversR.get(2, 2)[0];
second3x1[0] = tvec.get(0, 0)[0];
second3x1[1] = tvec.get(1, 0)[0];
second3x1[2] = tvec.get(2, 0)[0];
result[0] = first3x3[0][0]*second3x1[0] + first3x3[0][1]*second3x1[1] + first3x3[0][2]*second3x1[2];
result[1] = first3x3[1][0]*second3x1[0] + first3x3[1][1]*second3x1[1] + first3x3[1][2]*second3x1[2];
result[2] = first3x3[2][0]*second3x1[0] + first3x3[2][1]*second3x1[1] + first3x3[2][2]*second3x1[2];
System.out.println(">>> position [0] = " + result[0] + " [1] = " + result[1] + " [2] = " + result[2]);
but alas i get garbage numbers for my known camera parameters.
could anyone help ? im sure im missing something blindingly obvious here !!!!
Regards
Matt.